【Autoware】Open Planner代码分析

1. 概述

本篇主要对Open Planner的代码进行分析，主要包括op_global_planner和op_local_planner两个package。
在阅读本文时，应先参照前面两篇关于Autoware即Open Planner的文章。如下列所示：
【Autoware】Autoware人机交互工具Runtime_Manager分析及使用教程

【论文阅读】Open Source Integrated Planner for Autonomous Navigation in Highly Dynamic Environments

【Autoware】OpenPlanner使用教程–从RuntimeManager面板

通过上述文章，能够使用runtime manager并在上面运行open planner的程序，且对相关的原理有一定的理解。可以结合论文一起看。

注意：
论文中提到，global planning的功能在Way Planner node中实现，local planning的功能在DP Planner node中实现，确实实现了，但是这个是老版本。新版本中，将local planning的部分分成了generator，motion_prediction，evaluator，behavior_state, common_params几个单独的模块，但在使用local planning功能时，这些节点需要同时启动。（之前一直对这件事感到很迷惑，为什么论文里的说法和代码里的不一样，后来在官方的某个README中发现关于这件事的解释: previously known as dp_planner, now it is a collection of node with each task of local planning is separated.
）

下面先用一张图表明各个package之间topic的数据流，相互之间的发布和订阅关系。

其中蓝色字体标注的是rviz的相关topic。
可以看到，由open_planner发布的lane_waypoints_array包含了这一段最小cost的global_path；
=>op_trajectory_generator订阅并处理后发布local_trajectores；然后由op_trajectory_evaluator
=>op_behavior_selector订阅并处理后发布current_behavior；
上述三个topic被op_trajectory_evaluator订阅，并同时订阅op_motion_predictor发布的predicted_objects并处理后发布local_weighted_trajectories和local_trajectory_cost

2. 包结构

Open Planner包含四个包，op_global_planner、op_local_planner、op_simulation_package、op_utilities。

• op_global_planner包含一个node，名称也是op_global_planner。
• op_local_planner包含的node多一些，有五个，分别是op_behavior_selector，op_common_params，op_motion_predictor，op_trajectory_evaluator，op_trajectory_generator，每个node对应一个launch文件，在同一个包的对应文件夹下。
• op_simulation_package包含三个node，分别是op_car_simulator、op_perception_simulator、op_signs_simulator。
• op_utilities包含三个node，op_bag_player、op_data_logger、op_pose2tf。
  基本上每一个node都有一个对应的和node同名的主文件和一个core cpp文件。

实际上这里就对应了Runtime Manager面板中复选框可以选择的内容，除了op_data_logger包之外。意味着每在runtime_manager中选择一个package的复选框，即为启动该package，效用等同于运行

$ roslaunch <package_name> <launch_file> # 这是roslaunch的语法

比如Computing Tab页中，可以在对应的computing.yaml配置文件中可以看到如下描述

- name : OpenPlanner - Local planning
      desc : Local Planner for OpenPlanner
      subs :
      - name : op_common_params
        desc : load common local planning parameters
        cmd  : roslaunch op_local_planner op_common_params.launch
        param: op_common_params
        gui  :
          dialog_width  : 600
          dialog_height : 550

op_common_params对应的cmd就是roslaunch op_local_planner op_common_params.launch

3. op_global_planner

输入输出在ROS中主要以topic的方式进行，op_global_planner订阅和发布的topic分别是：
构造函数中订阅：

sub_start_pose = nh.subscribe("/initialpose", 1, &GlobalPlanner::callbackGetStartPose, this);
sub_goal_pose = nh.subscribe("move_base_simple/goal", 1, &GlobalPlanner::callbackGetGoalPose, this);
sub_current_pose = nh.subscribe("/current_pose", 10, &GlobalPlanner::callbackGetCurrentPose, this);
// 下面三个是if-else关系，三选一
sub_robot_odom = nh.subscribe("/odom", 10,  &TrajectoryGen::callbackGetRobotOdom, this);
sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this);
sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this);
//Mapping Section
sub_lanes = nh.subscribe("/vector_map_info/lane", 1, &BehaviorGen::callbackGetVMLanes,  this);
sub_points = nh.subscribe("/vector_map_info/point", 1, &BehaviorGen::callbackGetVMPoints,  this);
sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1, &BehaviorGen::callbackGetVMdtLanes,  this);
sub_intersect = nh.subscribe("/vector_map_info/cross_road", 1, &BehaviorGen::callbackGetVMIntersections,  this);
sup_area = nh.subscribe("/vector_map_info/area", 1, &BehaviorGen::callbackGetVMAreas,  this);
sub_lines = nh.subscribe("/vector_map_info/line", 1, &BehaviorGen::callbackGetVMLines,  this);
sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1, &BehaviorGen::callbackGetVMStopLines,  this);
sub_signals = nh.subscribe("/vector_map_info/signal", 1, &BehaviorGen::callbackGetVMSignal,  this);
sub_vectors = nh.subscribe("/vector_map_info/vector", 1, &BehaviorGen::callbackGetVMVectors,  this);
sub_curbs = nh.subscribe("/vector_map_info/curb", 1, &BehaviorGen::callbackGetVMCurbs,  this);
sub_edges = nh.subscribe("/vector_map_info/road_edge", 1, &BehaviorGen::callbackGetVMRoadEdges,  this);
sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1, &BehaviorGen::callbackGetVMWayAreas,  this);
sub_cross_walk = nh.subscribe("/vector_map_info/cross_walk", 1, &BehaviorGen::callbackGetVMCrossWalks,  this);
sub_nodes = nh.subscribe("/vector_map_info/node", 1, &BehaviorGen::callbackGetVMNodes,  this);

发布的topic包括：

pub_Paths = nh.advertise<autoware_msgs::LaneArray>("lane_waypoints_array", 1, true);
pub_PathsRviz = nh.advertise<visualization_msgs::MarkerArray>("global_waypoints_rviz", 1, true);
pub_MapRviz  = nh.advertise<visualization_msgs::MarkerArray>("vector_map_center_lines_rviz", 1, true);
pub_GoalsListRviz = nh.advertise<visualization_msgs::MarkerArray>("op_destinations_rviz", 1, true);

左侧是c++中定义的变量名，存储数据，右侧为topic的名称，可以在rviz中添加显示。
其中nh为nodehandle句柄。advertise函数是ros中定义的用来BasePublisher类下面的发布function。

3.1 op_global_planner_core.cpp中代码的主要逻辑

主要的业务逻辑在MainLoop()函数中，

