move_base代码解析（四）局部路径规划：TrajectoryPlannerROS::computeVelocityCommands

在第三章中讲述了executeCycle的总体作用，可以看到这个函数的作用主要是将全局路径规划的路径给到局部路径规划，并判断机器人是否到位，如果没有到位就调用computeVelocityCommands函数计算机器人的速度。这里也就是move_base的局部路径规划的所在之处。这章简单张开看一下move_base的局部路径规划的流程：

move_base中的局部路径规划函数的默认入口函数应该是TrajectoryPlannerROS::computeVelocityCommands，这个函数主要包含了以下几个部分：

1、获取机器人的全局坐标

路径规划首先需要知道机器人在哪儿，所以算法的第一步先得到机器人的位姿

	std::vector<geometry_msgs::PoseStamped> local_plan;
    geometry_msgs::PoseStamped global_pose;
    if (!costmap_ros_->getRobotPose(global_pose)) {
      return false;
    }

2、获取全局路径规划

在获取了机器人的坐标后，下一步就是获取之前规划出来的全局路径规划了：

	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;
    }

这里的transformGlobalPlan是定义在goal_function中的函数，该函数的作用是根据本地代价地图，从全局路径global_plan截取一段，并将路径点从/map坐标系转换到/odom坐标系，生成结果放在transformed_plan。所以这里transformed_plan大小是小于全局路径点的。它的大小由：

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

决定，也就是代价地图的大小。

3、路径清除

在机器人运动过程中，全局路径中的前面一部分点位是被经历过的，局部路径中的一部分点也是已经走过的。为了减小后面的计算量。所以将这些点从路径中删除：

void prunePlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
    ROS_ASSERT(global_plan.size() >= plan.size());
    std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
    std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
    while(it != plan.end()){
      const geometry_msgs::PoseStamped& w = *it;
      // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
      double x_diff = global_pose.pose.position.x - w.pose.position.x;
      double y_diff = global_pose.pose.position.y - w.pose.position.y;
      double distance_sq = x_diff * x_diff + y_diff * y_diff;
      if(distance_sq < 1){
        ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.pose.position.x, global_pose.pose.position.y, w.pose.position.x, w.pose.position.y);
        break;
      }
      it = plan.erase(it);
      global_it = global_plan.erase(global_it);
    }
  }

这里的清除方式很简单，因为存储全局路径的容器是按顺序存放的，所以先经历过的点肯定是在global_plan的最前端，则我们判断这个点离机器人的距离。因为随着机器人的运动这个点会逐渐远离机器人。当其远离机器人的距离超过1m以上，则我们将这个点从全局路径global_plan以及代价地图中删除。

4、如果机器人到达目标点附近但是角度不满足要求

在计算速度时，首先判断当前机器人是否到达目标点，如果到达目标点的话，则需要再判断角度是否也满足阈值要求：

