ROS运动规划学习七---base_local_planner

---

前言

经过之前的学习，我们知道dwa_local_planner局部路径规划只是起到桥梁作用，主要的处理函数在base_local_planner中,下面对其进行简要的分析。由于功能包中的函数太多，不一一列举了，需要的可以去看源码，这里介绍两个重要的类：SimpleTrajectoryGenerator类和SimpleScoredSamplingPlanner类。

---

一、simple_trajectory_generator.cpp

采样速度获取函数

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) {
    //获得最大旋转速度
    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;
	
	  // x,y方向和角速度的采样个值不能为0
  	if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {
    //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_) {   //不用DWA
      // 首先用当前位置到目标位置的距离和仿真时间sim_time处理得到max_vel_x和max_vel_y边界，
      // 目的是模拟仿真时间内匀减速到0，刚好到达目标点的情景
      double dist = hypot(goal[0] - pos[0], goal[1] - pos[1]);
      max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);
      max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);
      // 然后基于acc_lim * sim_time得到一种边界，还有设置的速度参数限制作为一种边界，
      // 选取边界中空间较小的边界。这种策略，能够获得较大的采样空间
      max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
      max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
      max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);
      min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
      min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
      min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
    } else {
      // DWA方法，直接用acc_lim * 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_);
    }
    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]);
    // 得到速度空间边界后，根据x，y，theta三个采样个数进行插补，进而组合出整个速度采样空间
    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();
          //ROS_DEBUG("Sample %f, %f, %f", vel_samp[0], vel_samp[1], vel_samp[2]);
          sample_params_.push_back(vel_samp);  //存放采样空间的组合
        }
        th_it.reset();
      }
      y_it.reset();
    }
    }
}

二、simple_scored_sampling_planner

通过SimpleTrajectoryGenerator的成员函数initialise()产生一系列的采样空间，之后利用采样空间和样本来得到一系列的轨迹。该文件对这一系列轨迹进行打分，计算得到最优轨迹，并将最优轨迹对应的速度采样下发给底座。

2.1 simple_scored_sampling_planner.h

根据给定的产生器和代价函数计算一个局部轨迹。假设代价越小越好，同时负值的代价代表无穷大

class SimpleScoredSamplingPlanner : public base_local_planner::TrajectorySearch 
{
public:
	  ~SimpleScoredSamplingPlanner() {}
	  SimpleScoredSamplingPlanner() {}
	  //处理一系列的产生器和打分器，打分器返回代价>0，若<0意味着无效路径，
	  //产生器是fallback产生器，在先前的产生器没有找到有效路径时，才计算，用到每个产生器直到不在返回局部路径或者技术满足要求,然后重置计数并尝试列表的下一个
	  //若max_samples = -1 (默认值)：每一个采样规划器会持续的调用产生器直到产生器遍历了所有的样本
	  SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples = -1);
	  //获取轨迹的得分，遍历各个打分项进行求和
	  double scoreTrajectory(Trajectory& traj, double best_traj_cost);
	  //得到最优的轨迹
	  bool findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored = 0);
private:
	  std::vector<TrajectorySampleGenerator*> gen_list_;
	  std::vector<TrajectoryCostFunction*> critics_;
	  int max_samples_;
	};
}

2.2 simple_scored_sampling_planner.cpp

SimpleScoredSamplingPlanner::scoreTrajectory()这里作为桥梁，调用打分器对应的函数scoreTrajectory()进行打分—即代价，得到对应的代价后乘以相应的scale作为最终的打分结果，进行累加得到所有的代价和，之后与之前最好的代价进行判断，若大于最好的，则直接输出对应的代价。具体的打分函数需要仔细去看

...

  double SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost)
  {
 	......
  }
  bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) 
  {
    Trajectory loop_traj;
    Trajectory best_traj;
    double loop_traj_cost, best_traj_cost = -1;
    bool gen_success;
    int count, count_valid;
    //调用各个打分项的prepare()函数进行准备 
    for (std::vector<TrajectoryCostFunction*>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end(); ++loop_critic) {
      TrajectoryCostFunction* loop_critic_p = *loop_critic;
      if (loop_critic_p->prepare() == false) {
        ROS_WARN("A scoring function failed to prepare");
        return false;
      }
    }
    //对每一个轨迹的产生器进行遍历生成对应的轨迹 ，并进行对应的打分
    for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {
      count = 0;
      count_valid = 0;
      TrajectorySampleGenerator* gen_ = *loop_gen;
      // 检查是否有更多路径
      while (gen_->hasMoreTrajectories()) {
        // 生成下一个轨迹。此处调用的是轨迹生成器simple_trajectory_generator的函数nextTrajectory()
        //前面的生成轨迹的依据的采样速度已经生成。
        gen_success = gen_->nextTrajectory(loop_traj);
        if (gen_success == false) {
          continue;
        }
        // 对生成的轨迹进行打分 
        loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
        if (all_explored != NULL) {
          loop_traj.cost_ = loop_traj_cost;
          all_explored->push_back(loop_traj);
        }

        if (loop_traj_cost >= 0) {
          count_valid++;
          if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
            // 将更优秀的cost赋值给best_traj_cost
            best_traj_cost = loop_traj_cost;
            // 将cost更低的轨迹也赋值给best_traj
            best_traj = loop_traj;
          }
        }
        count++;
        if (max_samples_ > 0 && count >= max_samples_) {
          break;
        }
      }
      // 对最终轨迹的速度和cost进行填充
      if (best_traj_cost >= 0) {
        traj.xv_ = best_traj.xv_;
        traj.yv_ = best_traj.yv_;
        traj.thetav_ = best_traj.thetav_;
        traj.cost_ = best_traj_cost;
        traj.resetPoints();
        double px, py, pth;
        for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
          best_traj.getPoint(i, px, py, pth);
          // 对最终轨迹的点数据进行填充
          traj.addPoint(px, py, pth);
        }
      }
      ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
      if (best_traj_cost >= 0) {
        // do not try fallback generators
        break;
      }
    }
  }

