open_space_trajectory_optimizer_util.cc

浏览该文件的文档.

/******************************************************************************
 * Copyright 2023 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
#include "modules/planning/planning_open_space/utils/open_space_trajectory_optimizer_util.h"
 
namespace apollo {
namespace planning {
 
using common::math::Box2d;
using common::math::Vec2d;
 
bool OpenSpaceTrajectoryOptimizerUtil::BoxOverlapObstacles(
        const Box2d& bounding_box,
        const std::vector<std::vector<common::math::LineSegment2d>> obstacles_linesegments_vec) {
    for (const auto& obstacle_linesegments : obstacles_linesegments_vec) {
        for (const common::math::LineSegment2d& linesegment : obstacle_linesegments) {
            if (bounding_box.HasOverlap(linesegment)) {
                ADEBUG << "collision start at x: " << linesegment.start().x();
                ADEBUG << "collision start at y: " << linesegment.start().y();
                ADEBUG << "collision end at x: " << linesegment.end().x();
                ADEBUG << "collision end at y: " << linesegment.end().y();
                return true;
            }
        }
    }
    return false;
}
 
void OpenSpaceTrajectoryOptimizerUtil::GeneratePointBox(
        const std::vector<std::vector<common::math::LineSegment2d>> obstacles_linesegments_vec,
        const std::vector<std::pair<double, double>>& result,
        std::vector<std::vector<Vec2d>>& box_vec) {
    auto vehicle_param = common::VehicleConfigHelper::GetConfig().vehicle_param();
    double shift_distance = vehicle_param.length() / 2.0 - vehicle_param.back_edge_to_center();
    double init_length = vehicle_param.length();
    double init_width = vehicle_param.width();
    double rate = 1.0;
 
    box_vec.clear();
    for (int i = 0; i < result.size() - 1; i++) {
        double x = result[i].first;
        double y = result[i].second;
        double x_next = result[i + 1].first;
        double y_next = result[i + 1].second;
        double heading = std::atan2(y_next - y, x_next - x);
        AINFO << "init pos: " << x << " " << y;
        // ego_box.Shift(shift_vec);
        // x += shift_distance;
        Box2d box({x, y}, heading, init_length, init_width);
        for (int j = 1; j < 10; j++) {
            if (OpenSpaceTrajectoryOptimizerUtil::BoxOverlapObstacles(box, obstacles_linesegments_vec))
                break;
            box = Box2d({x, y}, heading, init_length + j * 0.2, init_width + j * 0.2);
        }
        auto corners = box.GetAllCorners();
        box_vec.push_back(corners);
        // 3-----------2
        //  |         |
        //  |         |
        // 4-----------1
        ADEBUG << "corners 1: " << corners[0].x() << " " << corners[0].y();
        ADEBUG << "corners 2: " << corners[1].x() << " " << corners[1].y();
        ADEBUG << "corners 3: " << corners[2].x() << " " << corners[2].y();
        ADEBUG << "corners 4: " << corners[3].x() << " " << corners[3].y();
    }
}
 
void OpenSpaceTrajectoryOptimizerUtil::PathPointNormalizing(
        double rotate_angle,
        const Vec2d& translate_origin,
        double* x,
        double* y,
        double* phi) {
    *x -= translate_origin.x();
    *y -= translate_origin.y();
    double tmp_x = *x;
    *x = (*x) * std::cos(-rotate_angle) - (*y) * std::sin(-rotate_angle);
    *y = tmp_x * std::sin(-rotate_angle) + (*y) * std::cos(-rotate_angle);
    *phi = common::math::NormalizeAngle(*phi - rotate_angle);
}
 
void OpenSpaceTrajectoryOptimizerUtil::PathPointDeNormalizing(
        double rotate_angle,
        const Vec2d& translate_origin,
        double* x,
        double* y,
        double* phi) {
    double tmp_x = *x;
    *x = (*x) * std::cos(rotate_angle) - (*y) * std::sin(rotate_angle);
    *y = tmp_x * std::sin(rotate_angle) + (*y) * std::cos(rotate_angle);
    *x += translate_origin.x();
    *y += translate_origin.y();
    *phi = common::math::NormalizeAngle(*phi + rotate_angle);
}
 
}  // namespace planning
}  // namespace apollo