if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) {
      if (latch_xy_goal_tolerance_) {
        xy_tolerance_latch_ = true;
      }
      //检查是否到达目标“朝向、姿态”
      //获取当前朝向和目标姿态的差值
      double angle = getGoalOrientationAngleDifference(global_pose, goal_th);
      //check to see if the goal orientation has been reached
      //到达目标位置，如果差值小于容忍度
      if (fabs(angle) <= yaw_goal_tolerance_) 

如果机器人的坐标已经再目标点附近，同时判断出来角度的差值也小于阈值，则我们认为机器人已经再目标点离，此时将速度全都设置为0。如果此时角度差超过阈值，则认为机器人当前还没有完全到位，则调用函数更新机器人运动速度：

		//如果到达位置，但朝向和姿态没达到目标要求
        //将全局路径拷贝进来，并认为全局路径的最后一个点就是终点
        tc_->updatePlan(transformed_plan);
        //给定当前机器人的位置和朝向，计算机器人应该跟随的“best”轨迹，存储在drive_cmds中
        Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
        //发布代价地图点云
        map_viz_.publishCostCloud(costmap_);

        //copy over the odometry information
        //得到里程计信息
        nav_msgs::Odometry base_odom;
        odom_helper_.getOdom(base_odom);

首先这里的updatePlan(transformed_plan)将前面获取的全局路径规划中代价地图部分的位姿传递给trajectory_planner。然后再调用findBestPath(global_pose, robot_vel, drive_cmds)函数计算出实际的机器人的速度。数据存储在drive_cmds中返回。展开看一下findBestPath这个函数：

4.1、获取机器人的footprint

		//用当前位姿获取机器人footprint
    	std::vector<base_local_planner::Position2DInt> footprint_list =
        footprint_helper_.getFootprintCells(
            pos,
            footprint_spec_,
            costmap_,
            true);

在给定位置获取足迹的单元格。这里应该会根据机器人模型填充不同的单元格。然后将初始footprint内的所有cell标记 “within_robot = true”，表示在机器人足迹内：

	for (unsigned int i = 0; i < footprint_list.size(); ++i) {
      path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
    }

4.2、更新地图

	//确保根据全局规划global plan更新路径，并计算代价
    //更新哪些地图上的cell是在全局规划路径上的，target_dist设置为0
    //并且通过它们和其他点的相对位置计算出来地图上所有点的target_dist
    path_map_.setTargetCells(costmap_, global_plan_);
    goal_map_.setLocalGoal(costmap_, global_plan_);

对于setLocalGoal函数，是用来计算MapGrid地图中所有点到局部地图路径末端点的最短距离。在该函数中，首先对映射到局部地图中的路径进行栅格级的插补(MapGrid::adjustPlanResolution)，从而使得整个路径是栅格连续的。然后，查找路径在局部地图中的最后一个点，并将MapGrid地图中的该点标记为已访问（标记为已访问的点可以进行最短路径计算），然后调用computeTargetDistance函数采用类似dijkstra算法的逐步探索的方式，计算出MapGrid地图中所有点（栅格级）相对于与标记点的最短距离。

对于setTargetCells函数，是用来计算轨迹MapGrid地图中所有点到局部地图路径的最短距离。与setLocalGoal函数的区别主要在于，首先将局部地图路径中所有的点标记为已访问，然后调用computeTargetDistance函数就会计算MapGrid地图中所有点到整个局部地图路径的最短距离，具体算法需要进一步了解。

因此，在经过预计算后，对于path_costs_和alignment_costs这两个考虑与路径距离的打分项，其内部的MapGrid地图中存储的是所有点到路径的最短距离；对于goal_costs_和goal_front_costs_这两个考虑与局部目标点距离的打分项，其内部的MapGrid地图中存储的是所有点到局部目标点的最短距离。

4.3、找到代价最低的轨迹

当我们有了地图，有了路径，下一步自然是要找到一条代价最低的路径：

//输入分别是目前机器人位置，速度以及加速度限制
Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
        vel[0], vel[1], vel[2],
        acc_lim_x_, acc_lim_y_, acc_lim_theta_);//生成诸多可能轨迹，选取其中打分最高的。这里也就是最关键的一步。

4.3.1、计算可行的线速度和角速度范围

这里先对最大线速度进行一个限制，即保证速度既不超过预设的最大速度限制，也不超过“起点与目标直线距离/总仿真时间”。

	//compute feasible velocity limits in robot space
    //声明最大/小线速度，最大/小角速度
    double max_vel_x = max_vel_x_, max_vel_theta;
    double min_vel_x, min_vel_theta;
    //如果最终的目标是有效的（全局规划不为空）
    //检查最终点是否是有效的，判断变量在updatePlan中被赋值
    if( final_goal_position_valid_ ){
      //计算当前位置和目标位置之间的距离：final_goal_dist
      double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y );
      //最大速度：在预设的最大速度和
      max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ );
    }

4.3.2、计算线速度与角速度的上下限

使用的限制是由当前速度在一段时间内，由最大加减速度达到的速度范围，这里进行了一个判断，即是否使用dwa法：