1. 支持三种地图格式，分别为MAP_KML_FILE,MAP_FOLDER,MAP_AUTOWARE
   这几个参数是从launch配置文件中读取的，其代表的含义分别为kml格式的google地图文件，地图文件夹中的文件，autoware自定义格式的文件。

2. 起点终点定义

PlannerHNS::WayPoint m_CurrentPose;
std::vector<PlannerHNS::WayPoint> m_GoalsPos;

注意这里，起点是一个点，终点是包含多个waypoint的vector，在实际运行中，第一次设置起点和终点后，会先从当前位置or起始位置规划到第一个终点的位置，然后再规划走到vector里面的下一个终点。

3. 生成全局路径的函数为GenerateGlobalPlan，调用方法为

bool bNewPlan = GenerateGlobalPlan(m_CurrentPose, goalPoint, m_GeneratedTotalPaths);

其中调用了PlannerH.cpp中的PlanUsingDP()方法

3.1.1 PlanUsingDP代码分析

3.1.1.1 代码

double PlannerH::PlanUsingDP(const WayPoint& start,
    const WayPoint& goalPos,
    const double& maxPlanningDistance,
    const bool bEnableLaneChange,
    const std::vector<int>& globalPath,
    RoadNetwork& map,
    std::vector<std::vector<WayPoint> >& paths, vector<WayPoint*>* all_cell_to_delete){...}

3.1.1.2 代码逻辑分析

先获取起点和终点位置，然后
a. 调用GetCloseWaypointFromMap()方法获得vector map中的最近的pStart和pGoal，
b. 调用GetRelativeInfo()方法获得pStart和pGoal的relativeInfo， start_info和goal_info即相关信息，数据类型为RelativeInfo，定义在RoadNetwork.h中。
调用代码为：

PlanningHelpers::GetRelativeInfo(pStart->pLane->points, start, start_info);

GetRelativeInfo的定义代码为：

bool PlanningHelpers::GetRelativeInfo(const std::vector<WayPoint>& trajectory, const WayPoint& p, RelativeInfo& info, const int& prevIndex)

通过函数定义可以方便的查看各个变量的数据类型。

c. 调用PlanningHelpers.cpp中的BuildPlanningSearchTreeV2方法，寻找是否存在由pStart到达pGoal的最短路径

这个函数的大致逻辑为：
使用动态规划DP算法寻找是否存在由pStart到达pGoal的最短路径。会从pStart开始，不断遍历它能够到达的周边的下一个waypoint，（判断如果enbaleLaneChange，会找它的pLeft和pRight的wayPoint，这在前面构建vector map时会创建；如果没有enbaleLaneChange，则找它pFronts里的所有waypoints），加入到nextLeafToTrace，然后找到nextLeafToTrace里面WayPoint->cost最小的那个waypoint，把它设为选择的当前waypoint（代码里为pH），然后继续遍历nextLeafToTrace。本质上为贪心算法，只要保证在所有可到达的waypoint里，选择cost最小的那个waypoint，就保证里local optimal solution。这时保证有一条path with min_cost能够到达goal，并返回最终找到的那个waypoint（代码里为pH）。
本段参考自另一篇文章，原文链接：https://blog.csdn.net/weixin_41697321/article/details/125088981

d. 使用BuildPlanningSearchTreeStraight再次寻找路径

回到PlanUsingDP函数，如果BuildPlanningSearchTreeV2失败，则会调用BuildPlanningSearchTreeStraight再次寻找路径，它和BuildPlanningSearchTreeV2的区别在于它不会考虑laneChange造成的pLeft和pRight的wayPoint，只考虑pFronts里的waypoints。
接下来调用TraversePathTreeBackwards函数，它是由BuildPlanningSearchTreeV2返回的能到达的最接近goal的waypoint（pHead)，遍历回到pStart。首先pHead会按直路遍历回pStart，如果存在laneChange导致的pLeft或者pRight，就会先调到pLeft或者pRight，然后遍历回到pStart。找到的全部路径会存放进PlanUsingDP函数里的vector path里。
本段参考自另一篇文章，原文链接：https://blog.csdn.net/weixin_41697321/article/details/125088981

3.1.2 回到op_global_planner的MainLoop中的逻辑：

生成的路径存储在m_GeneratedTotalPaths参数中，然后调用VisualizeAndSend()将数据可视化，发布到对应的topic中以使其在rviz中显示

pub_GoalsListRviz.publish(goals_array);

其中goals_array变量的类型为visualization_msgs::MarkerArray goals_array;。
m_GeneratedTotalPaths的变量类型如下，定义在op_global_planner_core.h文件中，属于protected类型的变量

std::vector<std::vector<PlannerHNS::WayPoint> > m_GeneratedTotalPaths;

3.1.3 Start and Goal position

op_global_planner_core.h

PlannerHNS::WayPoint m_CurrentPose;
std::vector<PlannerHNS::WayPoint> m_GoalsPos;

4. op_local_planner

分别有5个node，每个node之间的流程如下图所示：

4.1 op_trajectory_generator

主要的业务逻辑依然是在MainLoop函数里面

4.1.1 订阅的topic

sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this);
sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryGen::callbackGetCurrentPose, this);
sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this);//global_planner输出的topic
//依然的三选一,默认值是current_velocity
"/odom";"/current_velocity";"/can_info"

4.1.2 发布的topic

pub_LocalTrajectories = nh.advertise<autoware_msgs::LaneArray>("local_trajectories", 1);//作为op_trajectory_evaluator的订阅
pub_LocalTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("local_trajectories_gen_rviz", 1);

4.1.3 基础参数读取

在op_trajectory_generator.launch中，提供了4个参数类型，与代码中的对应关系如下
launch.samplingTipMargin <=> cpp.carTipMargin
launch.samplingOutMargin <=> cpp.rollInMargin
samplingSpeedFactor <=> rollInSpeedFactor
enableHeadingSmoothing <=> enableHeadingSmoothing
其中，可以在runtime_manager面板中设置的参数如下

有一些其他参数需要在op_common_params.launch中读取，在runtime_manager面板中也可以进行相应设置。

要注意的是，对应论文中的这张图来看，cartip对应的就是代码中的carTipMargin变量，Roll in对应的是代码中的rollInMargin变量，而Roll out对于common params中设置的rollOutDensity和rollOutNumber，分别定义Roll out的密度和个数。

4.1.4 生成rollOuts的方法

MainLoop()中的主要逻辑分三段:

