apollo::planning::OpenSpaceRoiUtil类 参考

#include <open_space_roi_util.h>

apollo::planning::OpenSpaceRoiUtil 的协作图:

静态 Public 成员函数
static bool	FormulateBoundaryConstraints (OpenSpaceInfo *const open_space_info)
	main process to compute and load info needed by open space planner
static void	GetRoiXYBoundary (const std::vector< std::vector< common::math::Vec2d > > &roi_parking_boundary, std::vector< double > *const XYBoundary)
	get XY boundary which is minmax x or y in roi boundary
static void	TransformByOriginPoint (const common::math::Vec2d &origin_point, const double heading, std::vector< std::vector< common::math::Vec2d > > *roi_parking_boundary)
static void	TransformByOriginPoint (const common::math::Vec2d &origin_point, const double heading, std::vector< common::math::Vec2d > *roi_parking_boundary)
static void	TransformByOriginPoint (const common::math::Vec2d &origin_point, const double heading, common::math::Vec2d *point)
static bool	LoadObstacles (const double filtering_distance, const double perception_obstacle_buffer, const std::vector< double > &xy_boundary, Frame *const frame, std::vector< std::vector< common::math::Vec2d > > *const roi_parking_boundary)
	Load obstacle polygon to roi_boundary in clock wise order
static bool	FilterOutObstacle (const double filtering_distance, const Frame &frame, const std::vector< double > &xy_boundary, const Obstacle &obstacle)
	is obstacle can be filterd out, filtered by distance and roi XY bound
static bool	LoadObstacleInHyperPlanes (OpenSpaceInfo *const open_space_info)
	brief Transform the vertice presentation of the obstacles into linear inequality as Ax>b
static bool	GetHyperPlanes (const size_t &obstacles_num, const Eigen::MatrixXi &obstacles_edges_num, const std::vector< std::vector< Vec2d > > &obstacles_vertices_vec, Eigen::MatrixXd *A_all, Eigen::MatrixXd *b_all)
static void	TransformByOriginPoint (const Vec2d &origin_point, const double &origin_heading, OpenSpaceInfo *open_space_info)
static bool	IsPolygonClockwise (const std::vector< Vec2d > &polygon)
static bool	AdjustPointsOrderToClockwise (std::vector< Vec2d > *polygon)
static bool	UpdateParkingPointsOrder (const apollo::hdmap::Path &nearby_path, std::vector< Vec2d > *points)

详细描述

在文件 open_space_roi_util.h 第 36 行定义.

成员函数说明

AdjustPointsOrderToClockwise()

bool apollo::planning::OpenSpaceRoiUtil::AdjustPointsOrderToClockwise ( std::vector< Vec2d > *  polygon )

static

在文件 open_space_roi_util.cc 第 308 行定义.

                                                                             {
    if (!IsPolygonClockwise(*polygon)) {
        // counter clockwise reverse it
        ADEBUG << "point is anticlockwise,reverse";
        std::reverse(polygon->begin(), polygon->end());
        return true;
    } else {
        return false;
    }
}

FilterOutObstacle()

bool apollo::planning::OpenSpaceRoiUtil::FilterOutObstacle ( const double  filtering_distance, const Frame &  frame, const std::vector< double > &  xy_boundary, const Obstacle &  obstacle  )

static

is obstacle can be filterd out, filtered by distance and roi XY bound

参数

xy_boundary	fileter obstacle which out of xy boundary
filtering_distance	filter out distance which is far away from adc

返回

true can be filtered out

在文件 open_space_roi_util.cc 第 143 行定义.

                                  {
    if (obstacle.IsVirtual() || !obstacle.IsStatic()) {
        return true;
    }
 
    const auto &open_space_info = frame.open_space_info();
    const auto &origin_point = open_space_info.origin_point();
    const auto &origin_heading = open_space_info.origin_heading();
    const auto &obstacle_box = obstacle.PerceptionBoundingBox();
    auto obstacle_center_xy = obstacle_box.center();
 
    // xy_boundary in xmin, xmax, ymin, ymax.
    obstacle_center_xy -= origin_point;
    obstacle_center_xy.SelfRotate(-origin_heading);
    if (obstacle_center_xy.x() < xy_boundary[0] || obstacle_center_xy.x() > xy_boundary[1]
        || obstacle_center_xy.y() < xy_boundary[2] || obstacle_center_xy.y() > xy_boundary[3]) {
        return true;
    }
 
    // Translate the end pose back to world frame with endpose in x, y, phi, v
    const auto &end_pose = open_space_info.open_space_end_pose();
    Vec2d end_pose_x_y(end_pose[0], end_pose[1]);
    end_pose_x_y.SelfRotate(origin_heading);
    end_pose_x_y += origin_point;
    const auto &vehicle_state = frame.vehicle_state();
    // Get vehicle state
    Vec2d vehicle_x_y(vehicle_state.x(), vehicle_state.y());
 
    // Use vehicle position and end position to filter out obstacle
    const double vehicle_center_to_obstacle = obstacle_box.DistanceTo(vehicle_x_y);
    const double end_pose_center_to_obstacle = obstacle_box.DistanceTo(end_pose_x_y);
    if (vehicle_center_to_obstacle > filtering_distance && end_pose_center_to_obstacle > filtering_distance) {
        return true;
    }
    return false;
}