① 使用dwa法，则用的是轨迹前向模拟的周期sim_period_（专用于dwa法计算速度的一个时间间隔）；

② 不使用dwa法，则用的是整段仿真时间sim_time_

	//should we use the dynamic window approach?
    //是否使用dwa算法，sim_peroid_是1/controller_frequency_，暂时不清楚sim_period_和sim_time_的区别
    if (dwa_) {
      //使用dwa窗口法，sim_period_是dwa计算最大、最小速度用的时间
      //计算速度、角速度范围，引入加速度限制条件（用sim_period_）
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
    } else {
      //忽略其中的逻辑，按照不同的规则生成路径，调用的子函数是generateTrajectory
      //不使用dwa窗口法
      //计算速度、角速度范围，引入加速度限制条件（用sim_time_）
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
    }

在生成范围时注意用预先给定的速度和角速度范围参数进行保护。

4.3.3、计算采样间隔以及初始采样速度

根据预设的线速度与角速度的采样数，和上面计算得到的范围，分别计算出采样间隔，并把范围内最小的线速度和角速度作为初始采样速度。不考虑全向机器人的情况，即不能y向移动，故暂不考虑vy上采样。

	//we want to sample the velocity space regularly
    //计算速度采样间隔
    double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);
    double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1);//计算角速度采样间隔

    double vx_samp = min_vel_x;//x方向速度采样点
    double vtheta_samp = min_vel_theta;//角速度采样点
    double vy_samp = 0.0;

    //keep track of the best trajectory seen so far
    Trajectory* best_traj = &traj_one;
    best_traj->cost_ = -1.0;//先初始化一个最优路径的代价=-1
    //下面生成的轨迹放到这里，和best_traj作比较，如果生成的轨迹代价更小，就选择它
    Trajectory* comp_traj = &traj_two;
    //先初始化一个当前“对比”路径的代价=-1，等会再用generateTrajectory函数生成路径和新的代价存放在里面
    comp_traj->cost_ = -1.0;
    //用于交换的指针
    Trajectory* swap = NULL;

为了迭代比较不同采样速度生成的不同路径的代价，这里声明best_traj和comp_traj并都将其代价初始化为-1

4.3.4、轨迹生成

遍历所有线速度和角速度，调用类内generateTrajectory函数用它们生成轨迹。二重迭代时，外层遍历线速度（正值），内层遍历角速度。在遍历时，单独拎出角速度=0，即直线前进的情况，避免由于采样间隔的设置而跃过了这种特殊情况。

边迭代生成边比较，获得代价最小的路径，存放在best_traj。

	  //【循环所有速度和角速度、打分】
      //外侧循环所有x速度
      for(int i = 0; i < vx_samples_; ++i) {
        //x速度循环内部遍历角速度
        //①角速度=0
        vtheta_samp = 0;
        //first sample the straight trajectory
        //传入当前位姿、速度、加速度限制，采样起始x向速度、y向速度、角速度，代价赋给comp_traj
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

        //if the new trajectory is better... let's take it
        //对比生成路径和当前最优路径的分数，如果生成的路径分数更小，就把当前路径和最优路径交换
        //这里会将第一次生成路径的代价赋给best_traj
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }
        //接下来产生机器人在原地旋转的轨迹
        vtheta_samp = min_vel_theta;
        //next sample all theta trajectories
        //接下来迭代循环生成所有角速度的路径、打分
        for(int j = 0; j < vtheta_samples_ - 1; ++j){
          generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
              acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

          //if the new trajectory is better... let's take it
          //同样地，如果新路径代价更小，和best_traj作交换
          if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
            swap = best_traj;
            best_traj = comp_traj;
            comp_traj = swap;
          }
          //遍历角速度
          vtheta_samp += dvtheta;
        }
        //遍历x速度
        vx_samp += dvx;
      }

这里需要注意到一个重要的函数就是generateTrajectory函数，局部路径规划的路径生成器就是来自于这里。这个函数出要逻辑如下：

4.3.4.1、计算总体速度

	//compute the magnitude of the velocities
    double vmag = hypot(vx_samp, vy_samp);