1. ExtractPartFromPointToDistanceDirectionFast()
   调用PlanningHelpers中的对应函数，传入参数为原始的globalPath，一组WayPoint的vector，currentPos，最小距离，路径密度，返回extractedPath

2. GenerateRunoffTrajectory()

   调用PlanningHelpers中的对应函数，然后调用PlanningHelpers中的CalculateRollInTrajectories()函数，拿到的返回值为local_rollOutPaths,在CalculateRollInTrajectories()函数中的对应变量名称为rollInPaths

3. PredictConstantTimeCostForTrajectory() 和 ConvertFromLocalLaneToAutowareLane()

4.1.4.1 MainLoop()中的GenerateRunoffTrajectory

MainLoop()中，GenerateRunoffTrajectory用来生成具体的路径。

std::vector<PlannerHNS::WayPoint> sampledPoints_debug;
      m_Planner.GenerateRunoffTrajectory(m_GlobalPathSections, m_CurrentPos,
                m_PlanningParams.enableLaneChange,
                m_VehicleStatus.speed,
                m_PlanningParams.microPlanDistance,
                m_PlanningParams.maxSpeed,
                m_PlanningParams.minSpeed,
                m_PlanningParams.carTipMargin,
                m_PlanningParams.rollInMargin,
                m_PlanningParams.rollInSpeedFactor,
                m_PlanningParams.pathDensity,
                m_PlanningParams.rollOutDensity,
                m_PlanningParams.rollOutNumber,
                m_PlanningParams.smoothingDataWeight,
                m_PlanningParams.smoothingSmoothWeight,
                m_PlanningParams.smoothingToleranceError,
                m_PlanningParams.speedProfileFactor,
                m_PlanningParams.enableHeadingSmoothing,
                -1 , -1,
                m_RollOuts, sampledPoints_debug);

调用的是PlannerH.cpp里面的GenerateRunoffTrajectory()函数。

明显可以看到这个函数的输入需要m_GlobalPathSections，就是全局路径的部分；以及m_CurrentPos当前位置；etc. 这里完全可以和论文中的5.1部分描述的需要的输入对应上。

其中m_GlobalPaths是从callbackGetGlobalPlannerPath订阅lane_waypoints_array这个topic获取的，然后在MainLoop里面经过ExtractPartFromPointToDistanceDirectionFast方法的初步处理后，放在m_GlobalPathSections变量中。

其中返回的m_RollOuts是rollOutsPaths，里面包含了多条local_rollOutPaths

rollOutsPaths.push_back(local_rollOutPaths);

生成之后，通过TrajectoriesToMarkers函数将rollouts转换成marker以在rviz中显示，进而publish

visualization_msgs::MarkerArray all_rollOuts;
PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts);
pub_LocalTrajectoriesRviz.publish(all_rollOuts);

4.1.4.2 PlannerH中的GenerateRunoffTrajectory方法

书接上回，4.1中描述的这个generate方法，
定义如下

void PlannerH::GenerateRunoffTrajectory(const std::vector<std::vector<WayPoint> >& referencePaths,const WayPoint& carPos, const bool& bEnableLaneChange, const double& speed, const double& microPlanDistance,
    const double& maxSpeed,const double& minSpeed, const double&  carTipMargin, const double& rollInMargin,
    const double& rollInSpeedFactor, const double& pathDensity, const double& rollOutDensity,
    const int& rollOutNumber, const double& SmoothDataWeight, const double& SmoothWeight,
    const double& SmoothTolerance, const double& speedProfileFactor, const bool& bHeadingSmooth,
    const int& iCurrGlobalPath, const int& iCurrLocalTraj,
    std::vector<std::vector<std::vector<WayPoint> > >& rollOutsPaths,
    std::vector<WayPoint>& sampledPoints_debug){...}

可以看到4.1中输入的m_GlobalPathSections在这个方法中是被作为referencePath参考路径。
如果referencePaths参考路径的数量不为零，且microPlanDistance最小规划路径长度不小于零，则继续生成rollOuts。
if(referencePaths.at(i).size()>0)如果这组globalPath里面的第i段path中的waypoints数量大于零，使用PlanningHelpers::CalculateRollInTrajectories函数生成local_rollOutPaths

4.1.4.3 PlanningHelpers::CalculateRollInTrajectories()

在这个函数里面，最终返回的变量是rollInPaths，只是变量名的不同，对应的是4.2中拿到的local_rollOutPaths变量。

这部分代码要结合论文来看，如论文的5.1部分描述的，主要的算法逻辑都在整个function里面。

且规划部分的主要算法实现，都在PlanningHelpers.cpp文件中。

