findBestPath之base_local_planner

参考文章：[无处不在的小土]base_local_planner^[1] 

计算速度分三个步骤，括号内是该步骤主要函数。

1. (SimpleTrajectoryGenerator::initialise)枚举出所有候选速度。
2. (SimpleTrajectoryGenerator::generateTrajectory)让机器人针对每个候选速度模拟运行，每次运行的结果存放在一个Trajectory类型的变量，这变量称为轨迹。N个候选速度生成了N个Trajectory。
3. (SimpleScoredSamplingPlanner::scoreTrajectory)计算每个轨迹得分，得分最高的就是要返回给上层的速度。

一、SimpleTrajectoryGenerator::initialise

功能是枚举出所有候选速度。因为只处理2D，速度有三个分量，线速度中的x、y，角速度中的theta。三个分量互相独立，根据数学上排列公式，候选速度数=x分量候选数 * y分量候选数 * theta分量候选数。要计算一个分量的候选数，只须要三个参数：最小速度min、最大速度max、样本数num_samples。

class VelocityIterator {
      VelocityIterator(double min, double max, int num_samples):
        current_index(0)
      {
        if (min == max) {
          samples_.push_back(min);
        } else {
          num_samples = std::max(2, num_samples);

          // e.g. for 4 samples, split distance in 3 even parts
          double step_size = (max - min) / double(std::max(1, (num_samples - 1)));

          // we make sure to avoid rounding errors around min and max.
          double current;
          double next = min;
          for (int j = 0; j < num_samples - 1; ++j) {
            current = next;
            next += step_size;
            samples_.push_back(current);
            // if 0 is among samples, this is never true. Else it inserts a 0 between the positive and negative samples
            if ((current < 0) && (next > 0)) {
              // 如果候选值中有负到正的变化，而且没有0，强制把0加入候选。
              samples_.push_back(0.0);
            }
          }
          samples_.push_back(max);
        }
      }
}

经过上面计算，samples_就存储着num_samples个候选速度，这些速度平均分布在[min, max]区间。但有个例外，如果候选值中有负到正的变化，而且没有0，强制把0加入候选，这时samples_中就增加到num_samples + 1。速度是0时，意味着机器人在那分量上是静止，静止可能会是个常用值。接下让看会调用VelocityIterator的SimpleTrajectoryGenerator::initialise。

void SimpleTrajectoryGenerator::initialise(
    const Eigen::Vector3f& pos,
    const Eigen::Vector3f& vel,
    const Eigen::Vector3f& goal,
    base_local_planner::LocalPlannerLimits* limits,
    const Eigen::Vector3f& vsamples,
    bool discretize_by_time) {

initialise有6个参数，其中pos, vel和goal是三个输入参数，分别记录了机器人的当前位姿(odom坐标系)、当前速度和目标点。limits记录机器人的各种运动限制，vsamples则记录了在x、y方向和theta的num_samples，discretize_by_time则用于配置采样器是否需要在时间上进行离散化处理。

  /*
   * We actually generate all velocity sample vectors here, from which to generate trajectories later on
   */
  double max_vel_th = limits->max_vel_theta;
  double min_vel_th = -1.0 * max_vel_th;
  discretize_by_time_ = discretize_by_time;
  Eigen::Vector3f acc_lim = limits->getAccLimits();
  pos_ = pos;
  vel_ = vel;
  limits_ = limits;
  next_sample_index_ = 0;
  sample_params_.clear();
  double min_vel_x = limits->min_vel_x;
  double max_vel_x = limits->max_vel_x;
  double min_vel_y = limits->min_vel_y;
  double max_vel_y = limits->max_vel_y;

  // if sampling number is zero in any dimension, we don't generate samples generically
  if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {

根据样本数量的限制，只要有一个维度的样本数量为0就不枚举候选速度。vsamples是个配置参数，DWAPlanner::reconfigure取出时会确保3个最小值是1。

    //compute the feasible velocity space based on the rate at which we run
    Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
    Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();

    if ( ! use_dwa_) {

      // 省略use_dwa_为false时的分支，它与我们的DWA算法无关。

    } else {
      // with dwa do not accelerate beyond the first step, we only sample within velocities we reach in sim_period
      max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
      max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
      max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);

      min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
      min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
      min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);

以计算max_vel[0]为例，分析当中逻辑。假设当前速度是vel[0]，加速度acc_lim[0]是1.25，模拟运行时间sim_period_是0.2秒，按这加速度，0.2秒后机器人最快也只能到vel[0]+0.25，如果这值小于max_vel_x，应该取vel[0]+0.25。取最大值vel[0]+0.25不会减少候选速度数目，但满足了加速度不能超过acc_lim[0]这个限制。所以以上6条语句的作用：根据机器人当前的速度和加速度限制，计算机器人的速度区间。

    }

    Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
    VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
    VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
    VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
    for(; !x_it.isFinished(); x_it++) {
      vel_samp[0] = x_it.getVelocity();
      for(; !y_it.isFinished(); y_it++) {
        vel_samp[1] = y_it.getVelocity();
        for(; !th_it.isFinished(); th_it++) {
          vel_samp[2] = th_it.getVelocity();
          sample_params_.push_back(vel_samp);
        }
        th_it.reset();
      }
      y_it.reset();
    }
  }
}