C ++hypot()函数是cmath标头的库函数,用于查找给定数字的斜边,接受两个数字并返回斜边的计算结果,即sqrt(x * x + y * y)。

4.3.4.2、计算步长

	int num_steps;
    if(!heading_scoring_) {
      //sim_granularity_：仿真点之间的距离间隔
      //步数 = max(速度模×总仿真时间/距离间隔，角速度/角速度间隔)，四舍五入
      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);
    }

局部路径规划的终点在cost_map的边界处，一般还是距离比较远的。所以需要在路径上按照一定的角度以及距离阈值采样。

4.3.4.3、计算每个步长的时间

	//至少一步
    if(num_steps == 0) {
      num_steps = 1;
    }
    //每一步的时间
    double dt = sim_time_ / num_steps;
    double time = 0.0;

4.3.4.4、声明一条轨迹

resetPoints函数创造一个新的轨迹，轨迹速度设置为sample，cost设置为-1

	//create a potential trajectory
    //声明轨迹
    traj.resetPoints();
    traj.xv_ = vx_samp;//线速度
    traj.yv_ = vy_samp;
    traj.thetav_ = vtheta_samp;//角速度
    traj.cost_ = -1.0;//默认代价-1.0

4.3.4.5、模拟每一步运动

在得到具体步数以及步长后，接下来就是计算每一步机器人的落点了，这里是通过一个for循环获取所有的落点，主要是执行了以下几个步骤：

1. 使用footprintCost计算出footprint

footprint为负，即在这一步遇到了障碍物，规划失败

 	  double footprint_cost = footprintCost(x_i, y_i, theta_i);

      //if the footprint hits an obstacle this trajectory is invalid
      //若返回一个负值，说明机器人在路径上遇障，直接return
      if(footprint_cost < 0){
        traj.cost_ = -1.0;
        return;

footprint_cost的计算方式是将当前机器人的footprint投影到costmap下，然后判断这个区域内的点的cost。对于costmap，其中每个栅格的cost计算方式如下：

在给定一个单元格与障碍物之间的距离的情况下，计算距离的成本值。首先进行距离的判断，如果距离为0，则返回代价为致命障碍物的代价值LETHAL_OBSTACLE;再继续往下判断，距离x分辨率是否小于内切半径，是的话，则返回代价为机器人内切圆膨胀障碍物的代价值INSCRIBED_INFLATED_OBSTACLE;否的话，则代价按照欧几里德距离递减，欧几里德距离为距离x分辨率，定义factor因子为指数函数，最后计算出cost值。可以看出，距离障碍物越远，代价值就越小；

static const unsigned char NO_INFORMATION = 255;  未知空间
static const unsigned char LETHAL_OBSTACLE = 254;  说明该单元格中存在一个实际的障碍。若机器人的中心在该单元格中，机器人必然会跟障碍物相撞
static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;  说明该单元格离障碍物的距离小于机器人内切圆半径。若机器人的中心位于等于或高于"Inscribed" cost的单元格，机器人必然会跟障碍物相撞
static const unsigned char FREE_SPACE = 0;  没有障碍的空间

/** @brief  Given a distance, compute a cost.给定一个距离，计算一个cost代价
   * @param  distance The distance from an obstacle in cells 单元格与障碍物之间的距离
   * @return A cost value for the distance 距离的成本值
   * 
   * */
virtual inline unsigned char computeCost(double distance) const
  {
    unsigned char cost = 0;
    if (distance == 0)
      cost = LETHAL_OBSTACLE;
    else if (distance * resolution_ <= inscribed_radius_)
      cost = INSCRIBED_INFLATED_OBSTACLE;
    else
    {
      // make sure cost falls off by Euclidean distance 确保成本以欧几里德距离递减
      double euclidean_distance = distance * resolution_;
      double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
      cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
    }
    return cost;
  }

而根据机器人模型又可以分为两种情况：

圆形机器人：对于圆形机器人，需要判断机器人的圆心是否处于FREE_SPACE就可以了。之所以不用考虑半径问题是因为在costmap初始化问题时已经考虑了膨胀半径的问题了。一般情况下costmap的膨胀半径会略大于机器人内切圆半径，这样子如果机器人处于膨胀层就说明机器人的边会碰到障碍物。而这些膨胀层就是上面的INSCRIBED_INFLATED_OBSTACLE。所以对于圆形的机器人，不管它的中心点处于NO_INFORMATION、LETHAL_OBSTACLE还是INSCRIBED_INFLATED_OBSTACLE都是不可取的，所以圆形机器人的代价值应该在0-252。

多边形机器人：而多边形机器人则需要计算每一条边上的点，判断这条边有没有与障碍物重合，这里的边进入膨胀层其实是可以通行的，因为膨胀层在本质上从空间角度来看是可通行的空间，如果机器人的边界都在膨胀层而没有入侵到障碍物层，则机器人本身也不会与障碍物发生冲突。所以多边形机器人的代价值范围应该在0-253。

2. 计算occcost

	  //更新occ_cost：max(max(occ_cost，路径足迹代价)，当前位置的代价)
      //也就是把所有路径点的最大障碍物代价设置为路径的occ_cost
      occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));

