接着上一篇，继续DWAPlannerROS::computeVelocityCommands

如果x y坐标已经在目标的距离误差范围内

if (latchedStopRotateController_.isPositionReached(
      &planner_util_, current_pose_)  )
{
  // 发布空的路径，这段可以删除
  std::vector<geometry_msgs::PoseStamped> local_plan;
  std::vector<geometry_msgs::PoseStamped> transformed_plan;
  publishGlobalPlan(transformed_plan);
  publishLocalPlan(local_plan);
  // 这个类保存了所有 DWA参数，只有一个getAccLimits函数
  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，它用到一个参数latch_xy_goal_tolerance

isPositionReached

/**
 * returns true if we have passed the goal position.
 * Meaning we might have overshot on the position beyond tolerance, yet still return true.
 * Also goal orientation might not yet be true
 */
bool LatchedStopRotateController::isPositionReached(LocalPlannerUtil* planner_util,
    tf::Stamped<tf::Pose> global_pose)
{
  double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;

  // 认为 global goal 是 global plan的最后一个点
  // 显然是把全局路径的最后一个点赋值给 goal_pose
  tf::Stamped<tf::Pose> goal_pose;
  if ( ! planner_util->getGoal(goal_pose))
  {
    return false;
  }
  double goal_x = goal_pose.getOrigin().getX();
  double goal_y = goal_pose.getOrigin().getY();

  // 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)
  {
    xy_tolerance_latch_ = true;
    return true;
  }
  return false;
}

这里有两种情况可以出现 if(true)， getGoalPositionDistance不用看也知道是求 global_pose 和 goal的空间距离，再跟参数xy_goal_tolerance比较。

latch_xy_goal_tolerance_本意是在LatchedStopRotateController构造函数里获取设置的参数，但是源码在这里出现了bug，应当是命名空间导致的。在DWA参数里设置其为true没有效果，从参数服务器看到的确实是true，但是在源码里的latch_xy_goal_tolerance_还是默认的false. 可以直接修改构造函数的设置值。 剩下的关键就是变量xy_tolerance_latch_了，有一个关键函数void resetLatching() { xy_tolerance_latch_ = false; }。 如何实现这一情况，在下面函数。

注意isPositionReached只是判断，没有发布零速度，没有停止

computeVelocityCommandsStopRotate

机器人到达位置时，停止平移，只做旋转

bool LatchedStopRotateController::computeVelocityCommandsStopRotate(
	geometry_msgs::Twist& cmd_vel,
    Eigen::Vector3f acc_lim,
    double sim_period,
    LocalPlannerUtil* planner_util,
    OdometryHelperRos& odom_helper_,
    tf::Stamped<tf::Pose> global_pose,
    // obstacle_check 在调用里是 DWAPlanner::checkTrajectory
    boost::function<bool (Eigen::Vector3f pos,
                          Eigen::Vector3f vel,
                          Eigen::Vector3f vel_samples)> obstacle_check)
{
  // global goal is the last point in the global plan
  tf::Stamped<tf::Pose> goal_pose;
  if ( ! planner_util->getGoal(goal_pose) )
  {
    ROS_ERROR("Could not get goal pose");
    return false;
  }
  base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();
  // latch goal tolerance, if we ever reach the goal location, 
  // we'll  just rotate in place
  // 这里就是latched机制了，在第5篇再详细解释。这里先不考虑latch，看下面的情况。
  if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_ )
  {
    ROS_INFO("Goal position reached, stopping and turning in place");
    xy_tolerance_latch_ = true;
  }
  // 检查是否达到了 goal orientation
  double goal_th = tf::getYaw(goal_pose.getRotation());
  double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
  if (fabs(angle) <= limits.yaw_goal_tolerance)
  {
    // 到达， 停止转动
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0.0;
    rotating_to_goal_ = false;
  }

如果发布了零速度，会直接运行到最后的return true

最后看角度没有到导航误差范围的情况，比较复杂

else 
{
    tf::Stamped<tf::Pose> robot_vel;
    odom_helper_.getRobotVel(robot_vel);
    nav_msgs::Odometry base_odom;
    odom_helper_.getOdom(base_odom);
    // if we're not stopped yet... we want to stop... 
    // taking into account the acceleration limits of the robot

    /*  这里是机器人在继续运动的情况
     rotating_to_goal_  在构造函数里是 false
      stopped函数： 如果里程计的角速度小于 rot_stopped_vel，且线速度小于 
    trans_stopped_vel， 则返回 true，认为已经停止 */
    if ( ! rotating_to_goal_ && !base_local_planner::stopped(
    	 base_odom, limits.rot_stopped_vel, limits.trans_stopped_vel)  )
    {
      if ( ! stopWithAccLimits(
          global_pose,
          robot_vel,
          cmd_vel,
          acc_lim,
          sim_period,
          obstacle_check) )
      {
        ROS_INFO("Error when stopping.");
        return false;
      }
      ROS_DEBUG("Stopping...");
    }
    //if we're stopped... then we want to rotate to goal
    else
    {
      //set this so that we know its OK to be moving
      rotating_to_goal_ = true;
      if ( ! rotateToGoal(
          global_pose,
          robot_vel,
          goal_th,
          cmd_vel,
          acc_lim,
          sim_period,
          limits,
          obstacle_check)  )
      {
        ROS_INFO("Error when rotating.");
        return false;
      }
      ROS_DEBUG("Rotating...");
    }
  }
  return true;
}