void PlanningHelpers::CalculateRollInTrajectories(const WayPoint& carPos, const double& speed, const vector<WayPoint>& originalCenter, int& start_index,
    int& end_index, vector<double>& end_laterals ,
    vector<vector<WayPoint> >& rollInPaths, const double& max_roll_distance,
    const double& maxSpeed, const double&  carTipMargin, const double& rollInMargin,
    const double& rollInSpeedFactor, const double& pathDensity, const double& rollOutDensity,
    const int& rollOutNumber, const double& SmoothDataWeight, const double& SmoothWeight,
    const double& SmoothTolerance, const bool& bHeadingSmooth,
    std::vector<WayPoint>& sampledPoints)
{ 
  WayPoint p;
  double dummyd = 0;

  //iLimitIndex是限制总waypoints个数的
  int iLimitIndex = (carTipMargin/0.3)/pathDensity;//默认值就是(4/0.3)/0.5约等于27=>???为什么除以0.3
  if(iLimitIndex >= originalCenter.size())//originalCenter是调用方法传入的referencePaths中的第i段，orginalCenter.size()就是第i段global path的waypoint个数
    iLimitIndex = originalCenter.size() - 1;

  //Get Closest Index
  RelativeInfo info;
  GetRelativeInfo(originalCenter, carPos, info);//carPos就是trajectory_generator里面传入的m_CurrentPos
  double remaining_distance = 0;
  int close_index = info.iBack;//将back点的index赋给close_index
  for(unsigned int i=close_index; i< originalCenter.size()-1; i++)//遍历originalCenter里面的所有waypoints
    {
    if(i>0)
      remaining_distance += distance2points(originalCenter[i].pos, originalCenter[i+1].pos);//计算每两个相邻点的距离，累加，就是这段globalPath的总距离
    }
  
  //初始的rollIn距离就是GetRelativeInfo里面计算得到的垂直距离
  double initial_roll_in_distance = info.perp_distance ; //GetPerpDistanceToTrajectorySimple(originalCenter, carPos, close_index);


  vector<WayPoint> RollOutStratPath;
  
  //calculate the starting index
  double d_limit = 0;
  unsigned int far_index = close_index;//close_index，前面iBack点的index

  //calculate end index
  double start_distance = rollInSpeedFactor*speed+rollInMargin;//初始位置由速度因子×速度+rollInMargin决定
  if(start_distance > remaining_distance)//如果初始位置已经超出了总距离，将总距离赋值给初始位置
    start_distance = remaining_distance;

  d_limit = 0;
  for(unsigned int i=close_index; i< originalCenter.size(); i++)//从close_index开始，就是iBack点的index开始，为originalCenter中的每个点，计算和它和前一个点的distance
    {
      if(i>0)
        d_limit += distance2points(originalCenter[i].pos, originalCenter[i-1].pos);

      if(d_limit >= start_distance)//当所有路径点之间的累计距离大于初始距离时，将far_index的值设为当前这个waypoint的index，跳出循环
      {
        far_index = i;
        break;
      }
    }

  int centralTrajectoryIndex = rollOutNumber/2;//中间trajectory的index就是所有rollOut数目的一半
  vector<double> end_distance_list;
  for(int i=0; i< rollOutNumber+1; i++)//遍历rollOutNumber，用rollOutDensity乘...算出来纵向上，每个rollOut的距离，距离central的距离
    {
      double end_roll_in_distance = rollOutDensity*(i - centralTrajectoryIndex);
      end_distance_list.push_back(end_roll_in_distance);
    }

  start_index = close_index;
  end_index = far_index;
  end_laterals = end_distance_list;//laterals:横向

  /***平滑轨迹，采用共轭梯度的方法，是一项非线性插值优化技术，同时提升曲率***/
  /**这部分应该是分段nSteps**/
  //calculate the actual calculation starting index
  d_limit = 0;
  unsigned int smoothing_start_index = start_index;
  unsigned int smoothing_end_index = end_index;

  //从起点开始算一遍，从终点开始算一遍，找所有路径点累计距离大于carTipMargin的路径点的index，找到之前，每次都给flag+1
  for(unsigned int i=smoothing_start_index; i< originalCenter.size(); i++)//smoothing_start_index
  {
    if(i > 0)
      d_limit += distance2points(originalCenter[i].pos, originalCenter[i-1].pos);
    if(d_limit > carTipMargin)//当所有路径点之间的累计距离大于carTipMargin时时，跳出循环
      break;

    smoothing_start_index++;//start；每遍历一个点，smoothing_start_index+1
  }

  d_limit = 0;
  for(unsigned int i=end_index; i< originalCenter.size(); i++)//end_index
  {
    if(i > 0)
      d_limit += distance2points(originalCenter[i].pos, originalCenter[i-1].pos);
    if(d_limit > carTipMargin)
      break;

    smoothing_end_index++;//end；
  }

  int nSteps = end_index - smoothing_start_index;


  vector<double> inc_list;
  rollInPaths.clear();
  vector<double> inc_list_inc;
  for(int i=0; i< rollOutNumber+1; i++)
  {
    double diff = end_laterals.at(i)-initial_roll_in_distance;
    inc_list.push_back(diff/(double)nSteps);
    rollInPaths.push_back(vector<WayPoint>());
    inc_list_inc.push_back(0);
  }



  vector<vector<WayPoint> > execluded_from_smoothing;//构造空vector，里面存储waypoint的vector
  for(unsigned int i=0; i< rollOutNumber+1 ; i++)
    execluded_from_smoothing.push_back(vector<WayPoint>());


  /***这部分就是采样***/
  //Insert First strait points within the tip of the car range
  for(unsigned int j = start_index; j < smoothing_start_index; j++)//为什么要小于smoothing_start_index？？
  {
    p = originalCenter.at(j);//遍历初始一段路径的所有路径点
    double original_speed = p.v;
    for(unsigned int i=0; i< rollOutNumber+1 ; i++)
    {
      p.pos.x = originalCenter.at(j).pos.x -  initial_roll_in_distance*cos(p.pos.a + M_PI_2);
      p.pos.y = originalCenter.at(j).pos.y -  initial_roll_in_distance*sin(p.pos.a + M_PI_2);
      if(i!=centralTrajectoryIndex)
        p.v = original_speed * LANE_CHANGE_SPEED_FACTOR;
      else
        p.v = original_speed ;

      if(j < iLimitIndex)//如果waypoint在初始路径总路径点个数内，就不用进行平滑
        execluded_from_smoothing.at(i).push_back(p);
      else
        rollInPaths.at(i).push_back(p);//否则加入到rollInPaths中去

      sampledPoints.push_back(p);//无论是否需要平滑，都是采样的点集里面的
    }
  }

  for(unsigned int j = smoothing_start_index; j < end_index; j++)
    {
      p = originalCenter.at(j);
      double original_speed = p.v;
      for(unsigned int i=0; i< rollOutNumber+1 ; i++)
      {
        inc_list_inc[i] += inc_list[i];
        double d = inc_list_inc[i];
        //这两句就是算采样点的计算方法
        p.pos.x = originalCenter.at(j).pos.x -  initial_roll_in_distance*cos(p.pos.a + M_PI_2) - d*cos(p.pos.a+ M_PI_2);
        p.pos.y = originalCenter.at(j).pos.y -  initial_roll_in_distance*sin(p.pos.a + M_PI_2) - d*sin(p.pos.a+ M_PI_2);
        if(i!=centralTrajectoryIndex)
          p.v = original_speed * LANE_CHANGE_SPEED_FACTOR;
        else
          p.v = original_speed ;

        rollInPaths.at(i).push_back(p);

        sampledPoints.push_back(p);
      }
    }

  //Insert last strait points to make better smoothing
  for(unsigned int j = end_index; j < smoothing_end_index; j++)
  {
    p = originalCenter.at(j);
    double original_speed = p.v;
    for(unsigned int i=0; i< rollOutNumber+1 ; i++)
    {
      double d = end_laterals.at(i);//就是前面的diff/(double)nSteps;diff = end_laterals.at(i)-initial_roll_in_distance;
      //这两句就是算采样点的计算方法
      p.pos.x  = originalCenter.at(j).pos.x - d*cos(p.pos.a + M_PI_2);
      p.pos.y  = originalCenter.at(j).pos.y - d*sin(p.pos.a + M_PI_2);
      if(i!=centralTrajectoryIndex)
        p.v = original_speed * LANE_CHANGE_SPEED_FACTOR;
      else
        p.v = original_speed ;
      rollInPaths.at(i).push_back(p);

      sampledPoints.push_back(p);
    }
  }

  for(unsigned int i=0; i< rollOutNumber+1 ; i++)//把参数2到3的之间的所有点插入到参数1的位置
    rollInPaths.at(i).insert(rollInPaths.at(i).begin(), execluded_from_smoothing.at(i).begin(), execluded_from_smoothing.at(i).end());
    //std::cout << "rollInPaths after insert:" << rollInPaths.at(i).toString() << std::endl;

  ///***   Smoothing From Car Heading Section ***///
//  if(bHeadingSmooth)
//  {
//    for(unsigned int i=0; i< rollOutNumber+1 ; i++)
//    {
//      unsigned int cut_index = GetClosestNextPointIndex(rollInPaths.at(i), RollOutStratPath.at(RollOutStratPath.size()-1));
//      rollInPaths.at(i).erase(rollInPaths.at(i).begin(), rollInPaths.at(i).begin()+cut_index);
//      rollInPaths.at(i).insert(rollInPaths.at(i).begin(), RollOutStratPath.begin(), RollOutStratPath.end());
//    }
//  }
  ///***   -------------------------------- ***///



  d_limit = 0;
  for(unsigned int j = smoothing_end_index; j < originalCenter.size(); j++)
    {
    if(j > 0)
      d_limit += distance2points(originalCenter.at(j).pos, originalCenter.at(j-1).pos);

    if(d_limit > max_roll_distance)//max_roll_distance是函数参数，PlannerH里面传入的microPlanDistance，common_params文件中读取的，含义为maxLocalPlanDistance，默认值50
      break;

      p = originalCenter.at(j);
      double original_speed = p.v;
      for(unsigned int i=0; i< rollInPaths.size() ; i++)
      {
        double d = end_laterals.at(i);
        p.pos.x  = originalCenter.at(j).pos.x - d*cos(p.pos.a + M_PI_2);
        p.pos.y  = originalCenter.at(j).pos.y - d*sin(p.pos.a + M_PI_2);

        if(i!=centralTrajectoryIndex)
          p.v = original_speed * LANE_CHANGE_SPEED_FACTOR;
        else
          p.v = original_speed ;

        rollInPaths.at(i).push_back(p);

        sampledPoints.push_back(p);
      }
    }

  for(unsigned int i=0; i< rollOutNumber+1 ; i++)
  {
    SmoothPath(rollInPaths.at(i), SmoothDataWeight, SmoothWeight, SmoothTolerance);//三个参数均是函数直接传参,前面每设置参数值，用的定义函数时的默认值
  }

//  for(unsigned int i=0; i< rollInPaths.size(); i++)
//    CalcAngleAndCost(rollInPaths.at(i));
}

