停止：LatchedStopRotateController

• 只有位置、角度、当前速度，三个条件都满足的情况下，导航才算到达了目标goal。
• 在次序上，是先让满足位置，然后角度和当前速度。rose_ros当前速度总是满足的，因而只要考虑位置、角度。
• 满足位置后，机器人为满足角度需要继续移动，这些移动可能导致机器人不再满足位置要求，这时latch_xy_goal_tolerance=true可让不再检查位置。
• 如何旋转至目标角度？那自然是以一个特定速度移动机器人。这个速度不再使用候选速度那套逻辑，线速度固定(0, 0)，角速度则可以认为是机器人当前角度和目标角速差v_theta_samp。形成(0, 0, v_theta_samp)这速度后，用DWAPlanner::checkTrajectory走出一条轨迹，如果这条轨迹能过所有准则，即cost>=0，那不管cost是何时，就把(0, 0, v_theta_samp)发向机器人。

DWAPlannerROS的停止处理逻辑，由LatchedStopRotateController类完成，主要包括三个部分：停止判断、加速停止、旋转至目标角度。

一、停止判断

在move_base的每个控制周期，都会利用DWAPlannerROS::isGoalReached（内部利用LatchedStopRotateController::isGoalReached函数实现）判断是否已经到达了目标点（即位置在xy_goal_tolerance以内，角度在yaw_goal_tolerance以内，且速度小于trans_stopped_velocity，rot_stopped_velocity）。

LatchedStopRotateController::isGoalReached只比较，不执行发送速度这种会改变机器人的操作。

bool LatchedStopRotateController::isGoalReached(LocalPlannerUtil* planner_util,
    OdometryHelperRos& odom_helper,
    const geometry_msgs::PoseStamped& global_pose) {
  double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;
  double theta_stopped_vel = planner_util->getCurrentLimits().theta_stopped_vel;
  double trans_stopped_vel = planner_util->getCurrentLimits().trans_stopped_vel;

  //copy over the odometry information
  nav_msgs::Odometry base_odom;
  odom_helper.getOdom(base_odom);

  //we assume the global goal is the last point in the global plan
  geometry_msgs::PoseStamped goal_pose;
  if ( ! planner_util->getGoal(goal_pose)) {
    return false;
  }

global_pose。机器人当前位姿。DWAPlannerROS运行在本地代价地图，它的global坐标系是odom，global_pose就是base_footprint-->odom的tf。图1示例中机器人待在起始点没动过，所以global_pose值很小。

goal_pose。目标在odom坐标系下的值。为理解这值对照下图1中的planner_util->global_plan_。global_plan_存储着map坐标系下的全局路径，路径最后个点[99]就是目标点，把它变换到odom坐标系下就是goal_pose。示例中map、odom重合，所以goal_pose和global_plan_[99]坐标值相等。

base_odom。机器人当前线速度、角速度。图1是不接收里程计场景，它总是0。意味着后面base_local_planner::stopped(...)总返回true。

  double goal_x = goal_pose.pose.position.x;
  double goal_y = goal_pose.pose.position.y;

  base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();

  //check to see if we've reached the goal position
  if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
      base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
    //if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
    //just rotate in place
    if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_) {
      ROS_DEBUG("Goal position reached (check), stopping and turning in place");
      xy_tolerance_latch_ = true;
    }

根据配置变量latch_xy_goal_tolerance_，分为锁存位置和不锁存位置两种处理逻辑。锁存位置(true)，即如果机器人在行驶过程中出现过位置满足xy_goal_tolerance的条件时，则设置xy_tolerance_latch_为true，代表已经达到过目标位置，后者目标判断定不用再考虑最终位置分量，只旋转至目标角度即可。

