Navigation源码阅读之base_local_planner

base_local_planner是直接决策小车以何种轨迹运行、下发何种速度的大类，可能由于开发的历史原因，下面的文件全部放在这个包里，显得有些杂乱，咱们慢慢捋一捋。

首先看看navigation包的nav_core包，nav_core是一个纯虚类，里面对base_local_planner进行了父类的虚函数声明，它使用默认构造函数，initialize函数接受名字、tf、代价地图进行“构造”，另外三个接口分别是setPlan（用来将全局路径传到局部规划器），computeVelocityCommands（计算下发速度），以及isGoalReached（判断本次导航是否结束）。这就是base_local_planner虚类的声明。那么来继承这个类的，默认是DWAPlannerROS和TrajectoryPlannerROS以pluginlib的形式继承，而我们一般使用ＤＷＡ算法进行速度计算而不是。dwa_local_planner已经作为一个独立的包放在外面，这一块就不再赘述。

我们从ＤＷＡ算法开始，依次看看调用的base_local_planner的函数流程：

一、从DWAPlannerROS::computeVelocityCommands开始：

１．调用了getLocalPlan函数，这个函数是LocalPlannerUtil::getLocalPlan，这个类的作用大概是对发过来的全局路径进行tf上的转化以及裁剪，它依次调用的两个函数均位于goal_functions.cpp中，这个文件下全是工具函数，不从属于类。下列均为工具函数：

（１）publishPlan这个函数是将局部路径发给rviz，我们在rviz中可以同时观看两个nav_msgs/Path格式的topic（全局、局部）。

（２）prunePlan将全局路径裁剪成前方一小段局部路径，采用的方法是用迭代器依次判断与当前位置的距离，小于某距离时跳出迭代器循环，将路径其他部分剔除。这个方法非常暴力，默认的前方距离是１m，这个数字不是可配置的rosparam，让人费解。

（３）transformGlobalPlan是利用计算局部代价地图的范围，将范围之外的剔除，在局部代价地图内的路径点的frame转化为global_frame。

double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
                                       costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);

（４）getGoalPose是将全局路径的最后一个元素取出，利用tf中的poseStampedMsgToTF获取goal_pose的各参数，tf的具体操作方法我们之后再分析。

（５）isGoalReached是判断两个因素，一个是位置是否足够接近终点（容忍度之内），一个是车是否停下（按照里程计反馈）。

２．回到computeVelocityCommands，它接着调用了DWAPlanner::updatePlanAndLocalCosts函数，而这个函数是依靠那几个打分项活着的，检查一下就可以发现该函数中出现的打分项有：path_costs_，goal_costs_，goal_front_costs_，alignment_costs_，一直在围绕这些打分项做文章。这几个打分项，其实都属于base_local_planner::MapGridCostFunction类。转到map_grid_cost_function.cpp，这里详细讲述了打分前的准备工作。

（１）在updatePlanAndLocalCosts函数中调用的setTargetPoses接口其实就是将外部处理好的局部路径传到成员变量而已，其他什么都不做。

（２）其他成员函数都调用了MapGrid的对象，先转到map_grid.cpp看看它的实现方式：

①MapGrid反复使用了装有MapCell的vector，再转到map_cell.cpp（这调用层级有点厚）～

private:

  std::vector<MapCell> map_; ///< @brief Storage for the MapCells

②MapCell这个类是真的皮，默认构造函数和复制构造函数啥都不做，但是初始化列表里说明了地图体素距小车无限远，暂时先这么初始化吧。

MapCell::MapCell()
    : cx(0), cy(0),
      target_dist(DBL_MAX),
      target_mark(false),
      within_robot(false)
  {}

③回到MapGrid类，commonInit函数是将整个栅格地图初始化（其实就是将每个体素塞进vector都标好位置），而getIndex函数就正好是找到vector的下标。sizeCheck函数是检查栅格地图是否填充正确，不对就再来一遍。

④MapGrid::updatePathCell是为了检查小车前方的check_cell和目前位于的current_cell的距离，如果检查出这个cell是障碍物那么距离设为障碍物的类型。

unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
   (cost == costmap_2d::LETHAL_OBSTACLE ||
    cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
    cost == costmap_2d::NO_INFORMATION))
{
  check_cell->target_dist = obstacleCosts();
  return false;
}

⑤MapGrid::adjustPlanResolution是为了根据地图以及代价地图的分辨率对全局路径进行填充，防止路径点过于稀疏。虽然函数看起来很花哨，但是仔细看就很容易发现意图。