4.1.4.4 SmoothPath

论文中提到，这里使用了共轭梯度(CG)的算法。
梯度、散度、旋度这篇文章详细介绍了梯度的概念，梯度就是上升速度最快的方向。
常用的最速梯度下降法，就是沿着梯度的反方向行进。结合维基百科中介绍的，如下图所示：

绿色线是按照最速梯度下降的轨迹，而这个轨迹比较陡峭，为了使轨迹更加平滑，使用共轭梯度来进行计算。

论文中提及是在[这本书](https://epubs.siam.org/doi/book/10.1137/1.9780898718003 https://www-users.cse.umn.edu/~saad/IterMethBook_2ndEd.pdf)里面的Chapter6，看了下应该是6.7和6.8两节，介绍了conjugate gradient，但具体的没太看懂。

代码中具体的计算方式，和这篇文章轨迹平滑方法中提到的计算方式完全相同。但不知道这位大佬从哪里找到的这个算法。

SmoothPath function不仅在local_trajectory_generator中有使用，在global_planning_core中和其他许多个地方，都有被使用来做路径平滑，因此是一个很重要的函数。

void PlanningHelpers::SmoothPath(vector<WayPoint>& path, double weight_data,
    double weight_smooth, double tolerance)
    /**头文件中定义了参数默认值static void SmoothPath(std::vector<WayPoint>& path, double weight_data =0.25,double weight_smooth = 0.25,double tolerance = 0.01)
    每次被调用时，可能都会被赋予不同的weight参数**/
{

  if (path.size() <= 2 )
  {
    //cout << "Can't Smooth Path, Path_in Size=" << path.size() << endl;
    return;
  }

  const vector<WayPoint>& path_in = path;
  vector<WayPoint> smoothPath_out =  path_in;//目前path_in和smoothPath_out里面存放的都是传入的path参数

  double change = tolerance;
  double xtemp, ytemp;
  int nIterations = 0;//只是一个计数器，不参与逻辑判断

  int size = path_in.size();//也就是path中点的总个数

  while (change >= tolerance)//change最一开始等于tolerance
  {
    change = 0.0;//change每次都被重置为零，则下一次while判断的时候，只累计了x和y坐标分别在平滑后的值和原值的差异，这个值大于等于tolerance，就会继续平滑
    for (int i = 1; i < size - 1; i++)
    {
//      if (smoothPath_out[i].pos.a != smoothPath_out[i - 1].pos.a)
//        continue;

      xtemp = smoothPath_out[i].pos.x;
      ytemp = smoothPath_out[i].pos.y;//把原来的xy的坐标都存放在临时变量中

      smoothPath_out[i].pos.x += weight_data//一阶导:a2-a1
          * (path_in[i].pos.x - smoothPath_out[i].pos.x);
      smoothPath_out[i].pos.y += weight_data
          * (path_in[i].pos.y - smoothPath_out[i].pos.y);

      smoothPath_out[i].pos.x += weight_smooth//非连续点的二阶导，Hessian matrix,(a3-a2)-(a2-a1)=a3+a1-2*a2
          * (smoothPath_out[i - 1].pos.x + smoothPath_out[i + 1].pos.x
              - (2.0 * smoothPath_out[i].pos.x));//权重0.25*(前后两个点的x坐标之和减去当前坐标的2倍)，累加到原坐标上
      smoothPath_out[i].pos.y += weight_smooth
          * (smoothPath_out[i - 1].pos.y + smoothPath_out[i + 1].pos.y
              - (2.0 * smoothPath_out[i].pos.y));

      change += fabs(xtemp - smoothPath_out[i].pos.x);
      change += fabs(ytemp - smoothPath_out[i].pos.y);

    }
    nIterations++;
  }

  path = smoothPath_out;
}

使用轨迹平滑后，可以得到平滑的轨迹曲线，如下图所示，global 和local trajectory都有做轨迹平滑，即使在转弯处也可获得不错的效果。

4.1.4.5 GetRelativeInfo

bool PlanningHelpers::GetRelativeInfo(const std::vector<WayPoint>& trajectory, const WayPoint& p, RelativeInfo& info, const int& prevIndex )
{
  if(trajectory.size() < 2) return false;//不判断少于两个waypoint的路径

  WayPoint p0, p1;
  if(trajectory.size()==2)//如果只有两个路径点，前一个点的index是0，后一个点的index是1.p0点就是第一个路径点，p1点是两个路径点的中间点。
  {
    p0 = trajectory.at(0);
    p1 = WayPoint((trajectory.at(0).pos.x+trajectory.at(1).pos.x)/2.0,
            (trajectory.at(0).pos.y+trajectory.at(1).pos.y)/2.0,
            (trajectory.at(0).pos.z+trajectory.at(1).pos.z)/2.0, trajectory.at(0).pos.a);//a是GPSPoint的direction
    info.iFront = 1;
    info.iBack = 0;
  }
  else
  {
    info.iFront = GetClosestNextPointIndexFast(trajectory, p, prevIndex);//找到距离最近的一个下一个waypoint的index，该函数返回值是mini_index
//    WayPoint p2 = p;
//    int old_index = GetClosestNextPointIndex(trajectory, p2, prevIndex);
//    if(old_index != info.iFront)
//      cout << " Alert Alert !!!! fast: " << info.iFront << ", slow: " << old_index  << endl;

    if(info.iFront > 0)//如果前点的index大于零，那back点就是front点的前一个index上的点
      info.iBack = info.iFront -1;
    else
      info.iBack = 0;//否则back点就是起始点，index为0

    if(info.iFront == 0)//如果front点的index为0，那么把路径点在index=0处的点，也就是front点赋值给p0，下一个点即index=1处的点赋值给p1
    {
      p0 = trajectory.at(info.iFront);
      p1 = trajectory.at(info.iFront+1);
    }
    else if(info.iFront > 0 && info.iFront < trajectory.size()-1)//如果front点的index大于零，且小于总路径点个数-1的值，那么把front点赋值给p1，front前一个index的点赋值给p0
    {
      p0 = trajectory.at(info.iFront-1);
      p1 = trajectory.at(info.iFront);
    }
    else//如果front点的index值>=路径点总个数-1，这种应该就只有等于一种可能了吧，把p0点设为front的前一个点;p1点设置为p0点和front点的中间点？？？
    {
      p0 = trajectory.at(info.iFront-1);
      p1 = WayPoint((p0.pos.x+trajectory.at(info.iFront).pos.x)/2.0, (p0.pos.y+trajectory.at(info.iFront).pos.y)/2.0, (p0.pos.z+trajectory.at(info.iFront).pos.z)/2.0, p0.pos.a);
    }
  }

  WayPoint prevWP = p0;//p0赋值给前一个waypoint
  Mat3 rotationMat(-p1.pos.a);//rotation matrix R
  Mat3 translationMat(-p.pos.x, -p.pos.y);//translation matrix t
  Mat3 invRotationMat(p1.pos.a);//rotation matrix的逆 R-1，用一个GPSPoint的direction构造
  Mat3 invTranslationMat(p.pos.x, p.pos.y);//translation matrix t的逆

  p0.pos = translationMat*p0.pos;//把p0的pos做坐标系转换，分别左乘translation和rotation
  p0.pos = rotationMat*p0.pos;

  p1.pos = translationMat*p1.pos;
  p1.pos = rotationMat*p1.pos;

  double m = (p1.pos.y-p0.pos.y)/(p1.pos.x-p0.pos.x);
  info.perp_distance = p1.pos.y - m*p1.pos.x; // solve for x = 0 //perpendicular distance点到线的垂直距离
  //链接https://byjus.com/question-answer/write-the-formula-for-the-perpendicular-distance-of-a-point-from-a-line/

  if(std::isnan(info.perp_distance) || std::isinf(info.perp_distance)) info.perp_distance = 0;

  info.to_front_distance = fabs(p1.pos.x); // distance on the x axes


  info.perp_point = p1;
  info.perp_point.pos.x = 0; // on the same y axis of the car
  info.perp_point.pos.y = info.perp_distance; //perp distance between the car and the trajectory

  info.perp_point.pos = invRotationMat  * info.perp_point.pos;
  info.perp_point.pos = invTranslationMat  * info.perp_point.pos;

  info.from_back_distance = hypot(info.perp_point.pos.y - prevWP.pos.y, info.perp_point.pos.x - prevWP.pos.x);//hypot函数是cmath里面的函数，求两个点的斜边

  info.angle_diff = UtilityH::AngleBetweenTwoAnglesPositive(p1.pos.a, p.pos.a)*RAD2DEG;

  return true;
}

4.1.4.5 ExtractPartFromPointToDistanceDirectionFast

void PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(const vector<WayPoint>& originalPath, const WayPoint& pos, const double& minDistance,
    const double& pathDensity, vector<WayPoint>& extractedPath)
{
  if(originalPath.size() < 2 ) return;

  extractedPath.clear();

  int close_index = GetClosestNextPointIndexDirectionFast(originalPath, pos);//给定global path和current_pos，得到在某个方向上最近的点的index
  double d = 0;

  if(close_index + 1 >= originalPath.size())
    close_index = originalPath.size() - 2;

  for(int i=close_index; i >=  0; i--)//close_index是中间的分割点，这个是从close_index开始往前找点
  {
    extractedPath.insert(extractedPath.begin(),  originalPath.at(i));
    if(i < originalPath.size())
      d += hypot(originalPath.at(i).pos.y - originalPath.at(i+1).pos.y, originalPath.at(i).pos.x - originalPath.at(i+1).pos.x);
    if(d > 10)//为何要大于10
      break;
  }

  //extractedPath.push_back(info.perp_point);
  d = 0;
  for(int i=close_index+1; i < (int)originalPath.size(); i++)//close_index是中间的分割点，这个是从close_index开始往后找点
  {
    extractedPath.push_back(originalPath.at(i));
    if(i > 0)
      d += hypot(originalPath.at(i).pos.y - originalPath.at(i-1).pos.y, originalPath.at(i).pos.x - originalPath.at(i-1).pos.x);
    if(d > minDistance)//??
      break;
  }

  if(extractedPath.size() < 2)
  {
    cout << endl << "### Planner Z . Extracted Rollout Path is too Small, Size = " << extractedPath.size() << endl;
    return;
  }

  FixPathDensity(extractedPath, pathDensity);
  CalcAngleAndCost(extractedPath);
}

4.1.4.6 其他相关但不太要紧的函数，只有少部分

void PlanningHelpers::FixPathDensity(vector<WayPoint>& path, const double& distanceDensity)
{
  if(path.size() == 0 || distanceDensity==0) return;//特殊情况处理

  double d = 0, a = 0;
  double margin = distanceDensity*0.01;
  double remaining = 0;
  int nPoints = 0;
  vector<WayPoint> fixedPath;
  fixedPath.push_back(path.at(0));
  for(unsigned int si = 0, ei=1; ei < path.size(); )
  {
    d += hypot(path.at(ei).pos.x- path.at(ei-1).pos.x, path.at(ei).pos.y- path.at(ei-1).pos.y) + remaining;
    a = atan2(path.at(ei).pos.y - path.at(si).pos.y, path.at(ei).pos.x - path.at(si).pos.x);//返回y，x的反正切

    ...
}

5. op_behavior_selector

同样的主要业务逻辑在MainLoop里面

5.1 订阅的topic

sub_current_pose = nh.subscribe("/current_pose", 10,  &BehaviorGen::callbackGetCurrentPose, this);//必选项
//传统三选一,默认值是current_velocity
"/odom";"/current_velocity";"/can_info"
//
sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this);//global_planner输出的topic
sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories", 1, &BehaviorGen::callbackGetLocalPlannerPath, this);
sub_TrafficLightStatus = nh.subscribe("/light_color", 1, &BehaviorGen::callbackGetTrafficLightStatus, this);
sub_TrafficLightSignals  = nh.subscribe("/roi_signal", 1, &BehaviorGen::callbackGetTrafficLightSignals, this);
sub_Trajectory_Cost = nh.subscribe("/local_trajectory_cost", 1, &BehaviorGen::callbackGetLocalTrajectoryCost, this);

sub_twist_raw = nh.subscribe("/twist_raw", 1, &BehaviorGen::callbackGetTwistRaw, this);
sub_twist_cmd = nh.subscribe("/twist_cmd", 1, &BehaviorGen::callbackGetTwistCMD, this);

//Mapping Section
//...这部分同上

5.2 发布的topic

pub_LocalPath = nh.advertise<autoware_msgs::Lane>("final_waypoints", 1,true);
pub_LocalBasePath = nh.advertise<autoware_msgs::Lane>("base_waypoints", 1,true);
pub_ClosestIndex = nh.advertise<std_msgs::Int32>("closest_waypoint", 1,true);
pub_BehaviorState = nh.advertise<geometry_msgs::TwistStamped>("current_behavior", 1);//作为op_trajectory_evaluator的订阅
pub_SimuBoxPose    = nh.advertise<geometry_msgs::PoseArray>("sim_box_pose_ego", 1);
pub_BehaviorStateRviz = nh.advertise<visualization_msgs::MarkerArray>("behavior_state", 1);
pub_SelectedPathRviz = nh.advertise<visualization_msgs::MarkerArray>("local_selected_trajectory_rviz", 1);

op_common_params

op_motion_predictor

1. 订阅的topic

sub_StepSignal = nh.subscribe("/pred_step_signal",1,&MotionPrediction::callbackGetStepForwardSignals,this);
sub_tracked_objects  = nh.subscribe("/tracked_objects",1,&MotionPrediction::callbackGetTrackedObjects,this);
sub_current_pose   = nh.subscribe("/current_pose", 10,&MotionPrediction::callbackGetCurrentPose,this);
//依然的三选一,默认值是current_velocity
"/odom";"/current_velocity";"/can_info"
//Mapping Section
//...这部分同上

2. 发布的topic

pub_predicted_objects_trajectories = nh.advertise<autoware_msgs::DetectedObjectArray>("/predicted_objects", 1);//作为op_trajectory_evaluator的订阅
pub_PredictedTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("/predicted_trajectories_rviz", 1);
pub_CurbsRviz          = nh.advertise<visualization_msgs::MarkerArray>("/map_curbs_rviz", 1);
pub_ParticlesRviz = nh.advertise<visualization_msgs::MarkerArray>("prediction_particles", 1);

3.参数读取

4. predict 方法

MainLoop中，首先根据不同的地图类型load地图，三种类型分别为

• MAP_KML_FILE
• MAP_FOLDER
• MAP_AUTOWARE
  主要的处理逻辑在callbackGetTrackedObjects()回调函数中，该函数订阅tracked_objectstopic的信息，应该是使用ConvertFromOpenPlannerDetectedObjectToAutowareDetectedObject函数做的预测，要继续看下

op_trajectory_evaluator

1. 订阅的topic

//老生常谈项
sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this);
//经典三选一,默认值是current_velocity
"/odom";"/current_velocity";"/can_info";
//其他的
sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this);//global_planner的输出
sub_LocalPlannerPaths = nh.subscribe("/local_trajectories", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this);//op_trajectory_generator的输出
sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this);//op_motion_predictor的输出
sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this);//op_behavior_selector的输出