如果不锁存(false)，则必须始终满足机器人当前位置是否满足xy_goal_tolerance的条件，满足则代表到达了目标位置。

    double goal_th = tf2::getYaw(goal_pose.pose.orientation);
    double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
    //check to see if the goal orientation has been reached
    if (fabs(angle) <= limits.yaw_goal_tolerance) {
      //make sure that we're actually stopped before returning success
      if (base_local_planner::stopped(base_odom, theta_stopped_vel, trans_stopped_vel)) {

在到达目标位置的前提下，还要判断机器人角度是否满足目标角度yaw_goal_tolerance的需求，如果也满足，则判断当前速度是否满足停止条件，即theta速度小于theta_stopped_vel，x和y的速度小于trans_stopped_velocity。

只有位置、角度、速度，三个条件都满足的情况下，才算机器人到达了目标位姿，isGoalReached函数才会返回true。

        return true;
      }
    }
  }
  return false;
}

LatchedStopRotateController中另外有一个到达判断：isPositionReached，它就是让isGoalReached只判断位置。

二、到达位置后，如何减速、对齐角度

在DWAPlannerROS::computeVelocityCommands的每个计算周期内，都会先利用LatchedStopRotateController::isPositionReached函数判断是否已经到达目标位置，如果是，则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标角度的速度指令，否则才会使用DWAPlanner计算最优轨迹对应的速度命令。

对于isPositionReached的判断，和isGoalReached中判断达到位置的逻辑一样，即如果启动了锁存，且xy_tolerance_latch_标志位被标记（即满足过xy_goal_tolerance条件），则认为已经到达；如果未启用锁存，则需要基于当前位置是否满足xy_goal_tolerance确定是否到达了位置。

能执行到computeVelocityCommandsStopRotate，意味着机器人已到达指定位置，但可能不满足两个条件，一是机器人速度可能很快，快到不满足停止时速度；二是机器人角度和目标偏离太大。

bool LatchedStopRotateController::computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel, ...) {
  ...
  //check to see if the goal orientation has been reached
  double goal_th = tf2::getYaw(goal_pose.pose.orientation);
  double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
  if (fabs(angle) <= limits.yaw_goal_tolerance) {

目前机器人角度已满足要求，认为导航已成功。此种情况下，无视目前机器人速度是否符合条件，这种做法看去是有点粗略，但对常见的一周期速度就近似降到0场景，没啥部问题，反而逻辑上简单了。

    //set the velocity command to zero
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0.0;
    rotating_to_goal_ = false;
  } else {
    ...

    //if we're not stopped yet... we want to stop... taking into account the acceleration limits of the robot
    if ( ! rotating_to_goal_ && !base_local_planner::stopped(base_odom, limits.theta_stopped_vel, 
        limits.trans_stopped_vel)) {

目前机器人角度不满足要求，而且机器人速度很快，需要先把速度降下来，再考虑如何对正角度。stopWithAccLimits作用是计算出一个尽快能让机器人把当前速度变为0的速度，至于如何对齐目标角度，这不是它考虑的。

      if ( ! stopWithAccLimits(...)) {
        ROS_INFO("Error when stopping.");
        return false;
      }
      ROS_DEBUG("Stopping...");
    }
    //if we're stopped... then we want to rotate to goal
    else {

目前机器人角度不满足要求，但机器人速度是符合要求了，是该考虑如何对正角度。rotateToGoal作用是计算出一个尽快能让机器人对准目标角度的速度，这个速度线程度是0，只有角速度有值。

把rotating_to_goal_设为true，表示机器人进入到达位置后对齐角度过程，在这个过程会不执行操作，像上面的降速。

      //set this so that we know its OK to be moving
      rotating_to_goal_ = true;
      if ( ! rotateToGoal(...)) {
        ROS_INFO("Error when rotating.");
        return false;
      }
      ROS_DEBUG("Rotating...");
    }
  }

  return true;

}

三、降低速度：stopWithAccLimits

stopWithAccLimits作用是计算出一个尽快能让机器人把当前速度变为0的速度。

