// 局部的子目标 (transformed plan最后一个元素)
tf::Stamped<tf::Pose> goal_point;
tf::poseStampedMsgToTF(transformed_plan.back(), goal_point);
robot_goal_.x() = goal_point.getOrigin().getX();
robot_goal_.y() = goal_point.getOrigin().getY();

if (cfg_.trajectory.global_plan_overwrite_orientation)
{
    robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point, 
       goal_idx,  tf_plan_to_global);
  // overwrite/update goal orientation of the transformed plan with the actual goal
   // (enable using the plan as initialization)
  transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta());
}
else
    robot_goal_.theta() = tf::getYaw(goal_point.getRotation());

// plan only contains the goal, 一般在导航的最后出现size是1的情况
// 用实际的机器人位姿更新 transformed plan 的起点(allows using the plan as initial trajectory)
if (transformed_plan.size()==1) 
{    // 插入起点，但不初始化
  transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped() );
}
tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // 把起点更新为 机器人当前位姿

int main()
{
  VectorXf v(2);
  MatrixXf m(2,2), n(2,2);
  
  v << -1,
       2;
  
  m << 1,-2,
       -3,4;
  cout << "v.squaredNorm() = " << v.squaredNorm() << endl;
  cout << "v.norm() = " << v.norm() << endl;
  cout << "v.lpNorm<1>() = " << v.lpNorm<1>() << endl;
  cout << "v.lpNorm<Infinity>() = " << v.lpNorm<Infinity>() << endl;
  cout << endl;
  cout << "m.squaredNorm() = " << m.squaredNorm() << endl;
  cout << "m.norm() = " << m.norm() << endl;
  cout << "m.lpNorm<1>() = " << m.lpNorm<1>() << endl;
  cout << "m.lpNorm<Infinity>() = " << m.lpNorm<Infinity>() << endl;
}

ros::Publisher cancle_pub_ = nh_.advertise("move_base/cancel",1);

actionlib_msgs::GoalID first_goal;
cancle_pub_.publish(first_goal);

class OdometryHelperRos {
public:
  OdometryHelperRos(std::string odom_topic = "")
  { 
    setOdomTopic( odom_topic ); 
  }
  ~OdometryHelperRos() {}

  void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);

  void getOdom(nav_msgs::Odometry& base_odom);
  void getRobotVel(tf::Stamped<tf::Pose>& robot_vel);

  /** @brief Set the odometry topic.  This overrides what was set in the constructor, if anything.
   *
   * This unsubscribes from the old topic (if any) and subscribes to the new one (if any).
   *
   * If odom_topic is the empty string, this just unsubscribes from the previous topic. */
  void setOdomTopic(std::string odom_topic);

  std::string getOdomTopic() const { return odom_topic_; }

private:
  std::string odom_topic_;
  // we listen on odometry on the odom topic
  ros::Subscriber odom_sub_;
  nav_msgs::Odometry base_odom_;
  boost::mutex odom_mutex_;
  // global tf frame id
  std::string frame_id_; ///< The frame_id associated this data
};

private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
tc_ = blp_loader_.createInstance(local_planner);
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);

tf::Stamped<tf::Pose> robot_vel_tf;
base_local_planner::OdometryHelperRos odom_helper_;
odom_helper_.getRobotVel(robot_vel_tf);

geometry_msgs::Twist  robot_vel_;
robot_vel_.linear.x = robot_vel_tf.getOrigin().getX();
robot_vel_.linear.y = robot_vel_tf.getOrigin().getY();
robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation());

bool stopped(const nav_msgs::Odometry& base_odom, 
      const double& rot_stopped_velocity, const double& trans_stopped_velocity)
{
    return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity 
      && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
      && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
}

//costmap_2d::Costmap2DROS*  planner_costmap_ros_
world_model_ = new base_local_planner::CostmapModel(*(planner_costmap_ros_ ->getCostmap()) ); 
std::vector<geometry_msgs::Point> footprint = planner_costmap_ros_->getRobotFootprint();
double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint);