⑥MapGrid::setTargetCells是为了将global_plan经过的cells存储队列path_dist_queue中，这个队列每当经过一个体素（队头）就删除之，把新的体素塞在队尾。MapGrid::setLocalGoal函数也类似，用来判断小车下一时刻将通过哪个体素。

⑦MapGrid::computeTargetDistance函数是这个类的关键，它把局部地图里的所有体素均和一个队列中的元素（其实是路径点）进行距离计算，但是没有返回任何对象，有点费解。。。

（３）回到map_grid_cost_function.cpp，很多函数就容易理解了。MapGridCostFunction::scoreTrajectory是为了判断路径点前方的体素的评分，如果有错误返回负数。一个需要注意的点是，aggregationType_是一个枚举，它分别代表返回最后一点的值（如果没有处于碰撞危险中）、所有点的和，或者所有非零点的乘积。

３．那么再回到DWAPlannerROS::computeVelocityCommands，接下来调用了LatchedStopRotateController的对象，转到latched_stop_rotate_controller.cpp分析它的实现：

（１）LatchedStopRotateController::stopWithAccLimits以及rotateToGoal中，加速度的上下限是可配置参数，它们是在一个模拟时间周期内计算出所需的线速度与角速度。

（２）至于具体实现的computeVelocityCommandsStopRotate，是通过里程计读数的类对象odom_helper_的帮助来判断小车是否停止旋转。

①转到odom_helper_ros.cpp，可以看到它是很简单的通过接收base_controller的里程计读数，值得注意的是消息队列只有一位，这意味着收一个就扔一个～

odom_sub_ = gn.subscribe<nav_msgs::Odometry>( odom_topic_, 1, boost::bind( &OdometryHelperRos::odomCallback, this, _1 ));

二、接着从computeVelocityCommands调用DWAPlannerROS::dwaComputeVelocityCommands，再接着调用DWAPlanner::findBestPath：

（１）obstacle_costs_是ObstacleCostFunction的对象，进入obstacle_cost_function.cpp，发现它的构造函数上来就要构建一个costmapModel，不明觉厉啊，赶紧转到costmap_model.cpp：

①在头文件中，我们可以看到footprintCost函数，看过costmap层级定义的朋友明白这些形参其实就是指小车位置、车轮数（圆形或方形）、内切圆半径、外接圆半径。

virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
          double inscribed_radius, double circumscribed_radius);

 而这个函数作用是判断小车模型（圆或者方）最大可能的地图代价。

②CostmapModel::lineCost是用一条线来检查小车轮廓边缘是否侵犯了代价地图。

（２）ObstacleCostFunction::scoreTrajectory和前面的MapGrid类似，都是错误返回负数，并且正确情况分为最后一点的值与所有点的和。getScalingFactor是为了限制小车速度太大。

（３）回到DWAPlanner::findBestPath，设置好obstacle_costs之后，用一个SimpleTrajectoryGenerator初始化计算函数，转到simple_trajectory_generator.cpp：

①SimpleTrajectoryGenerator::initialise其实就是借助可配置参数，设置一个可选速度区间，用std::vector<Eigen::Vector3f>储存。

②SimpleTrajectoryGenerator::generateTrajectory创造了一个时间微分，通过位置与速度计算函数，得到新的轨迹。这时候看一下trajectory.cpp，发现base_local_planner::Trajectory其实就是由三个vector组成，分别存储X，Y和点之间的夹角。

③SimpleTrajectoryGenerator::computeNewPositions和computeNewVelocities函数均是通过速度区间求解下一个速度与位置。

（４）回到DWAPlanner::findBestPath，接着是SimpleScoredSamplingPlanner类对象scored_sampling_planner_，转到simple_scored_sampling_planner.cpp，findBestTrajectory是对外调用的接口，它通过遍历all_explored这个存储Trajectory的vector来寻找最优解，那么评分的标准在SimpleScoredSamplingPlanner::scoreTrajectory中：

①scoreTrajectory在轨迹上运行所有评分函数，创建正成本的加权和，一旦发现负成本或负成本之和大于正的._traj_cost累积值，就中止。

三、运行到这里，结果就很明确了，由于result_traj_是由三个vector组成，那分别取其中的xv_，yv_，thetav_，就可以得到本次周期所需要的速度了。当然用一个tf::Pose来返回这个cmd_vel的数值，也是很费解了，明明可以用一个matrix更便于理解，当然可能稳定性稍差。

这就是move_base——dwa_local_planner——base_local_planner的流程，用多层级、分布式的软件架构实现了导航算法～