FormulateBoundaryConstraints()

bool apollo::planning::OpenSpaceRoiUtil::FormulateBoundaryConstraints ( OpenSpaceInfo *const  open_space_info )

static

main process to compute and load info needed by open space planner

在文件 open_space_roi_util.cc 第 36 行定义.

                                                                                        {
    auto &obstacles_vertices_vec = *open_space_info->mutable_obstacles_vertices_vec();
    auto &obstacles_edges_num_vec = *open_space_info->mutable_obstacles_edges_num();
    size_t obstacles_vertices_vec_num = obstacles_vertices_vec.size();
    obstacles_edges_num_vec.resize(obstacles_vertices_vec_num, 1);
    for (size_t i = 0; i < obstacles_vertices_vec_num; i++) {
        CHECK_GT(obstacles_vertices_vec[i].size(), 1U);
        obstacles_edges_num_vec(i, 0) = static_cast<int>(obstacles_vertices_vec[i].size()) - 1;
    }
    open_space_info->set_obstacles_num(obstacles_vertices_vec.size());
    // Transform vertices into the form of Ax>b
    if (!LoadObstacleInHyperPlanes(open_space_info)) {
        AERROR << "fail at LoadObstacleInHyperPlanes()";
        return false;
    }
    return true;
}

GetHyperPlanes()

bool apollo::planning::OpenSpaceRoiUtil::GetHyperPlanes ( const size_t &  obstacles_num, const Eigen::MatrixXi &  obstacles_edges_num, const std::vector< std::vector< Vec2d > > &  obstacles_vertices_vec, Eigen::MatrixXd *  A_all, Eigen::MatrixXd *  b_all  )

static

在文件 open_space_roi_util.cc 第 200 行定义.

                              {
    if (obstacles_num != obstacles_vertices_vec.size()) {
        AERROR << "obstacles_num != obstacles_vertices_vec.size()";
        return false;
    }
 
    A_all->resize(obstacles_edges_num.sum(), 2);
    b_all->resize(obstacles_edges_num.sum(), 1);
 
    int counter = 0;
    double kEpsilon = 1.0e-5;
    // start building H representation
    for (size_t i = 0; i < obstacles_num; ++i) {
        size_t current_vertice_num = obstacles_edges_num(i, 0);
        Eigen::MatrixXd A_i(current_vertice_num, 2);
        Eigen::MatrixXd b_i(current_vertice_num, 1);
 
        // take two subsequent vertices, and computer hyperplane
        for (size_t j = 0; j < current_vertice_num; ++j) {
            Vec2d v1 = obstacles_vertices_vec[i][j];
            Vec2d v2 = obstacles_vertices_vec[i][j + 1];
 
            Eigen::MatrixXd A_tmp(2, 1), b_tmp(1, 1), ab(2, 1);
            // find hyperplane passing through v1 and v2
            if (std::abs(v1.x() - v2.x()) < kEpsilon) {
                if (v2.y() < v1.y()) {
                    A_tmp << 1, 0;
                    b_tmp << v1.x();
                } else {
                    A_tmp << -1, 0;
                    b_tmp << -v1.x();
                }
            } else if (std::abs(v1.y() - v2.y()) < kEpsilon) {
                if (v1.x() < v2.x()) {
                    A_tmp << 0, 1;
                    b_tmp << v1.y();
                } else {
                    A_tmp << 0, -1;
                    b_tmp << -v1.y();
                }
            } else {
                Eigen::MatrixXd tmp1(2, 2);
                tmp1 << v1.x(), 1, v2.x(), 1;
                Eigen::MatrixXd tmp2(2, 1);
                tmp2 << v1.y(), v2.y();
                ab = tmp1.inverse() * tmp2;
                double a = ab(0, 0);
                double b = ab(1, 0);
 
                if (v1.x() < v2.x()) {
                    A_tmp << -a, 1;
                    b_tmp << b;
                } else {
                    A_tmp << a, -1;
                    b_tmp << -b;
                }
            }
 
            // store vertices
            A_i.block(j, 0, 1, 2) = A_tmp.transpose();
            b_i.block(j, 0, 1, 1) = b_tmp;
        }
 
        A_all->block(counter, 0, A_i.rows(), 2) = A_i;
        b_all->block(counter, 0, b_i.rows(), 1) = b_i;
        counter += static_cast<int>(current_vertice_num);
    }
    return true;
}