double base_local_planner::CostmapModel::footprintCost(const geometry_msgs::Point &  position,
      const std::vector< geometry_msgs::Point > &   footprint,
      double  inscribed_radius,
      double  circumscribed_radius 
)

tf::Stamped<tf::Pose> input_pose;
input_pose.setOrigin(tf::Vector3(x, y, 0) );
input_pose.setRotation(tf::createQuaternionFromYaw(theta) );
input_pose.frame_id_ = "map";
input_pose.stamp_ = ros::Time::now();

tf::Stamped<tf::Pose> global_pose_in_local_costmap;
auto tf = context_->tf_;
try
{
    tf->transformPose( local_planner_costmap_ros_->getGlobalFrameID(), 
                  input_pose,
                  global_pose_in_local_costmap);
}
catch (tf::TransformException& ex)
{
    ROS_INFO("Cannot transform in inCollision !");
    return false;
}
double temp_x = global_pose_in_local_costmap.getOrigin().getX();
double temp_y = global_pose_in_local_costmap.getOrigin().getY();

double footprint_cost = world_model_->footprintCost(
    temp_x,
    temp_y,
    tf::getYaw(global_pose_in_local_costmap.getRotation() ),
    local_costmap_ros->getRobotFootprint() );

double CostmapModel::footprintCost(const geometry_msgs::Point& position, 
    const std::vector<geometry_msgs::Point>&  footprint,
    double inscribed_radius,  double circumscribed_radius )
{
    //used to put things into grid coordinates
    unsigned int cell_x, cell_y;
    //get the cell coord of the center point of the robot
    if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
      return -3.0;

    //省略  footprint的点少于3个，只取点位置的代价
    if(footprint.size() < 3)
    { ...... }
    // 把足迹视为多边形，循环调用 lineCost 计算多边形各边的cell，注意首尾闭合，最后返回代价
    // 对于圆形底盘，按16边形处理
    // now we really have to lay down the footprint in the costmap grid
    unsigned int x0, x1, y0, y1;
    double line_cost = 0.0;
    double footprint_cost = 0.0;
    // we need to rasterize each line in the footprint
    for(unsigned int i = 0; i < footprint.size() - 1; ++i)
    {
       //get the cell coord of the first point
      if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
        return -3.0;

      //get the cell coord of the second point
      if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
          return -3.0;

      line_cost = lineCost(x0, x1, y0, y1);
      footprint_cost = std::max(line_cost, footprint_cost);
      //if there is an obstacle that hits the line, return false
      if(line_cost < 0)
          return line_cost;
    }
    // 以下是连接 footprint 的第一个和最后一个点
    //get the cell coord of the last point
    if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
        return -3.0;

    //get the cell coord of the first point
    if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
        return -3.0;

    line_cost = lineCost(x0, x1, y0, y1);
    footprint_cost = std::max(line_cost, footprint_cost);

    if(line_cost < 0)
        return line_cost;

    // if all line costs are legal, return that footprint is legal
    return footprint_cost;
  }

// return a positive cost for a legal line 或者负值
double CostmapModel::lineCost(int x0, int x1, int y0, int y1) const
{
    double line_cost = 0.0;
    double point_cost = -1.0;
    for( LineIterator line( x0, y0, x1, y1 ); line.isValid(); line.advance() )
    {
      point_cost = pointCost( line.getX(), line.getY() );
      if(point_cost < 0)
        return point_cost;

      if(line_cost < point_cost)
        line_cost = point_cost;
    }
    return line_cost;
}

double CostmapModel::pointCost(int x, int y) const
{
    unsigned char cost = costmap_.getCost(x, y);
    //if the cell is in an obstacle the path is invalid
    if(cost == NO_INFORMATION)
      return -2;
    if(cost == LETHAL_OBSTACLE)
      return -1;

    return cost;
}