2. 发布的topic

pub_CollisionPointsRviz = nh.advertise<visualization_msgs::MarkerArray>("dynamic_collision_points_rviz", 1);
pub_LocalWeightedTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("local_trajectories_eval_rviz", 1);
pub_LocalWeightedTrajectories = nh.advertise<autoware_msgs::LaneArray>("local_weighted_trajectories", 1);
pub_TrajectoryCost = nh.advertise<autoware_msgs::Lane>("local_trajectory_cost", 1);
pub_SafetyBorderRviz = nh.advertise<visualization_msgs::Marker>("safety_border", 1);

3. 基础参数读取

这个launch文件的基础参数比较少，只有三个

  <arg name="enablePrediction"       default="false" />                
  <arg name="horizontalSafetyDistance"   default="1.2" />
  <arg name="verticalSafetyDistance"     default="0.8" />

4. 评估轨迹的方法

WayPoint类中定义了多种cost属性，包括

double    cost;
double    timeCost;
double    collisionCost;
double     laneChangeCost;
std::vector<std::pair<ACTION_TYPE, double> > actionCost;

而TrajectoryCost类中也定义了多种类型的cost，包括

class TrajectoryCost
{
public:
  int index;
  int relative_index;
  double closest_obj_velocity;
  double distance_from_center; //-> 到中心线的距离
  double priority_cost; //0 to 1 ->到中心线的距离绝对值
  double transition_cost; // 0 to 1
  double closest_obj_cost; // 0 to 1
  double cost;
  double closest_obj_distance;