costmap_.getCost的作用为给定一个栅格（mx, my）,返回这个单元的代价值。每个栅格具体的代价值在前面局部地图初始化的时候应该都计算好了的(4.2)，这里只需要查询就可以了。所以这里occ_cost的值相当于获取了0、footprint_cost以及getCost(cell_x, cell_y)三者中的最大者。这个值越大说明了机器人走这条路前往目的地的代价就越大。对于圆形机器人来说footprint_cost应该等于getCost(cell_x, cell_y)>0,而矩形机器人的话则既可能footprint_cost>getCost(cell_x, cell_y)也可能footprint_cost

3. 计算goaldist和pathdist

这里似乎是计算了两个距离值，一个是到目标点的距离，一个是到全局路径的距离？这两个值越大代表当前仿真得到的路径距离目标点的代价越大。

4. 判断是否需要根据朝向打分

heading_scoring_timestep_是给朝向打分时在时间上要看多远，也就是在路径上走过一个特定时刻后，才为朝向打分一次。

5. 轨迹打分

前面4.3.4.2计算了一个迭代次数作为for循环的次数，在达到该次数之前会一直循环前面四个步骤并将每次循环计算得到的轨迹点添加到traj.point内作为备选路径点，直到达到循环次数，此时机器人基本上也就位于了这条轨迹的端点。此时需要计算并返回这条轨迹的最终代价值，这个值最后也会作为这条轨迹是否被选上的依据：

	double cost = -1.0;
    if (!heading_scoring_) {
      //代价=路径距离+目标距离+障碍物代价（乘以各自的比例）
      cost = path_distance_bias_ * path_dist + goal_dist * goal_distance_bias_ + occdist_scale_ * occ_cost;
    } else {
      //代价=路径距离+目标距离+障碍物代价+航向角（如果有航偏则会增大代价）
      cost = occdist_scale_ * occ_cost + path_distance_bias_ * path_dist + 0.3 * heading_diff + goal_dist * goal_distance_bias_;
    }
    //设置称当前路径的代价
    traj.cost_ = cost;

最终的打分原则比较简单，就是三个代价值再乘以各自的比例，如果考虑角度分的话再加一个角度分，最后cost存储到这条轨迹的traj.cost中。至此，一条路径的规划以及这条路径的分值就确定下来了。

4.3.5、轨迹选择

在上一步骤中我们逐步产生了很多条轨迹，但是这些轨迹中实际使用的只有一条，怎么区分出我们需要的这一条轨迹，就是根据traj.cost参数：

if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
            swap = best_traj;
            best_traj = comp_traj;
            comp_traj = swap;
          }

