现在网上看到的对于base_local_planner的解读很多都是概念性的，或者直接翻译自英文wiki。每当打开源码的时候看到这么多类和方法真的让人头大。

现在因为工作需要对base_local_planner做自己的修改，正好也没有人做过完整的源码解读的工作，我就决定试试看。这个包比之前的robot_post_ekf大太多，不太可能每个参数声明的时候都去做解释，所以只会在程序涉及到的时候才会说明。对应的文件会用文件表示，对应的函数用粗体表示，对应的变量用斜体表示。

因为本人水平有限，在解读的过程中难免会有疏漏，欢迎在评论区交流或者直接邮箱联系我：zhxue146@foxmail.com。

---

base_local_planner源码解读

构造函数与初始化

速度与路径计算：setPlan

总结与梳理

---

绪论

首先明确，navigation中的全局路径规划、局部路径规划以及恢复行为均为插件形式，这里有我写的ROS内导航插件机制的短文，最好先看一看。

navigation中的相关package

主要涉及其中三个包，其中含义如下(转载博客链接)：

1. nav_core:

该包定义了整个导航系统关键包的接口函数，包括base_global_planner, base_local_planner以及recovery_behavior的接口。里面的函数全是虚函数，所以该包只是起到规范接口的作用，真正功能的实现在相应的包当中。之后添加自己编写的全局或局部导航算法也需要在这里声明

2. move_base

这个是整个navigation stack当中进行宏观调控的看得见的手。它主要干的事情是这样的：
　维护一张全局地图（基本上是不会更新的，一般是静态costmap类型），维护一张局部地图（实时更新，costmap类型），维护一个全局路径规划器global_planner完成全局路径规划的任务， 维护一个局部路径规划器base_local_planner完成局部路径规划的任务。
　然后提供一个对外的服务，负责监听nav_msgs::goal类型的消息，然后调动全局规划器规划全局路径，再将全局路径送进局部规划器，局部规划器结合周围障碍信息（从其维护的costmap中查询），全局路径信息，目标点信息采样速度并评分获得最高得分的轨迹（即是采样的最佳速度），然后返回速度值，由move_base发送Twist类型的cmd_vel消息上，从而控制机器人移动，完成导航任务。

3. base_local_planner

完成局部窗口内的路径规划任务，机器人行动速度的具体生成在此包当中完成。目前有两种局部路径规划算法实现，一是航迹推算法（TrajectoryROS），一是动态窗口法(DWA)，该包内部的默认实现是航迹推算法，但是留出了DWA的定义接口，DWA的实现在dwa_local_planner中。

用简单的话来说，move_base中会对local_planner进行声明并且调用nav_core中定义的接口函数，至于怎么具体实现，本文的情况中使用就是自带的base_local_planner。

函数调用顺序：从move_base开始

既然对于base_local_planner的声明与调用均在move_base中，那么首先进入mova_base包中去一探究竟。move_base采用actionlib机制，最好先去了解一下。

move_base_node.cpp是对于move_base.cpp的ros包装，所以直接进入move_base.cpp。关于整个move_base内部运行的顺序，请看这里，这里摘录如下：

从构造函数的actionServer 和线程函数planThread(等待executeCb通知他工作，规划一下路径，这个是global plan)

 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base",   boost::bind(&MoveBase::executeCb, this, _1), false);planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
 

每次有goal发布的时候，都会去调用executeCb,executeCb会去调用executeCycle
进行local plan,发布cmd_vel的topic，根据小车处于的状态，进行相应的实现(会进行plan,control,clear obstacle)

因为本文讨论的是local_planner，会把精力集中在move_base对local_planner的调用上。在move_base.cpp的构造函数中129-132行声明local_planner、初始化并设定全局cost_map：

      tc_ = blp_loader_.createInstance(local_planner);ROS_INFO("Created local_planner %s", local_planner.c_str());tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);tc_->setGlobalCostmap(planner_costmap_ros_);
 