bool LatchedStopRotateController::stopWithAccLimits(const geometry_msgs::PoseStamped& global_pose,
    const geometry_msgs::PoseStamped& robot_vel,
    geometry_msgs::Twist& cmd_vel,
    Eigen::Vector3f acc_lim,
    double sim_period,
    boost::function<bool (Eigen::Vector3f pos,
                          Eigen::Vector3f vel,
                          Eigen::Vector3f vel_samples)> obstacle_check) {

  //slow down with the maximum possible acceleration... 
  //we should really use the frequency that we're running at to determine what is feasible
  //but we'll use a tenth of a second to be consistent with the implementation of the local planner.
  double vx = sign(robot_vel.pose.position.x) * std::max(0.0, (fabs(robot_vel.pose.position.x) - acc_lim[0] * sim_period));
  double vy = sign(robot_vel.pose.position.y) * std::max(0.0, (fabs(robot_vel.pose.position.y) - acc_lim[1] * sim_period));

  double vel_yaw = tf2::getYaw(robot_vel.pose.orientation);
  double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim[2] * sim_period));

robot_vel.pose.position.x等于twist.twist.linear.x，robot_vel.pose.position.y等于twist.twist.linear.y，robot_vel.pose.orientation来自twist.twist.angular.z，参考OdometryHelperRos::getRobotVel。

如何计算出一个尽快能让机器人把当前速度变为0的速度，简单操作，把存放结果的cmd_vel中的线速度、角速度都设为0，就可以了。这么操作有个问题，如果目前机器人速度很快，快到以最大加速度(acc_lim)减速，到sim_period后还是到达不了0，这时为安全就须要给指定个按最大加速度算出的结束速度。vx、vy、vel_yaw正是以着这个加速度约束去计算。

负速度表示的是机器人要反方向运动，不要轻易理解为负数是个无效速度。举个示例，目前x方向线程度(position.x)是-0.6，(acc_lim[0] * sim_period)是0.25，那算出的结果速度是-0.35，即在下一刻，在x方向，希望机器人以着0.35米/秒速度反方向运行。

  //we do want to check whether or not the command is valid
  double yaw = tf2::getYaw(global_pose.pose.orientation);
  bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.pose.position.x, global_pose.pose.position.y, yaw),
                                  Eigen::Vector3f(robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw),
                                  Eigen::Vector3f(vx, vy, vth));

obstacle_check指向的操作是DWAPlanner::checkTrajectory。它试图以当前机器人位姿、当前速度，判断在地图上，是否能以速度(vx, vy, vth)走出一条路径。能走出一条路径(cost > 0)，或目前已基本接近goal(cost == 0)，则返回true。

  //if we have a valid command, we'll pass it on, otherwise we'll command all zeros
  if(valid_cmd){
    ROS_DEBUG_NAMED("latched_stop_rotate", "Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
    cmd_vel.linear.x = vx;
    cmd_vel.linear.y = vy;
    cmd_vel.angular.z = vth;
    return true;
  }
  ROS_WARN("Stopping cmd in collision");
  cmd_vel.linear.x = 0.0;
  cmd_vel.linear.y = 0.0;
  cmd_vel.angular.z = 0.0;
  return false;
}

四、旋转至目标角度：rotateToGoal

rotateToGoal负责计算要让机器人旋转至目标角度，该运行什么速度。首先将x和y速度设置为0，对于theta速度，利用当前角度与目标角度的差的比例控制（系数为1）产生，然在三个方面进行微调，让它是目前机器人状态下可到达的速度。接着利用DWAPlanner::checkTrajectory进行轨迹打分，验证这速度是否可行。

向上返回的cmd_vel，其线速度一定是0，角速度在失败时是0，成功时v_theta_samp。

