apollo::planning::OpenSpaceTrajectoryOptimizerUtil类 参考

#include <open_space_trajectory_optimizer_util.h>

apollo::planning::OpenSpaceTrajectoryOptimizerUtil 的协作图:

静态 Public 成员函数
static bool	BoxOverlapObstacles (const common::math::Box2d &bounding_box, const std::vector< std::vector< common::math::LineSegment2d > > obstacles_linesegments_vec)
	check if box collision with obstacles
static void	GeneratePointBox (const std::vector< std::vector< common::math::LineSegment2d > > obstacles_linesegments_vec, const std::vector< std::pair< double, double > > &result, std::vector< std::vector< common::math::Vec2d > > &box_vec)
	check if box collision with obstacles
static void	PathPointNormalizing (double rotate_angle, const common::math::Vec2d &translate_origin, double *x, double *y, double *phi)
static void	PathPointDeNormalizing (double rotate_angle, const common::math::Vec2d &translate_origin, double *x, double *y, double *phi)

详细描述

在文件 open_space_trajectory_optimizer_util.h 第 40 行定义.

成员函数说明

BoxOverlapObstacles()

bool apollo::planning::OpenSpaceTrajectoryOptimizerUtil::BoxOverlapObstacles ( const common::math::Box2d &  bounding_box, const std::vector< std::vector< common::math::LineSegment2d > >  obstacles_linesegments_vec  )

static

check if box collision with obstacles

在文件 open_space_trajectory_optimizer_util.cc 第 29 行定义.

                                                                                        {
    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;
}

GeneratePointBox()

void apollo::planning::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< common::math::Vec2d > > &  box_vec  )

static

check if box collision with obstacles

在文件 open_space_trajectory_optimizer_util.cc 第 46 行定义.

                                              {
    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();
    }
}

PathPointDeNormalizing()

void apollo::planning::OpenSpaceTrajectoryOptimizerUtil::PathPointDeNormalizing ( double  rotate_angle, const common::math::Vec2d &  translate_origin, double *  x, double *  y, double *  phi  )

static

在文件 open_space_trajectory_optimizer_util.cc 第 99 行定义.

                     {
    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);
}

PathPointNormalizing()

void apollo::planning::OpenSpaceTrajectoryOptimizerUtil::PathPointNormalizing ( double  rotate_angle, const common::math::Vec2d &  translate_origin, double *  x, double *  y, double *  phi  )

static

在文件 open_space_trajectory_optimizer_util.cc 第 85 行定义.

                     {
    *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);
}

---

该类的文档由以下文件生成:

• modules/planning/planning_open_space/utils/open_space_trajectory_optimizer_util.h
• modules/planning/planning_open_space/utils/open_space_trajectory_optimizer_util.cc