DWA规划路径时，机器人到了位置误差范围内会开始转动，直到满足角度误差。但是如果两个误差设置太小，很可能在转动过程中机器人又不能满足位置误差了，这样就又进入局部规划了，这种现象被称为dance，也就是当机器人接近目标点时，会在附近左右摆来摆去的。这显然不是我们想要的。

latch_xy_goal_tolerance机制是确保机器人一旦到达目标的position就停止，默认是关闭的。有些人反复设置总是不起作用，原因其实是LatchedStopRotateController的bug，它的构造函数中的latch_xy_goal_tolerance参数设置为false，即使在yaml中设置为了true，参数服务器里也是true，用日志在源码里打印会发现变量latch_xy_goal_tolerance_还是false，只能去构造函数里修改。

但是latched机制不能一直打开，否则又会给全局路径造成问题。因为接收到新目标时，不能再latch机器人了，move_base将reset latching. 但是全局路径在机器人到达目标前是不会关闭的，结果会出现：第一次latch机制是正常的，然后再发导航目标，机器人会依然不动。

解决的方法是在DWAPlannerROS::setPlan里，在reset latch之前检查全局路径的目标是否改变，若没有改变就不要reset latch。 更省事的方法是到move_base里把全局规划频率planner_frequency设置为0，这样全局规划只有在接收到新目标或局部规划失败时才进行规划，看各自需求了。

bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
{
    if (! isInitialized()) {
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }

    // zzp add in 9.17
    //when we get a new plan, we also want to clear any latch we may have on goal tolerances
    geometry_msgs::PoseStamped current_global_goal = orig_global_plan.back();

    if (first_goal_)
    {
      latchedStopRotateController_.resetLatching();
      previous_global_goal_ = current_global_goal;
      first_goal_ = false;
    }
    else
    {
      double dist = distance(current_global_goal, previous_global_goal_);
      if (dist > 0.2) {
        latchedStopRotateController_.resetLatching();
      }
      previous_global_goal_ = current_global_goal;
    }

    //when we get a new plan, we also want to clear any latch we may have on goal tolerances
    //latchedStopRotateController_.resetLatching();

    ROS_INFO("Got new plan");
    return dp_->setPlan(orig_global_plan);
}