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