首先这条轨迹的分数得大于零，这是由于cost中我们根据它的值是否大于零判断这条路径是否会跟障碍物有冲突，对于小于零的路径是走不了的，所以一条合适的路径必须大于零。其次它的代价值必须足够小。代价值越小，说明这条路径离全局路径越接近，同时离障碍物越远，那么从规划的角度类说这是一条很合适的路径。因为离全局路径越接近说明实际要走的路径越短，离障碍物越远说明这条路径很安全不容易出问题。

4.3.6、原地旋转轨迹评分

在轨迹生成中，还需要考虑一种情况就是只有角速度没有线速度的情况，考虑一下这种情况什么时候会发生：

1、当TrajectoryPlannerROS中，位置已经到达目标（误差范围内），姿态已达，则直接发送0速；姿态未达，则调用降速函数和原地旋转函数，并调用checkTrajectory函数检查合法性，直到旋转至目标姿态。而checkTrajectory函数调用的是scoreTrajectory和generateTrajectory，不会调用createTrajectory函数，所以，快要到达目标附近时的原地旋转，不会进入到这个函数的这部分来处理。

2、由于局部规划器的路径打分机制（后述）是：“与目标点的距离”和“与全局路径的偏离”这两项打分都只考虑路径终点的cell，而不是考虑路径上所有cell的综合效果，机器人运动到一个cell上，哪怕有任何一条能向目标再前进的无障碍路径，它的最终得分一定是要比原地旋转的路径得分来得高的。

所以，这里的原地自转，是行进过程中的、未达目标附近时的原地自转，并且，是机器人行进过程中遇到障碍、前方无路可走只好原地自转，或是连原地自转都不能满足，要由逃逸状态后退一段距离，再原地自转调整方向，准备接下来的行动。一种可能情况是机器人行进前方遇到了突然出现而不在地图上的障碍。

在处理这种情况时，由于机器人原地旋转时的角速度限制范围要比运动时的角速度限制范围更严格，底盘无法处理过小的原地转速，故要注意处理这层限制。同样调用generateTrajectory函数，生成路径。

//循环所有角速度
    double heading_dist = DBL_MAX;

    for(int i = 0; i < vtheta_samples_; ++i) {
      //enforce a minimum rotational velocity because the base can't handle small in-place rotations
      //强制最小原地旋转速度
      double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
        : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
      //产生遍历角速度的路径
      generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
          acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

      //if the new trajectory is better... let's take it...
      //note if we can legally rotate in place we prefer to do that rather than move with y velocity
      //如果新路径代价更小
      if(comp_traj->cost_ >= 0
          && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0)
          && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
        //获取新路径的终点（原地）
        double x_r, y_r, th_r;
        comp_traj->getEndpoint(x_r, y_r, th_r);
        //坐标计算
        x_r += heading_lookahead_ * cos(th_r);
        y_r += heading_lookahead_ * sin(th_r);
        unsigned int cell_x, cell_y;

        //make sure that we'll be looking at a legal cell
        //转换到地图坐标系，判断与目标点的距离
        if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
          double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
          //取距离最小的，放进best_traj
          if (ahead_gdist < heading_dist) {
            //if we haven't already tried rotating left since we've moved forward
            if (vtheta_samp < 0 && !stuck_left) {
              swap = best_traj;
              best_traj = comp_traj;
              comp_traj = swap;
              heading_dist = ahead_gdist;
            }
            //if we haven't already tried rotating right since we've moved forward
            else if(vtheta_samp > 0 && !stuck_right) {
              swap = best_traj;
              best_traj = comp_traj;
              comp_traj = swap;
              heading_dist = ahead_gdist;
            }
          }
        }
      }

      vtheta_samp += dvtheta;
    }

4.3.7、轨迹判断

在上述几种情况下的轨迹都考虑完后需要对整个轨迹的结果进行判断：如果说轨迹的cost最终大于0，说明找到了一条轨迹，则我们会返回这条轨迹：

