本篇主要对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的程序，且对相关的原理有一定的理解。建议结合论文一起看。
然后再阅读本文，如果只想看源码结构，也可以不看前面的。

包结构

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

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。

op_global_planner_core.cpp中代码的主要逻辑

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

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

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

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

生成的路径存储在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;

op_local_planner

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

op_trajectory_generator

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

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"

2. 生成rollOuts的方法

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_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);

3. 发布的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);

op_behavior_selector

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

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
//...这部分同上

3. 发布的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. 业务逻辑
3. 发布的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);

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. 业务逻辑
3. 发布的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);