reconfigureCB中的tc_是用来动态修改参数的，相当于重新声明local_planner，与构造函数中形式一样。因此重点在executeCycle上，其中tc_分别出现了三次，如下：

    //第821行，较为简单，单纯的清除上一次全局规划并且设定为新的全局规划if(!tc_->setPlan(*controller_plan_)){//第856行，简单，返回是否到达终点的flagif(tc_->isGoalReached()){//第882行，计算计划和速度的重点if(tc_->computeVelocityCommands(cmd_vel)){
 

现在，整个move_base中对local_planner的调用顺序已经清晰，让我们回到正题，看看base_local_planner是怎么实现的。

---

构造函数与初始化

首先先看看英文wiki中如何使用base_local_planner：

#include 
 #include 
 #include ...tf::TransformListener tf(ros::Duration(10));
 costmap_2d::Costmap2DROS costmap("my_costmap", tf);base_local_planner::TrajectoryPlannerROS tp;
 

首先生成在trajectory_planner_ros.cpp中的base_local_planner::TrajectoryPlannerROS实例tp，base_local_planner::TrajectoryPlannerROS是对base_local_planner::TrajectoryPlanner的ROS包装。

在生成实例的过程中声明一些变量，最重要的是参数如下：

      WorldModel* world_model_; //控制器将会使用的世界模型TrajectoryPlanner* tc_; //轨迹控制器本体，同时也声明了许多许多个将要用的参数，简直没法展开说，要用的时候再说吧
 

轨迹控制器本体在trajectory_planner.cpp中，这里真正的涉及到了轨迹的生成与计算，以后我们会再见到它。

tp.initialize("my_trajectory_planner", &tf, &costmap);
 

我们可以对比move_base.cpp129-132行：

      tc_ = blp_loader_.createInstance(local_planner);ROS_INFO("Created local_planner %s", local_planner.c_str());tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);//多了这一句，在nav_core中定义这个虚函数，然而base_local_planner并没有重载，所以这里并没有意义。tc_->setGlobalCostmap(planner_costmap_ros_);
 

从initialize开始真正的base_local_planner，即trajectory_planner_ros.cpp第84-245行。首先将会是很多很多的参数读取以及纠正不科学的参数。在读取这些参数以后，我们终于开始正式生成轨迹和评价。

在第230行正式对base_local_planner进行声明：

      tc_ = new TrajectoryPlanner(*world_model_, *costmap_, footprint_spec_,acc_lim_x_, acc_lim_y_, acc_lim_theta_, sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta, holonomic_robot,max_vel_x, min_vel_x, max_vel_th_, min_vel_th_, min_in_place_vel_th_, backup_vel,dwa, heading_scoring, heading_scoring_timestep, meter_scoring, simple_attractor, y_vels, stop_time_buffer, sim_period_, angular_sim_granularity);
 

其构造函数在trajectory_planner_ros.cpp第141-172行，将许多flag定义为false，之后是187行：

    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, inscribed_radius_, circumscribed_radius_);
 

计算了costmap中需要使用的内接圆与外切圆半径。至此，构造函数与初始化结束。

---

速度与路径计算：setPlan

setPlan在trajectory_planner_ros.cpp中的第372-第525行，在这里将会计算机器人实际运行的速度，发布局部路径规划，可以说是base_local_planner的核心，接下来我会放慢点节奏，好好咀嚼这里。

首先将机器人的姿态以及全局路径规划转换到global_frame_上。

    if (!costmap_ros_->getRobotPose(global_pose)) {return false;}std::vector transformed_plan;//get the global plan in our frameif (!transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan)) {ROS_WARN("Could not transform the global plan to the frame of the controller");return false;}
 

