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

ros::NodeHandle private_nh("~/" + name);
g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
tf_ = tf;
costmap_ros_ = costmap_ros;
// 在局部代价地图中的位姿
costmap_ros_->getRobotPose(current_pose_);

// 局部代价地图
costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
// base_local_planner::LocalPlannerUtil
// 其实就是把三个参数赋值给 LocalPlannerUtil 的三个成员变量
planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());

//这里创建了dwa的共享指针，也就是  boost::shared_ptr<DWAPlanner> dp_;
dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));

if( private_nh.getParam( "odom_topic", odom_topic_ ))
{
  odom_helper_.setOdomTopic( odom_topic_ );
}

initialized_ = true;

// Warn about deprecated parameters -- remove this block in N-turtle
// 这些不必关注
nav_core::warnRenamedParameter(private_nh, "max_vel_trans", "max_trans_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_trans", "min_trans_vel");
nav_core::warnRenamedParameter(private_nh, "max_vel_theta", "max_rot_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_theta", "min_rot_vel");
nav_core::warnRenamedParameter(private_nh, "acc_lim_trans", "acc_limit_trans");
nav_core::warnRenamedParameter(private_nh, "theta_stopped_vel", "rot_stopped_vel");
// 动态调整
dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);

// 开始规划时再次获取位姿
if ( !costmap_ros_->getRobotPose(current_pose_))
{
  ROS_ERROR("Could not get robot pose");
  return false;
}

// TEB的部分全局路径也叫 transformed_plan，应该是因为下面的getLocalPlan参数也是这个名字
std::vector<geometry_msgs::PoseStamped> transformed_plan;
// 获取全局路径
if ( !planner_util_.getLocalPlan(current_pose_, transformed_plan) )
{
  ROS_ERROR("Could not get local plan");
  return false;
}

if(transformed_plan.empty())
{
  ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
  return false;
}
ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());

base_local_planner::transformGlobalPlan(
      *tf_,
      global_plan_,
      global_pose,
      *costmap_,
      global_frame_,
      transformed_plan)

if(limits_.prune_plan)   
{
    base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
}

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

void DWAPlanner::updatePlanAndLocalCosts(
      const geometry_msgs::PoseStamped&  global_pose,
      const std::vector<geometry_msgs::PoseStamped>&  new_plan,
      const std::vector<geometry_msgs::Point>& footprint_spec)
{
    global_plan_.resize(new_plan.size());
    for (unsigned int i = 0; i < new_plan.size(); ++i)
    {
      global_plan_[i] = new_plan[i];
    }

    obstacle_costs_.setFootprint(footprint_spec);

    // costs for going away from path
    path_costs_.setTargetPoses(global_plan_);

    // costs for not going towards the local goal as much as possible
    goal_costs_.setTargetPoses(global_plan_);

    // alignment costs
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();

    Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
    double sq_dist =
        (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) 
                                             +
        (pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);

    // we want the robot nose to be drawn to its final position
    // (before robot turns towards goal orientation), not the end of the
    // path for the robot center. Choosing the final position after
    // turning towards goal orientation causes instability when the
    // robot needs to make a 180 degree turn at the end
    std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
    double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);

    front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
      forward_point_distance_ * cos(angle_to_goal);
    front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
      sin(angle_to_goal);

    goal_front_costs_.setTargetPoses(front_global_plan);
    
   // keeping the nose on the path
  if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_)
  {
    alignment_costs_.setScale(path_distance_bias_);
    // costs for robot being aligned with path (nose on path, not ju
    alignment_costs_.setTargetPoses(global_plan_);
  }
  else
  {
    // once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
    alignment_costs_.setScale(0.0);
  }
}

Vector3f v1 = Vector3f::UnitX();

AngleAxisf aa(45*3.1415/180, v1);
// 返回弧度制表示的旋转角度 和 旋转轴(列向量)
cout << aa.angle() <<"  "<<endl<< aa.axis() <<endl; 
cout << aa.toRotationMatrix()<<endl;   // 转换为3 × 3的旋转矩阵

// fromRotationMatrix():从一个3×3的旋转矩阵构建AngleAxis对象。也就是提取旋转矩阵所表示的旋转角
//  inverse(): 返回以AngleAxis表示的当前旋转的逆旋转

Eigen::Quaterniond q1(x, y, z, w);
 
Eigen::Quaterniond q2(Vector4d(x, y, z, w));
 
Eigen::Quaterniond q2(Matrix3d(R));

// 参数顺序和 static_transform_publisher 的一致
Eigen::Vector3f angle(roll, pitch, yaw);
Eigen::Matrix3d rotation;
rotation = Eigen::AngleAxisd(angle[0], Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(angle[1], Eigen::Vector3d::UnitY()) *
           Eigen::AngleAxisd(angle[2], Eigen::Vector3d::UnitX()) ;

Eigen::Quaterniond quaternion(transform.transform.rotation.w,
                              transform.transform.rotation.x,
                              transform.transform.rotation.y,
                              transform.transform.rotation.z
                              );
Eigen::Matrix3d rotation = quaternion.matrix();

Translation<float,2>(tx, ty)
Translation<float,3>(tx, ty, tz)
Translation<float,N>(s)
Translation<float,N>(vecN)

Translation2d t1(1,4);
Translation2d t2(2,7);
Translation2d t3;
t3=t1*t2;
cout<<t3.translation()<<endl;

#include <Eigen/Eigenvalues>

A << 2,-2,0, -2,1,-2, 0,-2,0;
EigenSolver<Matrix3d> eigensolver(A);
if (eigensolver.info() == Success)
{
    cout << eigensolver.eigenvalues() << endl<<endl;
    cout << eigensolver.eigenvectors() << endl;
}