GetRoiXYBoundary()

void apollo::planning::OpenSpaceRoiUtil::GetRoiXYBoundary ( const std::vector< std::vector< common::math::Vec2d > > &  roi_parking_boundary, std::vector< double > *const  XYBoundary  )

static

get XY boundary which is minmax x or y in roi boundary

参数

XYBoundary

x_min x_max y_min y_max

返回

在文件 open_space_roi_util.cc 第 54 行定义.

                                             {
    XYBoundary->clear();
    XYBoundary->resize(4);  // x_min x_max y_min y_max
    *XYBoundary
            = {std::numeric_limits<double>::max(),
               std::numeric_limits<double>::lowest(),
               std::numeric_limits<double>::max(),
               std::numeric_limits<double>::lowest()};
    for (auto &bound_vec : roi_parking_boundary) {
        for (auto &pt : bound_vec) {
            XYBoundary->at(0) = std::min<double>(XYBoundary->at(0), pt.x());
            XYBoundary->at(1) = std::max<double>(XYBoundary->at(1), pt.x());
            XYBoundary->at(2) = std::min<double>(XYBoundary->at(2), pt.y());
            XYBoundary->at(3) = std::max<double>(XYBoundary->at(3), pt.y());
        }
    }
}

IsPolygonClockwise()

bool apollo::planning::OpenSpaceRoiUtil::IsPolygonClockwise ( const std::vector< Vec2d > &  polygon )

static

在文件 open_space_roi_util.cc 第 292 行定义.

                                                                         {
    double s = 0;
    for (size_t i = 0; i < polygon.size(); i++) {
        if (i + 1 < polygon.size()) {
            s += polygon.at(i).x() * polygon.at(i + 1).y() - polygon.at(i).y() * polygon.at(i + 1).x();
        } else {
            s += polygon.at(i).x() * polygon.at(0).y() - polygon.at(i).y() * polygon.at(0).x();
        }
    }
    if (s < 0) {
        return true;
    } else {
        return false;
    }
}

LoadObstacleInHyperPlanes()

bool apollo::planning::OpenSpaceRoiUtil::LoadObstacleInHyperPlanes ( OpenSpaceInfo *const  open_space_info )

static

brief Transform the vertice presentation of the obstacles into linear inequality as Ax>b

返回

true is success

在文件 open_space_roi_util.cc 第 184 行定义.

                                                                                     {
    *(open_space_info->mutable_obstacles_A()) = Eigen::MatrixXd::Zero(open_space_info->obstacles_edges_num().sum(), 2);
    *(open_space_info->mutable_obstacles_b()) = Eigen::MatrixXd::Zero(open_space_info->obstacles_edges_num().sum(), 1);
    // vertices using H-representation
    if (!GetHyperPlanes(
                open_space_info->obstacles_num(),
                open_space_info->obstacles_edges_num(),
                open_space_info->obstacles_vertices_vec(),
                open_space_info->mutable_obstacles_A(),
                open_space_info->mutable_obstacles_b())) {
        AERROR << "Fail to present obstacle in hyperplane";
        return false;
    }
    return true;
}

LoadObstacles()

bool apollo::planning::OpenSpaceRoiUtil::LoadObstacles ( const double  filtering_distance, const double  perception_obstacle_buffer, const std::vector< double > &  xy_boundary, Frame *const  frame, std::vector< std::vector< common::math::Vec2d > > *const  roi_parking_boundary  )

static

Load obstacle polygon to roi_boundary in clock wise order

参数

filtering_distance	filter out distance which is far away from adc
perception_obstacle_buffer	add extend buffer for obstacle box
roi_boundary	the roi polygon which adc can drive

返回

true is running success

在文件 open_space_roi_util.cc 第 100 行定义.

                                                                           {
    auto obstacles_by_frame = frame->GetObstacleList();
    size_t perception_obstacles_num = 0;
    // load vertices for perception obstacles(repeat the first vertice at the
    // last to form closed convex hull)
    for (const auto &obstacle : obstacles_by_frame->Items()) {
        if (FilterOutObstacle(filtering_distance, *frame, xy_boundary, *obstacle)) {
            continue;
        }
        ++perception_obstacles_num;
 
        Box2d original_box = obstacle->PerceptionBoundingBox();
        original_box.LongitudinalExtend(perception_obstacle_buffer);
        original_box.LateralExtend(perception_obstacle_buffer);
 
        // TODO(Jinyun): Check correctness of ExpandByDistance() in polygon
        // Polygon2d buffered_box(original_box);
        // buffered_box = buffered_box.ExpandByDistance(
        //    config_.perception_obstacle_buffer());
        // TODO(Runxin): Rotate from origin instead
        // original_box.RotateFromCenter(-1.0 * origin_heading);
        std::vector<Vec2d> vertices_ccw = original_box.GetAllCorners();
        std::vector<Vec2d> vertices_cw;
        while (!vertices_ccw.empty()) {
            auto current_corner_pt = vertices_ccw.back();
            vertices_cw.push_back(current_corner_pt);
            vertices_ccw.pop_back();
        }
        // As the perception obstacle is a closed convex set, the first vertice
        // is repeated at the end of the vector to help transform all four edges
        // to inequality constraint
        vertices_cw.push_back(vertices_cw.front());
        roi_parking_boundary->push_back(vertices_cw);
    }
 
    return true;
}