第395-419行，得到机器人当前速度以及终点的(x,y, θ theta θ)。第422-473行，查看机器人是否到达终点，如果到达了，就让机器人停下来。第475-478行：

    //将全局路径拷贝进来，并认为全军路径的最后一个点就是终点。tc_->updatePlan(transformed_plan);//计算应该跟随什么轨迹，给定当前机器人位置和朝向，计算机器人应该跟随的好轨迹。Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
 

findBestPath在trajectory_planner.cpp中第906至第977行，终于我们开始正式计算轨迹以及速度分量，将函数摘抄如下：

  //计算应该跟随什么轨迹，给定当前机器人位置和朝向，计算机器人应该跟随的好轨迹。Trajectory TrajectoryPlanner::findBestPath(tf::Stamped global_pose, tf::Stamped global_vel,tf::Stamped& drive_velocities){//将当前机器人位置和方向转变成float形式的向量Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));//重置地图path_map_.resetPathDist();goal_map_.resetPathDist();//暂时移走机器人footprint上的障碍物std::vector footprint_list =footprint_helper_.getFootprintCells(pos,footprint_spec_,costmap_,true);//将初始footprint上的所有cell标记for (unsigned int i = 0; i < footprint_list.size(); ++i) {path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;}//确保我们根据全局计划更新map并且计算代价path_map_.setTargetCells(costmap_, global_plan_);goal_map_.setLocalGoal(costmap_, global_plan_);ROS_DEBUG("Path/Goal distance computed");//找到代价最低的轨迹。输入分别是目前机器人位置，速度以及加速度限制，生成诸多可能轨迹，选取其中打分最高的。这里也就是最关键的一步。Trajectory best = createTrajectories(pos[0], pos[1], pos[2],vel[0], vel[1], vel[2],acc_lim_x_, acc_lim_y_, acc_lim_theta_);ROS_DEBUG("Trajectories created");/*//If we want to print a ppm file to draw goal distchar buf[4096];sprintf(buf, "base_local_planner.ppm");FILE *fp = fopen(buf, "w");if(fp){fprintf(fp, "P3n");fprintf(fp, "%d %dn", map_.size_x_, map_.size_y_);fprintf(fp, "255n");for(int j = map_.size_y_ - 1; j >= 0; --j){for(unsigned int i = 0; i < map_.size_x_; ++i){int g_dist = 255 - int(map_(i, j).goal_dist);int p_dist = 255 - int(map_(i, j).path_dist);if(g_dist < 0)g_dist = 0;if(p_dist < 0)p_dist = 0;fprintf(fp, "%d 0 %d ", g_dist, 0);}fprintf(fp, "n");}fclose(fp);}*/// 如果最后打分如果小于0，说明所有的路径都不可用if(best.cost_ < 0){drive_velocities.setIdentity();}else{tf::Vector3 start(best.xv_, best.yv_, 0);drive_velocities.setOrigin(start);tf::Matrix3x3 matrix;matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));drive_velocities.setBasis(matrix);}return best;}
 

轨迹返回：createTrajectories

createTrajectories是整个dwa_local_controller以及base_local_controller的关键，它在trajectory_planner_ros第535-第902行。

首先第539-第560行，读取数据，设定这一次仿真中可以到达的最大线速度和角速度。

第563-第569行，首先根据vx速度以及 θ theta θ的采样数，分别计算出每一次线速度和角速度的变化值dvx和dvtheta。并把最小的vx、vy、v θ theta θ作为初始采样。

第584-第645行，如果机器人没有在逃离，首先假设机器人会向前，那么按照按照顺序对所有采样到的vx，对每一个vx都去尝试v θ theta θ使用generateTrajectory生成轨迹并打分，保留分数最高的轨迹。如果是柔性机器人那么还有vy方向的采样和打分。第655-第697行，机器人不向前只旋转的情况下，对这些轨迹进行打分。保留下分数最高的轨迹。