bool LatchedStopRotateController::rotateToGoal(
    const geometry_msgs::PoseStamped& global_pose,
    const geometry_msgs::PoseStamped& robot_vel,
    double goal_th,
    geometry_msgs::Twist& cmd_vel,
    Eigen::Vector3f acc_lim,
    double sim_period,
    base_local_planner::LocalPlannerLimits& limits,
    boost::function<bool (Eigen::Vector3f pos,
                          Eigen::Vector3f vel,
                          Eigen::Vector3f vel_samples)> obstacle_check) {

• @global_pose。机器人当前位姿。DWAPlannerROS运行在本地代价地图，它的全局坐标系是odom，global_pose就是base_footprint-->odom的tf。
• @robot_vel。机器人当前速度。
• @goal_th。在odom坐标系下，goal要求的角度。
• @cmd_vel[输出参数]。为实现按goal_th，希望接下机器人运行的速度。只有返回值true时有效。
• @acc_lim。planner_util_.getCurrentLimits().getAccLimits()。实例：(1.25, 0, 5)。
• @sim_period。dp_->getSimPeriod()。实例：0.2秒。
• @limits。planner_util->getCurrentLimits()。用了当中两个字段，min_vel_theta(最小角速度)、max_vel_theta(最大角速度)。实例：(-1, 1)。
• @obstacle_check。一个可执行验证障碍的操作。

  double yaw = tf2::getYaw(global_pose.pose.orientation);
  double vel_yaw = tf2::getYaw(robot_vel.pose.orientation);
  cmd_vel.linear.x = 0;
  cmd_vel.linear.y = 0;
  double ang_diff = angles::shortest_angular_distance(yaw, goal_th);

  double v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_vel_theta, fabs(ang_diff)));

  //take the acceleration limits of the robot into account
  double max_acc_vel = fabs(vel_yaw) + acc_lim[2] * sim_period;
  double min_acc_vel = fabs(vel_yaw) - acc_lim[2] * sim_period;

  v_theta_samp = std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);

  //we also want to make sure to send a velocity that allows us to stop 
  //when we reach the goal given our acceleration limits
  double max_speed_to_stop = sqrt(2 * acc_lim[2] * fabs(ang_diff));
  v_theta_samp = std::min(max_speed_to_stop, fabs(v_theta_samp));

  v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_vel_theta, v_theta_samp));

如何能让机器人角度旋转至目标角度，简单的话，让以两个角度的差值(ang_diff)作为存储结果的cmd_vel中的角速度、线速度则都设为0，就可以了。这么操作有个问题，以机器人目前状态，真能得到这角速度吗，后面就是用三种条件对这个初略算出的速度(v_theta_samp)进行修正。

• [min_vel_theta, max_vel_theta]是机器人到做到的角速度范围，最终角速度须要在这范围。
• 机器人有个初速度vel_yaw，需是以该速度经过sim_period秒后能到达的速度范围[min_acc_vel, max_acc_vel]。
• 假设机器人做初速度为0的匀加速运动，为转过ang_diff弧度，末速度不能超过max_speed_to_stop。下面有说如何得出末速度公式： 。

  if (ang_diff < 0) {
    v_theta_samp = - v_theta_samp;
  }

  //we still want to lay down the footprint of the robot and check if the action is legal
  bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.pose.position.x, global_pose.pose.position.y, yaw),
      Eigen::Vector3f(robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw),
      Eigen::Vector3f( 0.0, 0.0, v_theta_samp));

obstacle_check指向的操作是DWAPlanner::checkTrajectory。它试图以当前机器人位姿、当前速度，判断在地图上，是否能以速度v_theta_samp走出一条路径。能走出一条路径(cost > 0)，或目前已基本接近goal(cost == 0)，则返回true。

  if (valid_cmd) {
    cmd_vel.angular.z = v_theta_samp;
    return true;
  }
  ROS_WARN("Rotation cmd in collision");
  cmd_vel.angular.z = 0.0;
  return false;
}

公式1：

公式2：，把做为应变量，可得到：  

假设机器人做初速度为0的匀加速运动，即为0，将公式2替换掉公式1中的，就可得到。