根据三参数min、max、num_samples，枚举出所有候选速度，生成的速度保存在sample_params_。下面一个实例。

vsamples{20, 1, 20}、min_vel{-0.025, 0, -1}、max_vel{0.25, 0, 1}
==> sample_params_.size(): 21 * 1 * 21 = 441
------
sample_params_[0] -0.025000, 0, -1.000000
sample_params_[1] -0.025000, 0, -0.894736
sample_params_[2] -0.025000, 0, -0.789473
sample_params_[3] -0.025000, 0, -0.684210
sample_params_[4] -0.025000, 0, -0.578947
sample_params_[5] -0.025000, 0, -0.473684
sample_params_[6] -0.025000, 0, -0.368421
sample_params_[7] -0.025000, 0, -0.263157
sample_params_[8] -0.025000, 0, -0.157894
sample_params_[9] -0.025000, 0, -0.052631
sample_params_[10] -0.025000, 0, 0
sample_params_[11] -0.025000, 0, 0.052631
sample_params_[12] -0.025000, 0, 0.157894
sample_params_[13] -0.025000, 0, 0.263157
sample_params_[14] -0.025000, 0, 0.368421
sample_params_[15] -0.025000, 0, 0.473684
sample_params_[16] -0.025000, 0, 0.578947
sample_params_[17] -0.025000, 0, 0.684210
sample_params_[18] -0.025000, 0, 0.789473
sample_params_[19] -0.025000, 0, 0.894736
sample_params_[20] -0.025000, 0, 1.000000
sample_params_[21] -0.010526, 0, -1.000000
......
sample_params_[436] 0.249999, 0, 0.578947
sample_params_[437] 0.249999, 0, 0.684210
sample_params_[438] 0.249999, 0, 0.789473
sample_params_[439] 0.249999, 0, 0.894736
sample_params_[440] 0.249999, 0, 1.000000

以下是和计算候选速度相关的配置变量。

注1。除非像万向轮，很多底盘不能做到转弯时只改y不改x，因此涉及到y的就设置为0。相关参数：vy_samples、min_vel_y、max_vel_y、acc_lim_y。

二、SimpleTrajectoryGenerator::generateTrajectory

让机器人针对某个候选速度模拟运行，运行的结果存放在一个Trajectory类型的变量，这变量称为轨迹。它的基本思路：给定一个时间段sim_time_(1.7秒)，让机器人按着候选速度运行1.7秒，这时会形成一条路径，在路径中等间隔取num_steps个坐标点，就可由候选速度和num_steps个坐标点组成轨迹。