第700-第846行，检查最优轨迹的分数是否大于0，也就是正常。如果正常的话，那么为了抑制震荡影响，当机器人在某方向移动时，对下一个周期的与其相反方向标记为无效，直到机器人从标记震荡的位置处离开一定距离，（声明与定义在trajectory_planner.h中第271-第275行），返回最佳轨迹。

最后，当最优轨迹不可用时，就让机器人缓慢退后。

轨迹生成与打分：generateTrajectory

现在来看看怎么具体生成轨迹并打分的，该函数定义在trajectory_planner.h中第247-249行，连带说明如下：

      //double vx_samp, double vy_samp, double vtheta_samp//分别为这次采样中使用的vx, yv, vtheta，生成的轨迹以引用的形式传回。void generateTrajectory(double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y,double acc_theta, double impossible_cost, Trajectory& traj);
 

这一部分可以说是base_local_planner的核心，所以我全部拷贝过来，慢慢做解释：

  void TrajectoryPlanner::generateTrajectory(double x, double y, double theta,double vx, double vy, double vtheta,double vx_samp, double vy_samp, double vtheta_samp,double acc_x, double acc_y, double acc_theta,double impossible_cost,Trajectory& traj) {// 确保运行一半的时候参数不会改变boost::mutex::scoped_lock l(configuration_mutex_);//记录初始时刻的x、y、theta、vx、vy、vthetadouble x_i = x;double y_i = y;double theta_i = theta;double vx_i, vy_i, vtheta_i;vx_i = vx;vy_i = vy;vtheta_i = vtheta;//计算速度的模double vmag = hypot(vx_samp, vy_samp);//计算仿真的步数int num_steps;if(!heading_scoring_) {num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);} else {num_steps = int(sim_time_ / sim_granularity_ + 0.5);}//我们希望至少有一步，即使一步都没有，我们也希望能为当前位置打分if(num_steps == 0) {num_steps = 1;}double dt = sim_time_ / num_steps;double time = 0.0;//创造一个潜在的轨迹traj.resetPoints();traj.xv_ = vx_samp;traj.yv_ = vy_samp;traj.thetav_ = vtheta_samp;traj.cost_ = -1.0;//初始化这个轨迹的costdouble path_dist = 0.0;double goal_dist = 0.0;double occ_cost = 0.0;double heading_diff = 0.0;for(int i = 0; i < num_steps; ++i){unsigned int cell_x, cell_y;//不希望路径跑出已知地图if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){traj.cost_ = -1.0;return;}//检查当前点路径的合法性double footprint_cost = footprintCost(x_i, y_i, theta_i);//如果遇到了障碍物，那么会返回一个负数if(footprint_cost < 0){traj.cost_ = -1.0;return;//更新occ_costocc_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));//如果只是想简简单单的跟随终点的话，这里调整为true即可if (simple_attractor_) {goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) *(x_i - global_plan_[global_plan_.size() -1].pose.position.x) +(y_i - global_plan_[global_plan_.size() -1].pose.position.y) *(y_i - global_plan_[global_plan_.size() -1].pose.position.y);} else {bool update_path_and_goal_distances = true;// 是否为朝向打分？if (heading_scoring_) {if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt) {heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i);} else {update_path_and_goal_distances = false;}}if (update_path_and_goal_distances) {//更新路径与目标的距离path_dist = path_map_(cell_x, cell_y).target_dist;goal_dist = goal_map_(cell_x, cell_y).target_dist;//如果一个路径上的点没法明确到达在终点，他就是无效的if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
 //            ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
 //                goal_dist, path_dist, impossible_cost);traj.cost_ = -2.0;return;}}}//这个点有效，加入轨迹traj.addPoint(x_i, y_i, theta_i);//计算速度vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);//计算位置x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);//增加时间time += dt;} double cost = -1.0;//更新costif (!heading_scoring_) {cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;} else {cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;}traj.cost_ = cost;}
 

总结与梳理

至此，整个计算过程结束。接下来会用流程图明确到底如何计算出速度的：