三、打分函数

• oscillation_cost_function.cpp:
  base_local_planner::OscillationCostFunction oscillation_costs_
  判断是否来回移动，左右来回移动，旋转是否来回变化，返回-5；没有来回变化返回0

double OscillationCostFunction::scoreTrajectory(Trajectory &traj) {
  if ((forward_pos_only_ && traj.xv_ < 0.0) ||
      (forward_neg_only_ && traj.xv_ > 0.0) ||
      (strafe_pos_only_  && traj.yv_ < 0.0) ||
      (strafe_neg_only_  && traj.yv_ > 0.0) ||
      (rot_pos_only_     && traj.thetav_ < 0.0) ||
      (rot_neg_only_     && traj.thetav_ > 0.0)) {
    return -5.0;
  }
  return 0.0;
}

• obstacle_cost_function.cpp
  base_local_planner::ObstacleCostFunction obstacle_costs_;
  获得对代价值进行调节的scale， 对轨迹中的每个点进行遍历，检测是否碰撞，计算代价得到f_cost，若sum_scores_为真进行累加，为假选择最大的代价。这里的footprintCost()函数调用的是base_local_planner::WorldModel类中的成员函数footprintCost()计算，得到代价后进行检查不能小于零，小于0，返回-6，不能超出地图范围，超出范围返回-7。

double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }
  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);
    if(f_cost < 0){
        return f_cost;
    }
    if(sum_scores_)
        cost +=  f_cost;
    else
        cost = std::max(cost, f_cost);
  }
  return cost;
}

• map_grid_cost_function.cpp
  地图打分函数有四个：
  base_local_planner::MapGridCostFunction path_costs_;
  base_local_planner::MapGridCostFunction goal_costs_;
  base_local_planner::MapGridCostFunction goal_front_costs_;
  base_local_planner::MapGridCostFunction alignment_costs_;
  先确定初始化cost值，for循环对每一个点进行预处理，是否移到前面，是否移到边上 ，是否越界，越界返回-4，没有越界，通过getCellCosts()函数得到grid_dist 代价，之后开始进行代价计算，三种计算方式：Last:直接用grid_dist 作为代价，Sum:将grid_dist 累加，Product：将grid_dist 进行累乘得到cost返回。
  四种打分函数按照参数的不同实现方式也不同。

double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0.0;
  if (aggregationType_ == Product) {
    cost = 1.0;
  }
  double px, py, pth;
  unsigned int cell_x, cell_y;
  double grid_dist;
  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    if (xshift_ != 0.0) {
      px = px + xshift_ * cos(pth);
      py = py + xshift_ * sin(pth);
    }
    if (yshift_ != 0.0) {
      px = px + yshift_ * cos(pth + M_PI_2);
      py = py + yshift_ * sin(pth + M_PI_2);
    }
    if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
      //we're off the map
      ROS_WARN("Off Map %f, %f", px, py);
      return -4.0;
    }
    grid_dist = getCellCosts(cell_x, cell_y);
    if (stop_on_failure_) {
      if (grid_dist == map_.obstacleCosts()) {
        return -3.0;
      } else if (grid_dist == map_.unreachableCellCosts()) {
        return -2.0;
      }
    }
    switch( aggregationType_ ) {
    case Last:
      cost = grid_dist;
      break;
    case Sum:
      cost += grid_dist;
      break;
    case Product:
      if (cost > 0) {
        cost *= grid_dist;
      }
      break;
    }
  }
  return cost;
}

• twirling_cost_function.cpp.cpp:
  base_local_planner::TwirlingCostFunction twirling_costs_
  最简单的打分方式，直接输出角速度，角速度越大，输出的代价越大

double TwirlingCostFunction::scoreTrajectory(Trajectory &traj) {
  return fabs(traj.thetav_);  
}

---

总结

本文对base_local_planner中的主要函数进行分析，函数之间互相调用，看着比较乱，对于其他的函数没有进行分析。