图1显示了一次generateTrajectory运行实例。对生成的traj，xv_、yv_、thetav_是候选速度。time_delta_是时间粒度，它和num_steps满足“time_delta_ = sim_time_ / num_steps”。x_pts_、y_pts_、th_pts_是按候选速度运行、运行路径中等间隔取的num_steps个坐标点，是odom坐标系下的坐标点，不是针对初始位姿的偏移。它们的size()相等，且都等于num_steps。(x_pts_[0], y_pts_[0], th_pts_[0])值是参数pos，即机器人当前位姿，back则是sim_time_时长后估计到达的坐标。当中只有cost_没被赋上最终值，只给了初始值-1。

计算中使用的时长变量是sim_time_，而不是SimpleTrajectoryGenerator::initialise中的sim_period_。

bool SimpleTrajectoryGenerator::generateTrajectory(
      Eigen::Vector3f pos,
      Eigen::Vector3f vel,
      Eigen::Vector3f sample_target_vel,
      base_local_planner::Trajectory& traj) {

generateTrajectory有四个参数，其中pos和vel分别是机器人目前的位姿和速度，sample_target_vel是某个候选速度，而traj是一个输出参数，将记录生成的轨迹

  double vmag = hypot(sample_target_vel[0], sample_target_vel[1]);
  double eps = 1e-4;

通过C语言的数学库函数hypot求取x和y的合速度值。hypot本身用于求取直角三角形的斜边长。局部变量eps是一个有限小量，用于对机器人的速度限幅作出适当的膨胀。

  traj.cost_   = -1.0; // placed here in case we return early
  //trajectory might be reused so we'll make sure to reset it
  traj.resetPoints();

首先清空轨迹traj。

  // make sure that the robot would at least be moving with one of
  // the required minimum velocities for translation and rotation (if set)
  if ((limits_->min_vel_trans >= 0 && vmag + eps < limits_->min_vel_trans) &&
      (limits_->min_vel_theta >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_vel_theta)) {
    return false;
  }
  // make sure we do not exceed max diagonal (x+y) translational velocity (if set)
  if (limits_->max_vel_trans >=0 && vmag - eps > limits_->max_vel_trans) {
    return false;
  }

判定候选速度是否在膨胀之后的候选空间内，若不是则报错退出。

  int num_steps;
  if (discretize_by_time_) {
    // discretize_by_time_默认是false，不进这个入口。
    num_steps = ceil(sim_time_ / sim_granularity_);
  } else {
    //compute the number of steps we must take along this trajectory to be "safe"
    double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity
    double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
    num_steps =
        ceil(std::max(sim_time_distance / sim_granularity_,
            sim_time_angle    / angular_sim_granularity_));
  }

  if (num_steps == 0) {
    return false;
  }

根据仿真粒度计算仿真步数，即样本点数。如果速度量过小，在sim_time_的时间内不能产生超过仿真粒度的线位移或者角位移就报错退出。线位移、角度位移各自独立计算仿真步数，num_steps是它们的较大值。

  //compute a timestep
  double dt = sim_time_ / num_steps;
  traj.time_delta_ = dt;

  Eigen::Vector3f loop_vel;
  if (continued_acceleration_) {
    // continued_acceleration_ = !use_dwa_，意味着continued_acceleration_总是false，不进这个入口。
    // assuming the velocity of the first cycle is the one we want to store in the trajectory object
    loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
    traj.xv_     = loop_vel[0];
    traj.yv_     = loop_vel[1];
    traj.thetav_ = loop_vel[2];
  } else {
    // assuming sample_vel is our target velocity within acc limits for one timestep
    loop_vel = sample_target_vel;
    traj.xv_     = sample_target_vel[0];
    traj.yv_     = sample_target_vel[1];
    traj.thetav_ = sample_target_vel[2];
  }

计算仿真步长。continued_acceleration_ = !use_dwa_，意味着continued_acceleration_总是false，即不会考虑加速度，速度也就等于候选速度。

  //simulate the trajectory and check for collisions, updating costs along the way
  for (int i = 0; i < num_steps; ++i) {

    //add the point to the trajectory so we can draw it later if we want
    traj.addPoint(pos[0], pos[1], pos[2]);

    if (continued_acceleration_) {
      //calculate velocities
      loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
      //ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
    }

    //update the position of the robot using the velocities passed in
    pos = computeNewPositions(pos, loop_vel, dt);

  } // end for simulation steps
  return true; // trajectory has at least one point

在一个for循环中，依次计算各个仿真步的坐标点，并将之添加到输出参数traj中。computeNewPositions功能：从当前坐标pos开始，经过loop_vel*dt位移后，算出会到达的新坐标。关于computeNewPositions中使用的方法，参考：https://github.com/ydsf16/aruco_ekf_slam/blob/master/ArUco-EKF%20SLAM.pdf。

}