//检查最优轨迹的分数是否大于0
    if (best_traj->cost_ >= 0) {
      // avoid oscillations of in place rotation and in place strafing
      //抑制震荡影响：当机器人在某方向移动时，对下一个周期的与其相反方向标记为无效
      //直到机器人从标记震荡的位置处离开一定距离，返回最佳轨迹。
      
      //若轨迹的采样速度≤0
      if ( ! (best_traj->xv_ > 0)) {
        //若轨迹的角速度<0
        if (best_traj->thetav_ < 0) {
          if (rotating_right) {
            stuck_right = true;
          }
          rotating_right = true;
        } else if (best_traj->thetav_ > 0) {
          if (rotating_left){
            stuck_left = true;
          }
          rotating_left = true;
        } else if(best_traj->yv_ > 0) {
          if (strafe_right) {
            stuck_right_strafe = true;
          }
          strafe_right = true;
        } else if(best_traj->yv_ < 0){
          if (strafe_left) {
            stuck_left_strafe = true;
          }
          strafe_left = true;
        }

        //set the position we must move a certain distance away from
        prev_x_ = x;
        prev_y_ = y;
      }

      double dist = hypot(x - prev_x_, y - prev_y_);
      if (dist > oscillation_reset_dist_) {
        rotating_left = false;
        rotating_right = false;
        strafe_left = false;
        strafe_right = false;
        stuck_left = false;
        stuck_right = false;
        stuck_left_strafe = false;
        stuck_right_strafe = false;
      }

      dist = hypot(x - escape_x_, y - escape_y_);
      if(dist > escape_reset_dist_ ||
          fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_){
        escaping_ = false;
      }

      return *best_traj;
    }

若找不到有效轨迹，进入逃逸状态，后退、原地自转，若找不到下一步的有效路径则再后退、自转，直到后退的距离或转过的角度达到一定标准，才会退出逃逸状态，重新规划前向轨迹。其中再加上震荡抑制。

vtheta_samp = 0.0;
    vx_samp = backup_vel_;//后退速度
    vy_samp = 0.0;
    generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
        acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

    //if the new trajectory is better... let's take it
    /*
       if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
       swap = best_traj;
       best_traj = comp_traj;
       comp_traj = swap;
       }
       */

    //we'll allow moving backwards slowly even when the static map shows it as blocked
    swap = best_traj;
    best_traj = comp_traj;
    comp_traj = swap;

    double dist = hypot(x - prev_x_, y - prev_y_);
    if (dist > oscillation_reset_dist_) {
      rotating_left = false;
      rotating_right = false;
      strafe_left = false;
      strafe_right = false;
      stuck_left = false;
      stuck_right = false;
      stuck_left_strafe = false;
      stuck_right_strafe = false;
    }

    //only enter escape mode when the planner has given a valid goal point
    if (!escaping_ && best_traj->cost_ > -2.0) {
      escape_x_ = x;
      escape_y_ = y;
      escape_theta_ = theta;
      escaping_ = true;
    }

    dist = hypot(x - escape_x_, y - escape_y_);

    if (dist > escape_reset_dist_ ||
        fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) {
      escaping_ = false;
    }


    //if the trajectory failed because the footprint hits something, we're still going to back up
    if(best_traj->cost_ == -1.0)
      best_traj->cost_ = 1.0;

    return *best_traj;

若后退速度生成的轨迹的终点有效（> -2.0），进入逃逸状态，循环后退、自转，并且记录下的逃逸位置和姿态，只有当离开逃逸位置一定距离或转过一定角度，才能退出逃逸状态，再次规划前向速度。逃逸判断和震荡判断一样，也已在上面多次执行，只要发布best_traj前就执行一次判断。

至此，4.3节最优的路径就已经得到了，这个路径会被返回到TrajectoryPlanner::findBestPath函数，再由这里返回到TrajectoryPlannerROS::computeVelocityCommands函数中。

4.4、旋转使机器人到达目标点

前面4.3虽然讲了一大通局部路径规划，但是emmmm其实这里没有用到，对于这种位置到达但是角度没有到的情况，其实是通过：

rotateToGoal(global_pose, robot_vel, goal_th, cmd_vel)

