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;
    }
    //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);
}

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;

bool DWAPlannerROS::dwaComputeVelocityCommands(
	tf::Stamped<tf::Pose> &global_pose, 
	geometry_msgs::Twist& cmd_vel )
{
    // dynamic window sampling approach to get useful velocity commands
    if(! isInitialized()  )
    {
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }

    tf::Stamped<tf::Pose> robot_vel;
    odom_helper_.getRobotVel(robot_vel);

    /* For timing uncomment
    struct timeval start, end;
    double start_t, end_t, t_diff;
    gettimeofday(&start, NULL);
    */

    //compute what trajectory to drive along
    tf::Stamped<tf::Pose> drive_cmds;
    drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
    
    // call with updated footprint
    base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds, costmap_ros_->getRobotFootprint());
    //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);

    /* For timing uncomment
    gettimeofday(&end, NULL);
    start_t = start.tv_sec + double(start.tv_usec) / 1e6;
    end_t = end.tv_sec + double(end.tv_usec) / 1e6;
    t_diff = end_t - start_t;
    ROS_INFO("Cycle time: %.9f", t_diff);
    */

    //pass along drive commands
    cmd_vel.linear.x = drive_cmds.getOrigin().getX();
    cmd_vel.linear.y = drive_cmds.getOrigin().getY();
    cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());

    //if we cannot move... tell someone
    std::vector<geometry_msgs::PoseStamped> local_plan;
    if(path.cost_ < 0)
    {
      ROS_DEBUG_NAMED("dwa_local_planner",
          "The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
      local_plan.clear();
      publishLocalPlan(local_plan);
      return false;
    }
    ROS_DEBUG_NAMED("dwa_local_planner", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.", 
                    cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);

    // Fill out the local plan
    for(unsigned int i = 0; i < path.getPointsSize(); ++i)
    {
      double p_x, p_y, p_th;
      path.getPoint(i, p_x, p_y, p_th);

      tf::Stamped<tf::Pose> p =
              tf::Stamped<tf::Pose>(tf::Pose(
                      tf::createQuaternionFromYaw(p_th),
                      tf::Point(p_x, p_y, 0.0)),
                      ros::Time::now(),
                      costmap_ros_->getGlobalFrameID());
      geometry_msgs::PoseStamped pose;
      tf::poseStampedTFToMsg(p, pose);
      local_plan.push_back(pose);
    }

    //publish information to the visualizer
    publishLocalPlan(local_plan);
    return true;
}

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)  );
}

/**
 * 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;
}

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;
  }

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;
}