以下是和计算候选速度相关的配置变量。

三、SimpleScoredSamplingPlanner::scoreTrajectory

功能是计算某个轨迹得分。计算轨迹得分基本思路：把评价轨迹的标准细分为七条，七条之和就是该轨迹得分。每一条称为准则，计算得分过程就是依次计算每一条准则上的得分，然后相加。这些准则对象虽然具有不同数据类型，但是都继承自同一个基类TrajectoryCostFunction。

class TrajectoryCostFunction {
public:
  /**
   *
   * General updating of context values if required.
   * Subclasses may overwrite. Return false in case there is any error.
   */
  virtual bool prepare() = 0;
 
  /**
   * return a score for trajectory traj
   */
  virtual double scoreTrajectory(Trajectory &traj) = 0;
 
  double getScale() {
    return scale_;
  }
 
  void setScale(double scale) {
    scale_ = scale;
  }
 
  virtual ~TrajectoryCostFunction() {}
 
protected:
  TrajectoryCostFunction(double scale = 1.0): scale_(scale) {}
 
private:
  double scale_;
};

这里定义了两个纯虚函数prepare和scoreTrajectory，分别用于更新准则的上下文和对候选轨迹打分。除此之外，还定义了一个私有成员scale_，用于指示该准则得分权重，它不是个虚函数，如果不给专门设置，权重默认1.0。

上表是ros源码出现的7条准则，rose_ros只用了当中三种：ObstacleCost、path_costs、goal_costs。接下分析scoreTrajectory。

double SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost) {

scoreTrajectory有2个参数，都是输入参数，traj是上面generateTrajectory算出的某个轨迹，best_traj_cost指示之前已比较出的最优轨迹得分。对于得分和轨迹优劣的对应关系，1）得分>=0时，得分越低轨迹越优。2）是负数，意味着该轨迹是不合理的，表示的是错误代码，而不是得分。

  double traj_cost = 0;
  int gen_id = 0;
  for(std::vector<TrajectoryCostFunction*>::iterator score_function = critics_.begin(); 
        score_function != critics_.end(); ++score_function) {
    TrajectoryCostFunction* score_function_p = *score_function;
    if (score_function_p->getScale() == 0) {
      continue;
    }
    double cost = score_function_p->scoreTrajectory(traj);

准则对象虽然具有不同的数据类型，但是都继承自同一个基类TrajectoryCostFunction，调用虚函数scoreTrajectory得到轨迹在该准则下得分。

    if (cost < 0) {
      traj_cost = cost;
      break;
    }
    if (cost != 0) {
      cost *= score_function_p->getScale();
    }
    traj_cost += cost;

在累加到总得分时，每条准则有着不同权重，虚函数getScale表示该准则的权重。

    if (best_traj_cost > 0) {
      // since we keep adding positives, once we are worse than the best, we will stay worse
      if (traj_cost > best_traj_cost) {
        break;
      }

目前累加的得分已超过最优得分，可确定此条轨迹不会是最优轨迹了。免得浪费cpu，退出计算此轨迹。

    }
    gen_id ++;
  }

  return traj_cost;
}

 3.1 TwirlingCostFunction

TwirlingCostFunction可说是最简单的准则类。接口prepare总是返回true，scoreTrajectory总是以轨迹的角速度值作为轨迹的代价。

class TwirlingCostFunction: public base_local_planner::TrajectoryCostFunction {
public:

  TwirlingCostFunction() {}
  ~TwirlingCostFunction() {}

  // add cost for making the robot spin
  double TwirlingCostFunction::scoreTrajectory(Trajectory &traj) { return fabs(traj.thetav_); }

  bool prepare() {return true;};
};