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

致谢：

感谢小春在整个过程中做出的努力，很多工作都是小春完成的。

原创博文，转载请标明出处。