dwa_local_planner

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

一、DWAPlannerROS::computeVelocityCommands

move_bae通过接口computeVelocityCommands进行本地路径规划，下面代码片段是DWAPlannerROS的实现。它有一个参数cmd_vel，这是一个输出参数，用于输出本地规划器计算的机器人控制指令。

bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
  // dispatches to either dwa sampling control or stop and rotate control, 
  // depending on whether we have been close enough to goal
  if ( ! costmap_ros_->getRobotPose(current_pose_)) {
    ROS_ERROR("Could not get robot pose");
    return false;
  }

在函数一开始先通过代价地图对象获取机器人的位姿。DWAPlannerROS运行在本地代价地图，它的全局坐标系是odom，current_pose_就是base_footprint-->odom的tf。

  std::vector<geometry_msgs::PoseStamped> transformed_plan;
  if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
    ROS_ERROR("Could not get local plan");
    return false;
  }

然后通过planner_util_对象的getLocalPlan接口，在全局轨迹中截取从当前位置到终点之间的路径，用本地变量transformed_plan记录。getLocalPlan除了截取剩余轨迹之外， 还会将全局轨迹点转换到代价地图的坐标系下。所以我猜这个本地变量的前缀transformed就是用来描述这一特性的。下文“二、LocalPlannerUtil::global_plan_、DWAPlanner::global_plan_”有叙述getLocalPlan。

  //if the global plan passed in is empty... we won't do anything
  if(transformed_plan.empty()) {
    ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
    return false;
  }

如果剩余的路径是空的，就没有继续规划的必要了。

图1给出一个运行实例。rviz显示了目标设置在大概(-0.5, -3)，绿色线条是全局规划器规划出的路径，存储在变量planner_util_.global_plan_。global_plan_坐标系是“map”，路径中有132个点。front基本等于map-->base_footprint的tf，即“零+current_pose_”，零是map和odom重合，current_pose_是base_footprint-->odom的tf；back基本等于目标点(-0.5, -3)。transformed_plan是从global_plan_生成的路径，由于map和odom重合，可认为就是原坐标值截取。本地代价地图的垂直半径1.5米，于是transformed_plan在距离1.5米处截止。图中列出了两个路径变量中的0、1、2、65、66，它们坐标值一样，但所属坐标系不一样，原因是上面所说的“getLocalPlan除了截取剩余轨迹之外， 还会将全局轨迹点转换到代价地图的坐标系下”。

  ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());

  // update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
  dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());

更新规划器的轨迹和本地代价。

  if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
    //publish an empty plan because we've reached our goal position
    std::vector<geometry_msgs::PoseStamped> local_plan;
    std::vector<geometry_msgs::PoseStamped> transformed_plan;
    publishGlobalPlan(transformed_plan);
    publishLocalPlan(local_plan);
    base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
    return latchedStopRotateController_.computeVelocityCommandsStopRotate(
        cmd_vel,
        limits.getAccLimits(),
        dp_->getSimPeriod(),
        &planner_util_,
        odom_helper_,
        current_pose_,
        boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));

然后通过辅助对象latchedStopRotateController_判定是否到达了终点。 这个辅助对象是包base_local_planner中定义的辅助类LatchedStopRotateController。 如果我们到达了终点，就发布空路径，同时通过辅助对象控制机器人停下来。

  } else {
    bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
    if (isOk) {
      publishGlobalPlan(transformed_plan);
    } else {
      ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
      std::vector<geometry_msgs::PoseStamped> empty_plan;
      publishGlobalPlan(empty_plan);
    }
    return isOk;
  }
}

没有到达目标位置，我们就通过成员函数dwaComputeVelocityCommands计算速度控制指令，并发布路径。

二、LocalPlannerUtil::global_plan_、DWAPlanner::global_plan_

• LocalPlannerUtil::global_plan_。makePlan规划出的全局路径，路径点是/map坐标系。示例值见图1中的planner_util_.global_plan_，有132个路径点。
• DWAPlanner::global_plan_。根据本地代价地图从LocalPlannerUtil::global_plan_截取出的一段路径，并把路径点转换到/odom坐标系。示例值见图1中的transformed_plan，从132中截取了67个路径点。transformed_plan会原样赋值给DWAPlanner::global_plan_。

生成这两个路径的大致逻辑。

1. (MoveBase::executeCycle)planThread线程规划出全局路径后，调用tc_->setPlan(*controller_plan_)，tc_类型是DWAPlannerROS。
2. (DWAPlannerROS::setPlan)把makePlan规划出的结果原样赋值给planner_util_.global_plan_。
3. (DWAPlannerROS::computeVelocityCommands)调用planner_util_.getLocalPlan，后者通过调用transformGlobalPlan，根据本地代价地图，从全局路径planner_util_.getLocalPlan截取一段，并将路径点从/map坐标系转换到/odom坐标系，生成结果放在transformed_plan。
4. (DWAPlannerROS::computeVelocityCommands)调用dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, ...)，dp_类型是DWAPlanner。
5. (DWAPlanner::updatePlanAndLocalCosts)把transformed_plan原样赋值给DWAPlanner::global_plan_。

2.1 transformGlobalPlan

根据本地代价地图，从全局路径global_plan截取一段，并将路径点从/map坐标系转换到/odom坐标系，生成结果放在transformed_plan。

• [IN]tf。用于获取坐标系变换tf。
• [IN]global_plan。makePlan生成的全局路径。
• [IN]global_pose。机器人在/odom坐标系下的当前位姿。即图1中的current_pose_。
• [IN]costmap。本地代价地图。
• [IN]global_frame。路径点要转换到的坐标系。示例“odom”。
• [OUT]transformed_plan。存储截取出的路径。

bool transformGlobalPlan(
      const tf2_ros::Buffer& tf,
      const std::vector<geometry_msgs::PoseStamped>& global_plan,
      const geometry_msgs::PoseStamped& global_pose,
      const costmap_2d::Costmap2D& costmap,
      const std::string& global_frame,
      std::vector<geometry_msgs::PoseStamped>& transformed_plan)

怎么判断路径点A是否要放入transformed_plan？——把机器人中心放到/map坐标系下，记为robot_pose。如果A和robot_pose之间路径小于1.5米（本地代价地图半径），则放入。

过程中路径点只是坐标系变换，不会发生插补。

在转换时，是不是已经走过的全局路径点，也会落在transformed_plan？——会。只要那些点和robot_pose距离小于1.5米。接下如果出现一次本地规划得不到轨迹，那MoveBase会调用makePlan，那些走过的全局路径点自然也没了。