  int lane_index;
  double lane_change_cost;
  double lateral_cost;
  double longitudinal_cost;
  bool bBlocked;
  std::vector<std::pair<int, double> > lateral_costs;	
  
  ...

根据论文阅读中的5.2部分可知，evaluator要做的事就是：

我们使用额外的cost function来衡量每个trajectory，这个function计算三中常规的cost度量，priorty cost， collision cost和transition cost，选择cost最小的trajectory。

4.1 三种cost的计算

根据论文5.2.1-5.2.3部分的描述，可以看到，一共需要计算三种cost，结合代码来看，其中5.2.1的Center Cost 对应TrajectoryCost类中的priority_cost；5.2.2的Transition Cost对应TrajectoryCost类中的transition_cost；5.2.3中的Collision Cost则对应WayPoint类中的collisionCost，该值在BehaviorPrediction中被用来PredictCurrentTrajectory等。

4.2 MainLoop主要逻辑

1. 和generator中一样，先调用ExtractPartFromPointToDistanceDirectionFast()方法把路径点做一次提取。
2. 如果m_bUseMoveingObjectsPrediction参数为真，即开启检测移动障碍物，则调用DoOneStepDynamic()，否则调用DoOneStepDynamic()，只计算静态障碍物。两个方法都是用来计算cost，返回值为BestTrajectory。最后发布到local_trajectory_cost话题

if(m_bUseMoveingObjectsPrediction)//是否检测运动中的障碍物，还是只检测静态障碍物
          //返回值为TrajectoryCost类型，bestTrajectory
          tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams,  m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory);
        else
          tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,  m_PlanningParams,  m_CarInfo,m_VehicleStatus, m_PredictedObjects);