来旋转到最终目标点的。这个函数的原理是：在达到目标点误差范围内，且速度降至极小后，最后一步的工作是原地旋转至目标姿态。它采用一种类似“反馈控制”的思想，通过计算当前姿态与目标姿态的差值，通过这个差值来控制下一步的角速度。

5、如果机器人还没有到达目标点附近

5.1 轨迹生成

如果机器人还没有到达目标点，那么这里就很简单了，肯定是局部规划轨迹，也就是前面的4.3节的全部内容：

	//若未到达目标点误差范围内，调用TrajectoryPlanner类的updatePlan函数，将global系下的全局规划传入，再调用findBestPath函数，进行局部规划，
    //速度结果填充在drive_cmds中，并得到局部路线plan。
    //如果没到目标“位置”，更新全局规划
    tc_->updatePlan(transformed_plan);

    //compute what trajectory to drive along
    //用当前机器人位姿和速度，计算速度控制命令
    //【核心：TrajectoryPlanner的findBestPath函数】
    Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);

5.2 最优轨迹判断以及速度获取

然后判断代价值，也就是生成出来的最优的代价值是否大于零，大于零代表是有效的轨迹，这时候可以给定速度以及角速度：

	cmd_vel.linear.x = drive_cmds.pose.position.x;
    cmd_vel.linear.y = drive_cmds.pose.position.y;
    cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);

这里的drive_cmds来自于findBestPath中的返回值：

Trajectory TrajectoryPlanner::findBestPath(const geometry_msgs::PoseStamped& global_pose,
      geometry_msgs::PoseStamped& global_vel, geometry_msgs::PoseStamped& drive_velocities)

而它的计算结果其实是来自于最优轨迹：

//找到代价最低的轨迹，输入分别是目前机器人位置，速度以及加速度限制
    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");

	  drive_velocities.pose.position.x = best.xv_;
      drive_velocities.pose.position.y = best.yv_;
      drive_velocities.pose.position.z = 0;
      tf2::Quaternion q;
      q.setRPY(0, 0, best.thetav_);
      tf2::convert(q, drive_velocities.pose.orientation);

在createTrajectories中我们对每一条轨迹采样时都有一个当前轨迹使用的速度以及角速度值，它来自于generateTrajectory的返回：

traj.resetPoints();
    traj.xv_ = vx_samp;//线速度
    traj.yv_ = vy_samp;
    traj.thetav_ = vtheta_samp;//角速度

所以至此我们就得到了一条最优轨迹所需要的速度以及角速度。

5.3 发布局部路径

在得到速度后，不仅要发布速度还要发布局部路径点位，前面4.3做最优路径规划时得到了最优路径上的连续点会在这里被发布出来，可以作为可视化的一部分便于调试，判断机器人的运动是否与规划的结果类似。

总结：move_base的局部路径规划主要执行了以下内容：首先获取机器人的全局坐标，然后生成局部代价地图。根据全局规划的结果判断当前机器人是否到达目标点，这里分两种情况考虑：如果位置到达了但是角度没有到达，则会调用rotateToGoal函数生成一个旋转的角速度使机器人的朝向最终到达目标点；如果机器人连位置都没有到达，则会调用findBestPath函数生成出一条前往目标点的最小代价路径。最小代价路径的生成方式主要是通过对机器人的速度以及角速度分别采样，再给定一定的采样周期，判断这一段时间内的机器人最终运动的目标点的代价值是否是所有采样样本中最小的，代价值的计算公式为：路径距离+目标距离+障碍物代价（乘以各自的比例）。即这条路径离目标点越近，离全局路径越近，离障碍物越远则它的得分越低，则它是最优的一条路径。然后再将这条采样路径的速度角度给到最终的发布器，就完成了computeVelocityCommands全部工作。

参考
1、Base Local Planner局部规划-TrajectoryPlanner源码解读-2

https://blog.csdn.net/neo11111/article/details/104713086/

2、MapGridCostFunction

https://www.cswamp.com/post/113