TransformByOriginPoint() [1/4]

void apollo::planning::OpenSpaceRoiUtil::TransformByOriginPoint ( const common::math::Vec2d &  origin_point, const double  heading, common::math::Vec2d *  point  )

static

在文件 open_space_roi_util.cc 第 92 行定义.

                                {
    *point -= origin_point;
    point->SelfRotate(-heading);
}

TransformByOriginPoint() [2/4]

void apollo::planning::OpenSpaceRoiUtil::TransformByOriginPoint ( const common::math::Vec2d &  origin_point, const double  heading, std::vector< common::math::Vec2d > *  roi_parking_boundary  )

static

在文件 open_space_roi_util.cc 第 83 行定义.

                                                          {
    for (auto &point : *roi_parking_boundary) {
        TransformByOriginPoint(origin_point, heading, &point);
    }
}

TransformByOriginPoint() [3/4]

void apollo::planning::OpenSpaceRoiUtil::TransformByOriginPoint ( const common::math::Vec2d &  origin_point, const double  heading, std::vector< std::vector< common::math::Vec2d > > *  roi_parking_boundary  )

static

在文件 open_space_roi_util.cc 第 74 行定义.

                                                                     {
    for (auto &bound_vec : *roi_parking_boundary) {
        TransformByOriginPoint(origin_point, heading, &bound_vec);
    }
}

TransformByOriginPoint() [4/4]

void apollo::planning::OpenSpaceRoiUtil::TransformByOriginPoint ( const Vec2d &  origin_point, const double &  origin_heading, OpenSpaceInfo *  open_space_info  )

static

在文件 open_space_roi_util.cc 第 275 行定义.

                                        {
    open_space_info->set_origin_heading(origin_heading);
    *open_space_info->mutable_origin_point() = origin_point;
    auto obstacles_vertices_vec = open_space_info->mutable_obstacles_vertices_vec();
    TransformByOriginPoint(origin_point, origin_heading, obstacles_vertices_vec);
    auto end_pose_vec = open_space_info->mutable_open_space_end_pose();
    Vec2d end_pose(end_pose_vec->at(0), end_pose_vec->at(1));
    TransformByOriginPoint(origin_point, origin_heading, &end_pose);
    end_pose_vec->at(0) = end_pose.x();
    end_pose_vec->at(1) = end_pose.y();
    end_pose_vec->at(2) -= origin_heading;
    auto xy_boundary = open_space_info->mutable_ROI_xy_boundary();
    GetRoiXYBoundary(*obstacles_vertices_vec, xy_boundary);
}

UpdateParkingPointsOrder()

bool apollo::planning::OpenSpaceRoiUtil::UpdateParkingPointsOrder ( const apollo::hdmap::Path &  nearby_path, std::vector< Vec2d > *  points  )

static

在文件 open_space_roi_util.cc 第 319 行定义.

                                                                                                            {
    AdjustPointsOrderToClockwise(points);
    double min_dist = std::numeric_limits<double>::max();
    size_t min_index = 0;
    double sum_l = 0;
    for (size_t i = 0; i < points->size(); i++) {
        double s = 0.0, l = 0.0;
        nearby_path.GetProjection(points->at(i), &s, &l);
        sum_l += l;
        ADEBUG << std::fixed << points->at(i).x() << "," << points->at(i).y() << "sl" << s << "," << l;
        if (std::fabs(s) + 2.0 * std::fabs(l) < min_dist) {
            min_dist = std::fabs(s) + 2.0 * std::fabs(l);
            min_index = i;
        }
    }
    std::vector<Vec2d> tmp_points(*points);
    for (size_t i = 0; i < points->size(); i++) {
        tmp_points[i] = points->at((i + min_index) % points->size());
    }
    if (sum_l < 0) {
        for (size_t i = 0; i < points->size(); i++) {
            points->at(i) = tmp_points[i];
        }
    } else {
        points->at(0) = tmp_points[0];
        for (size_t i = points->size() - 1; i > 0; i--) {
            points->at(points->size() - i) = tmp_points[i];
        }
    }
    return true;
}

---

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

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