3. 调用ConvertFromLocalLaneToAutowareLane()方法，获取local_lanes，发布到话题local_weighted_trajectories

4.3 DoOneStepStatic

TrajectoryCost TrajectoryDynamicCosts::DoOneStepStatic(const vector<vector<WayPoint> >& rollOuts,
    const vector<WayPoint>& totalPaths, const WayPoint& currState,
    const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
    const std::vector<PlannerHNS::DetectedObject>& obj_list, const int& iCurrentIndex)
{
  TrajectoryCost bestTrajectory;
  bestTrajectory.bBlocked = true;
  bestTrajectory.closest_obj_distance = params.horizonDistance;
  bestTrajectory.closest_obj_velocity = 0;
  bestTrajectory.index = -1;

  RelativeInfo obj_info;
  PlanningHelpers::GetRelativeInfo(totalPaths, currState, obj_info);
  int currIndex = params.rollOutNumber/2 + floor(obj_info.perp_distance/params.rollOutDensity);//为啥要这样算
  //std::cout <<  "Current Index: " << currIndex << std::endl;
  if(currIndex < 0)
    currIndex = 0;
  else if(currIndex > params.rollOutNumber)
    currIndex = params.rollOutNumber;//currIndex不能大于rollOutNumber

  m_TrajectoryCosts.clear();
  if(rollOuts.size()>0)
  {
    TrajectoryCost tc;
    int centralIndex = params.rollOutNumber/2;//中间路线就是总rollOutNumber的一半
    tc.lane_index = 0;
    for(unsigned int it=0; it< rollOuts.size(); it++)//rollOuts中的每一条trajectory，都创建一个tc
    {
      tc.index = it;
      tc.relative_index = it - centralIndex;
      tc.distance_from_center = params.rollOutDensity*tc.relative_index;
      tc.priority_cost = fabs(tc.distance_from_center);
      tc.closest_obj_distance = params.horizonDistance;
      if(rollOuts.at(it).size() > 0)
          tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost;//在这条rollOut轨迹第一个点处的laneChangeCost
      m_TrajectoryCosts.push_back(tc);
    }
  }

  CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);

  WayPoint p;
  m_AllContourPoints.clear();//头文件中定义了该变量
  for(unsigned int io=0; io<obj_list.size(); io++)
  {
    for(unsigned int icon=0; icon < obj_list.at(io).contour.size(); icon++)
    {
      p.pos = obj_list.at(io).contour.at(icon);//contour是GPSPoint类型
      p.v = obj_list.at(io).center.v;//中心点的速度
      p.id = io;
      p.cost = sqrt(obj_list.at(io).w*obj_list.at(io).w + obj_list.at(io).l*obj_list.at(io).l);//第io个障碍物的宽度长度的对角线，就是cost
      m_AllContourPoints.push_back(p);
    }
  }

  //计算横向和纵向的静态cost
  CalculateLateralAndLongitudinalCostsStatic(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);

  NormalizeCosts(m_TrajectoryCosts);

  int smallestIndex = -1;
  double smallestCost = DBL_MAX;
  double smallestDistance = DBL_MAX;
  double velo_of_next = 0;

  //cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
  for(unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
  {
    //cout << m_TrajectoryCosts.at(ic).ToString();
    if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
    {
      smallestCost = m_TrajectoryCosts.at(ic).cost;
      smallestIndex = ic;
    }

    if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
    {
      smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
      velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
    }
  }
  //cout << "Smallest Distance: " <<  smallestDistance << "------------------------------------------------------------- " << endl;

  if(smallestIndex == -1)
  {
    bestTrajectory.bBlocked = true;
    bestTrajectory.lane_index = 0;
    bestTrajectory.index = m_PrevCostIndex;
    bestTrajectory.closest_obj_distance = smallestDistance;
    bestTrajectory.closest_obj_velocity = velo_of_next;
  }
  else if(smallestIndex >= 0)
  {
    bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
  }

  m_PrevIndex = currIndex;
  return bestTrajectory;
}

代码调试

在对包中cpp文件的内容或其他内容进行更改后，需要重新编译，可以选择重新编译整个项目，如：

//release mode
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
//debug mode
colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
//With CUDA support
AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release

如果只更改了一个包，可以只编译对应的包

colcon build --packages-select <name-of-pkg>
//or
$ AUTOWARE_COMPILE_WITH_CUDA=0 colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo --packages-select <name-of-pkg>

注意：编译时必须在Autoware目录下，否则会出现一些错误，如：

ImportError: No module named catkin.environment_cache