Autoware 笔记 No. 6，基于openplanner的避障。
1. 前言
之前忘记发布了，现在补上。
该方法还是基于autoware1.10.0，openplanner的代码部分需要修改。这部分需要加载vector map，所以需要autoware网站上提供的地图工具。具体vector map工具请参考autoware官网。
各个配置可以参考之前的文章。
2. 配置
（1）Autoware的Runtime Manager的Setup项目配置
（2）Autoware的Runtime Manager的Map项目配置
这里特别说明一下vector map，是由autoware官网工具产生的，可以参考官网，会生成一些，路沿、车道线，轨迹等信息。
（3）Autoware的Runtime Manager的Sensing项目配置
（4）Autoware的Runtime Manager的Computing项目中planning的配置。
ndt_matching可以打开gpu
lidar_tracker项目需要勾选lidar_kf_contour_track（跟踪障碍物），相应的app配置为默认
Vector Map Filter Distance用于检测障碍物到路径的距离，如下图所示，黑色的粗线为航迹，那么障碍物在轨迹前或后方时，小于两倍的设置距离会被跟踪，而在轨迹范围内时，小于1倍的设置距离会被跟踪。
这里需要修改lidar_kf_contour_track引用的一个库文件PlannerHelpers.cpp的GetRelativeInfoLimited函数代码片段增加一句，如下所示。
info.bAfter = false; info.bBefore = false; if(info.iFront == 0) { info.bBefore = true; } else if(info.iFront == _trajectory.size()-1) { int s = _trajectory.size(); double angle_befor_last = UtilityH::FixNegativeAngle(atan2(_trajectory.at(s-2).pos.y - _trajectory.at(s-1).pos.y, _trajectory.at(s-2).pos.x - _trajectory.at(s-1).pos.x)); double angle_from_perp = UtilityH::FixNegativeAngle(atan2(info.perp_point.pos.y - _trajectory.at(s-1).pos.y, info.perp_point.pos.x - _trajectory.at(s-1).pos.x)); double diff_last_perp = UtilityH::AngleBetweenTwoAnglesPositive(angle_befor_last, angle_from_perp); info.after_angle = diff_last_perp; if(diff_last_perp > M_PI_2) { info.bAfter = true; } info.iFront = trajectory.size() - 1; // 增加这句来防止越界 }
op_global_planner的app设置。
Replanning用于到达目标点后，可以重新规划到下一个目标点。如果有了两个目标点，那么会循环，下方修改代码后，可以不循环。
Smooth用于平滑全局路径。
op_common_params的app设置
Plan Ditance用于设置朱路径两侧的衍生出的局部轨迹的长度；
Path Density用于局部轨迹上两个轨迹点的距；
Horizontal Density用于设置两个局部轨迹的间距；
Rollouts Number用于设置局部轨迹的数量；
Max Velocity轨迹上的执行速度，避障时局部轨迹速度减半，单位m/s；
Follow DIstance关键参数，沿着轨迹设置从多远检测障碍物；
Avoid Distance感知到路径上的障碍物，判断多远开始绕行；
Avoidance Limit停车是，障碍物距离多远可以开车；
Lateral Safety与Longitudinal Safty设置安全框尺寸，分别代表车辆安全框宽与长。
op_trajectory_generator的app设置
Tip Margin用于设置车身到路径分叉点的距离；
Roll In Margin用于设置局部轨迹弯折的距离；
op_motion_predictor的app设置
Detect curbs from map用于将路沿判定为障碍物。
由于采用vector map后，只能运行一次，为了业务端可以反复执行路径，需要对op_global_planner_core.cpp的MainLoop()及BehaviorStateMachine.cpp的MissionAccomplishedStateII::GetNextState()做修改。
op_global_planner_core.cpp的MainLoop()：
void GlobalPlanner::MainLoop() { ... if(m_GoalsPos.size() > 0) { if(m_GeneratedTotalPaths.size() > 0 && m_GeneratedTotalPaths.at(0).size() > 3) { // -----原有代码----- // if(m_params.bEnableReplanning) // -----上述代码修改为下面一行代码------ if(m_params.bEnableReplanning && m_iCurrentGoalIndex < m_GoalsPos.size() - 1) { PlannerHNS::RelativeInfo info; bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_GeneratedTotalPaths, m_CurrentPose, 0.75, info); if(ret == true && info.iGlobalPath >= 0 && info.iGlobalPath < m_GeneratedTotalPaths.size() && info.iFront > 0 && info.iFront < m_GeneratedTotalPaths.at(info.iGlobalPath).size()) { double remaining_distance = m_GeneratedTotalPaths.at(info.iGlobalPath).at(m_GeneratedTotalPaths.at(info.iGlobalPath).size()-1).cost - (m_GeneratedTotalPaths.at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance); if(remaining_distance <= REPLANNING_DISTANCE) { bMakeNewPlan = true; if(m_GoalsPos.size() > 0) m_iCurrentGoalIndex = (m_iCurrentGoalIndex + 1) % m_GoalsPos.size(); std::cout << "Current Goal Index = " << m_iCurrentGoalIndex << std::endl << std::endl; } } } } ... }
BehaviorStateMachine.cpp的MissionAccomplishedStateII::GetNextState()：
BehaviorStateMachine* MissionAccomplishedStateII::GetNextState() { // -------------新增代码支持任务结束后，重新规划到目标点---------------- PreCalculatedConditions* pCParams = GetCalcParams(); if(pCParams->currentGoalID == -1) return FindBehaviorState(this->m_Behavior); else { pCParams->prevGoalID = pCParams->currentGoalID; return FindBehaviorState(FORWARD_STATE); } // -------------原有代码-------------------- // return FindBehaviorState(this->m_Behavior); }
pure_pursuit模块在final_waypoints为空时会出异常，支持循环寻迹:
首先在pure_pursuit_core.h中添加 ros::Subscriber sub5_;
private: // handle ros::NodeHandle nh_; ros::NodeHandle private_nh_; // class PurePursuit pp_; // publisher ros::Publisher pub1_, pub2_, pub11_, pub12_, pub13_, pub14_, pub15_, pub16_, pub17_; // subscriber ros::Subscriber sub1_, sub2_, sub3_, sub4_; // -----2019.10.12----- ros::Subscriber sub5_; // xiaochun add //--------------------- // constant const int LOOP_RATE_; // processing frequency
增加回调函数声明
// callbacks void callbackFromConfig( const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config); void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); void callbackFromCurrentVelocity( const geometry_msgs::TwistStampedConstPtr& msg); void callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg_input); //modify by hxc void loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg); // add by hxc
添加补充的航迹点
void PurePursuitNode::initForROS() { // ros parameter settings private_nh_.param("is_linear_interpolation", is_linear_interpolation_, bool(true)); // ROS_INFO_STREAM("is_linear_interpolation : " << is_linear_interpolation_); private_nh_.param("publishes_for_steering_robot", publishes_for_steering_robot_, bool(false)); nh_.param("vehicle_info/wheel_base", wheel_base_, double(2.7)); // setup subscriber sub1_ = nh_.subscribe("final_waypoints", 10, &PurePursuitNode::callbackFromWayPoints, this); sub2_ = nh_.subscribe("current_pose", 10, &PurePursuitNode::callbackFromCurrentPose, this); sub3_ = nh_.subscribe("config/waypoint_follower", 10, &PurePursuitNode::callbackFromConfig, this); sub4_ = nh_.subscribe("current_velocity", 10, &PurePursuitNode::callbackFromCurrentVelocity, this); sub5_ = nh_.subscribe("loop_waypoints", 10, &PurePursuitNode::loopWaypointsCallback, this); // xiaochun add // setup publisher pub1_ = nh_.advertise<geometry_msgs::TwistStamped>("twist_raw", 10); pub2_ = nh_.advertise<autoware_msgs::ControlCommandStamped>("ctrl_cmd", 10); pub11_ = nh_.advertise<visualization_msgs::Marker>("next_waypoint_mark", 0); pub12_ = nh_.advertise<visualization_msgs::Marker>("next_target_mark", 0); pub13_ = nh_.advertise<visualization_msgs::Marker>("search_circle_mark", 0); pub14_ = nh_.advertise<visualization_msgs::Marker>("line_point_mark", 0); // debug tool pub15_ = nh_.advertise<visualization_msgs::Marker>("trajectory_circle_mark", 0); pub16_ = nh_.advertise<std_msgs::Float32>("angular_gravity", 0); pub17_ = nh_.advertise<std_msgs::Float32>("deviation_of_current_position", 0); // pub7_ = nh.advertise<std_msgs::Bool>("wf_stat", 0); }
增加sub5_的定义。
pure_pursuit_core.cpp 中添加
// ----------------2019.10.12----------------------- static autoware_msgs::Lane loopWaypoints; void PurePursuitNode::loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg) { loopWaypoints = *msg; }
对应sub5_的回调函数
修改void PurePursuitNode::callbackFromWayPoints(const autoware_msgs::LaneConstPtr &msg_input)函数，防止异常
void PurePursuitNode::callbackFromWayPoints(const autoware_msgs::LaneConstPtr &msg_input) { /****************** xiaochun add ********************/ autoware_msgs::Lane::Ptr msg(new autoware_msgs::Lane); *msg = *msg_input; if (!loopWaypoints.waypoints.empty()) { msg->waypoints.insert(msg->waypoints.end(), loopWaypoints.waypoints.begin(), loopWaypoints.waypoints.end()); loopWaypoints.waypoints.clear(); } if (msg->waypoints.empty()) return; // 如果waypoints为空，pp_会出异常 /****************** xiaochun add ********************/ if (!msg->waypoints.empty()) command_linear_velocity_ = msg->waypoints.at(0).twist.twist.linear.x; // 从航点中得到速度 else command_linear_velocity_ = 0; pp_.setCurrentWaypoints(msg->waypoints); is_waypoint_set_ = true; }
由于pure_pursuit的参数设置中有Waypoint及Dialog两个选项，由于寻迹需要采用Waypoint模式，而Waypoint采用航继点的速度，为了保证测试，希望寻迹也能用Dialog模式，所以需要修改代码，如下：
double PurePursuitNode::computeLookaheadDistance() const { // -----------新增代码------------ return const_lookahead_distance_; // -----------原有代码------------- if (param_flag_ == enumToInteger(Mode::dialog)) // 如果是dialog模式, 就直接返回lookahead_distance return const_lookahead_distance_; double maximum_lookahead_distance = current_linear_velocity_ * 10; // 否则返回介于min和max闭区间的数值 double ld = current_linear_velocity_ * lookahead_distance_ratio_; return ld < minimum_lookahead_distance_ ? minimum_lookahead_distance_ : ld > maximum_lookahead_distance ? maximum_lookahead_distance : ld; }
（5）增加障碍物检测，笛卡尔聚类方法
lidar_kf_contour_track障碍物通过消息 ClouldCluster发出的，所以对应lidar_kf_contour_track接收，双方的代码有些不一致，需要修改。
首先需要配置lidar_euclidean_cluster_detect.launch文件的主要参数
<launch>
<arg name="points_node" default="/points_raw"/><!--CHANGE THIS TO READ WHETHER FROM VSCAN OR POINTS_RAW -->
<arg name="remove_ground" default="true"/><!-- 删除地面上的点 -->
<arg name="downsample_cloud" default="false"/> <!-- Apply VoxelGrid Filter with the value given by "leaf_size"-->
<arg name="leaf_size" default="0.1"/><!-- Voxel Grid Filter leaf size-->
<arg name="cluster_size_min" default="20"/><!-- Minimum number of points to consider a cluster as valid-->
<arg name="cluster_size_max" default="100000"/><!-- Maximum number of points to allow inside a cluster-->
<arg name="sync" default="false"/>
<arg name="use_diffnormals" default="false"/>
<arg name="pose_estimation" default="true"/>
<arg name="clip_min_height" default="-1.0"/><!-- lidar的高度，根据实际情况定，注意为负值，这个参数与删除地面直接相关 -->
<arg name="clip_max_height" default="0.3"/><!-- 检测的物体的高度，从激光雷达中心开始 -->
<!-- hgl added-->
<arg name="lidar_front_car_range" default="0.5" /><!-- lidar到车前方的距离，为了去除扫描到车身上的点 -->
<arg name="lidar_back_car_range" default="-3.0" /><!-- lidar到车后方的距离，为了去除扫描到车身上的点 -->
<arg name="lidar_left_car_range" default="0.65" /><!-- lidar到车左侧的距离，为了去除扫描到车身上的点 -->
<arg name="lidar_right_car_range" default="-0.65" /><!-- lidar到车右侧的距离，为了去除扫描到车身上的点 -->
<arg name="lidar_back_range" default="-5.0" /><!-- lidar到车后方的有效距离，为了去除车后方的点 -->
<arg name="lidar_total_range" default="30.0" /><!-- lidar的有效探测范围 -->
<!-- hgl added-->
<arg name="keep_lanes" default="false"/>
<arg name="keep_lane_left_distance" default="5"/>
<arg name="keep_lane_right_distance" default="5"/>
<arg name="max_boundingbox_side" default="10"/>
<arg name="cluster_merge_threshold" default="1.5"/>
<arg name="clustering_distance" default="0.75"/>
<arg name="use_vector_map" default="false"/>
<arg name="vectormap_frame" default="map"/>
<arg name="wayarea_gridmap_topic" default="grid_map_wayarea"/>
<arg name="wayarea_gridmap_layer" default="wayarea"/>
<arg name="wayarea_no_road_value" default="255"/>
<arg name="output_frame" default="map"/><!-- 默认障碍物在velodyne坐标系下，而lidar_kf_contour_track需要地图坐标系下的障碍物信息，所以修改为map
-->
<arg name="remove_points_upto" default="0.0"/>
<arg name="use_gpu" default="true"/>
<arg name="use_multiple_thres" default="false"/>
<arg name="clustering_ranges" default="[15,30,45,60]"/><!-- Distances to segment pointcloud -->
<arg name="clustering_distances"
default="[0.5,1.1,1.6,2.1,2.6]"/><!-- Euclidean Clustering threshold distance for each segment -->
首先增加一些参数，用于修改车辆周围的点云
static bool _velodyne_transform_available; static bool _downsample_cloud; static bool _pose_estimation; static double _leaf_size; static int _cluster_size_min; static int _cluster_size_max; //******************** add by hgl ********************// static double _lidar_front_car_range; static double _lidar_back_car_range; static double _lidar_left_car_range; static double _lidar_right_car_range; static double _lidar_back_range; static double _lidar_total_range; //******************** add by hgl ********************// static bool _remove_ground; // only ground
在main()函数增加参数定义及赋值
ROS_INFO("clip_max_height: %f", _clip_max_height); /******************* hgl added *******************/ private_nh.param("lidar_front_car_range", _lidar_front_car_range, 0.5); ROS_INFO("lidar_front_car_range: %f", _lidar_front_car_range); private_nh.param("lidar_back_car_range", _lidar_back_car_range, -3.0); ROS_INFO("lidar_back_car_range: %f", _lidar_back_car_range); private_nh.param("lidar_left_car_range", _lidar_left_car_range, 0.6); ROS_INFO("lidar_left_car_range: %f", _lidar_left_car_range); private_nh.param("lidar_right_car_range", _lidar_right_car_range, -0.6); ROS_INFO("lidar_right_car_range: %f", _lidar_right_car_range); private_nh.param("lidar_back_range", _lidar_back_range, -5.0); ROS_INFO("lidar_back_range: %f", _lidar_back_range); private_nh.param("lidar_total_range", _lidar_total_range, 30.0); ROS_INFO("lidar_total_range: %f", _lidar_total_range); /******************* hgl added *******************/ private_nh.param("keep_lanes", _keep_lanes, false);
增加一个函数来限定激光雷达的测量范围，在lidar_euclidean_cluster_detect.cpp中添加filteredPointsBydistance函数，用于过滤掉车辆上的扫描点及一定范围外的扫描点。
void filteredPointsBydistance(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr) { out_cloud_ptr->points.clear(); for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++) { if (in_cloud_ptr->points[i].x < _lidar_front_car_range && in_cloud_ptr->points[i].x > _lidar_back_car_range && in_cloud_ptr->points[i].y < _lidar_left_car_range && in_cloud_ptr->points[i].y > _lidar_right_car_range) continue; if (in_cloud_ptr->points[i].x < _lidar_back_range) continue; double r = sqrt(pow(in_cloud_ptr->points[i].x, 2) + pow(in_cloud_ptr->points[i].y, 2)); if (r > _lidar_total_range) continue; // if (in_cloud_ptr->points[i].x > 5 || // in_cloud_ptr->points[i].x < -5.0 || // in_cloud_ptr->points[i].y > 2 || // in_cloud_ptr->points[i].y < -2) // continue; // if (in_cloud_ptr->points[i].y > 3 || // in_cloud_ptr->points[i].y < -3) // continue; out_cloud_ptr->points.push_back(in_cloud_ptr->points[i]); } }
修改velodyne_callback函数，在点云最开始处理filteredPointsBydistance函数。
pcl::PointCloud<pcl::PointXYZ>::Ptr current_sensor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr removed_points_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr inlanes_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr nofloor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr onlyfloor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr diffnormals_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr clipped_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);// hgl modified filteredPointsBydistance函数的结果 pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_clustered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); autoware_msgs::Centroids centroids; autoware_msgs::CloudClusterArray cloud_clusters; pcl::fromROSMsg(*in_sensor_cloud, *current_sensor_cloud_ptr); _velodyne_header = in_sensor_cloud->header; /******************* hgl modified ************************/ filteredPointsBydistance(current_sensor_cloud_ptr, filtered_cloud_ptr); //hgl add if (_remove_points_upto > 0.0) { removePointsUpTo(filtered_cloud_ptr, removed_points_cloud_ptr, _remove_points_upto); // removePointsUpTo(current_sensor_cloud_ptr, removed_points_cloud_ptr, _remove_points_upto); } else removed_points_cloud_ptr = filtered_cloud_ptr; // removed_points_cloud_ptr = current_sensor_cloud_ptr; /******************* hgl modified ************************/ if (_downsample_cloud) downsampleCloud(removed_points_cloud_ptr, downsampled_cloud_ptr, _leaf_size); else downsampled_cloud_ptr = removed_points_cloud_ptr; clipCloud(downsampled_cloud_ptr, clipped_cloud_ptr, _clip_min_height, _clip_max_height);
对发出的聚类（障碍物）进行适配，修改publishCloudClusters函数，ClouldCluster主要需要的数据类型有：
std_msgs/Header header uint32 id string label float64 score sensor_msgs/PointCloud2 cloud #聚类（障碍物）点云数据 geometry_msgs/PointStamped min_point geometry_msgs/PointStamped max_point geometry_msgs/PointStamped avg_point # 与centroid_point相同 geometry_msgs/PointStamped centroid_point # 点云中心点，取平均得到。 float64 estimated_angle # 估计的朝向 geometry_msgs/Vector3 dimensions # 聚类（障碍物）的长宽高 geometry_msgs/Vector3 eigen_values geometry_msgs/Vector3[] eigen_vectors #Array of 33 floats containing the FPFH descriptor std_msgs/Float32MultiArray fpfh_descriptor jsk_recognition_msgs/BoundingBox bounding_box geometry_msgs/PolygonStamped convex_hull # Indicator information # INDICATOR_LEFT 0 # INDICATOR_RIGHT 1 # INDICATOR_BOTH 2 # INDICATOR_NONE 3 uint32 indicator_state
为了适配lidar_euclidean_cluster_detect，对publishCloudClusters做一下修改：
cluster_transformed.header.frame_id = in_target_frame; cluster_transformed.dimensions = i->dimensions; cluster_transformed.eigen_values = i->eigen_values; cluster_transformed.eigen_vectors = i->eigen_vectors; /********************* xiaochu add ************************/ cluster_transformed.avg_point.point.x = cluster_transformed.centroid_point.point.x; cluster_transformed.avg_point.point.y = cluster_transformed.centroid_point.point.y; cluster_transformed.avg_point.point.z = cluster_transformed.centroid_point.point.z; cluster_transformed.indicator_state = 3; static unsigned int id = 0; // ID 一定要添加，否则lidar_kf_contour_track算法不能对障碍物做出区分，导致障碍物失效。 cluster_transformed.id = id++; /********************* xiaochu add ************************/ clusters_transformed.clusters.push_back(cluster_transformed);
3. 调试笔记：
（1）op_common_params设置中，车辆安全框的尺寸可以参数（参考之前）设置。激光雷达会有部分扫描点集中在车辆，lidar_kf_contour_track会将这部分扫描点通过IsCar函数默认为车辆，聚类方法lidar_euclidean_cluster_detect会将这部分点聚类。当人靠近车辆时，euclidean_cluster方法会将靠近车的人和车上的扫描点聚成一类，即将靠近车的人认为是车上的点。
为了解决问题，这里的处理方法为将lidar_kf_contour_track_core.cpp中的
if(!IsCar(obj, m_CurrentPos, m_Map)) continue;
语句去掉，然后在lidar_euclidean_cluster_detect.cpp中，增加删除车辆上的语句。详见上面笛卡尔聚类增加的filteredPointsBydistance函数。
（2）避障的横向控制问题
开避障后，在调试过程中，发现转弯处发现连续变道，问题原因是一侧的障碍物比另一侧的障碍物多，trajectoryCosts.at(ic).lateral_cost总是影响主路径的选择，我们尝试过权重从1.2开始降到0.0001，发现没有效果，所以最终设置为0。最终将TrajectoryDynamicCosts.cpp中的NormalizeCosts()一条语句，修改如下：
trajectoryCosts.at(ic).cost = (m_WeightPriority*trajectoryCosts.at(ic).priority_cost + m_WeightTransition*trajectoryCosts.at(ic).transition_cost + /*m_WeightLat*/0*trajectoryCosts.at(ic).lateral_cost + m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost)/4.0;
（3）轴距修改
车辆的轴距并没有在RuntimeManager显示，在op_common_params.launch中需要设置
<arg name="wheelBaseLength" default="1.45" /> <arg name="turningRadius" default="3.0" /> <arg name="maxSteerAngle" default="0.612" />
这里wheelBaseLength指的是轴距，需要手动修改。我们车的轴距1.45。
（4）修改障碍物绕行，距离车头2/3处开始返回主路径
在车辆运行过程中，我们发现车辆在绕行障碍物的过程中，车辆大约会在运行到车头距离障碍物2/3处开始返回主路径，有时会导致车辆的安全框碰到障碍物，车辆停止运行。
这里在op_planner下的TrajectoryDynamicCosts.cpp中CalculateLateralAndLongitudinalCostsStatic函数中，有一处跟车身相关的设置，carInfo.length除以了1.5，所以会在车头距离障碍物2/3处开始返回主路径。这里将除以1.5改为除以1，就可以让车辆行驶到车尾与障碍物平行开始返回主路径。
（5）绕行贴近障碍物停车
openplanner中并没有考虑车辆绕行过程中，将车辆安全框的宽度考虑进行，导致车辆在选择路径的时候，会选择距离障碍物最近的路径，绕行过程中由于障碍物不规则，会导致障碍物的边缘进入安全框，停车。所以需要在op_planner下的TrajectoryDynamicCosts.cpp中的CalculateLateralAndLongitudinalCostsStatic函数，某处判断加入车辆安全框的宽度（冲车辆边缘多出来的部分），具体修改如下代码。
以下修改针对（4）（5）两点。
if(lateralDist - 0.2 <= critical_lateral_distance // 减去0.2，就是为了加入考量车辆安全框的宽度 && longitudinalDist >= -carInfo.length / 1.0 /*/1.5*/ && longitudinalDist < params.minFollowingDistance)
（6）用我们制作的vector map，在vector map导入时，process died的错误
产生错误的原因是因为，我们在制作vector map时，如果两个航迹点的距离较近，那么resolution的值会很小，导致skip_factor/resolution的值会很小，由于skip是有符号整数，会导致skip溢出变为负值，所以在下边加一条语句，防止溢出。将PlanningHelpers.cpp下的PlanningHelpers::GetClosestNextPointIndexFast函数，修改如下
int skip = 1; if(resolution > 0) skip = skip_factor/resolution; // xiaochun add if (skip <= 0) { skip = __INT_MAX__; } // xiaochun ad
（7）出现车辆遇到长障碍物回拐问题
进过我们长时间测试，发现如果出现10米甚至更长的障碍物时，车辆会出现回拐的现象，原因是障碍物变成了一条线，车辆认为可以通过。我们修改op_planner下的TrajectoryDynamicCosts.cpp中的CalculateLateralAndLongitudinalCostsStatic函数
if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true) trajectoryCosts.at(iCostIndex).bBlocked = true; //-------------------------add by hxc------------------------ if (((obj_info.perp_distance - distance_from_center) * (car_info.perp_distance - distance_from_center) > 0) && lateralDist < fabs(car_info.perp_distance - distance_from_center) && longitudinalDist >= -carInfo.length && longitudinalDist <= 0) { trajectoryCosts.at(iCostIndex).bBlocked = true; } //-----------------------------------------------------------
致谢：
感谢小春在整个过程中做出的努力，很多工作都是小春完成的。
原创博文，转载请标明出处。
