base_local_planner源码解读

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

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

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

---

---

绪论

首先明确，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行，简单，返回是否到达终点的flag
    if(tc_->isGoalReached()){
      
    //第882行，计算计划和速度的重点
    if(tc_->computeVelocityCommands(cmd_vel)){

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

---

构造函数与初始化

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

#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/trajectory_planner_ros.h>

...

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<geometry_msgs::PoseStamped> transformed_plan;
    //get the global plan in our frame
    if (!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<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
      tf::Stamped<tf::Pose>& 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<base_local_planner::Position2DInt> 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 dist
    char buf[4096];
    sprintf(buf, "base_local_planner.ppm");
    FILE *fp = fopen(buf, "w");
    if(fp){
      fprintf(fp, "P3\n");
      fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
      fprintf(fp, "255\n");
      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、vtheta
    double 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;

    //初始化这个轨迹的cost
    double 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_cost
      occ_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;

	//更新cost
    if (!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;
  }

总结与梳理

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