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

std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
{
  std::string full_param_name;
  std::string full_radius_param_name;
  std::vector<geometry_msgs::Point> points;
  // 优先读footprint参数的值
  if (nh.searchParam("footprint",  full_param_name))
  {
    XmlRpc::XmlRpcValue footprint_xmlrpc;
    nh.getParam(full_param_name,  footprint_xmlrpc);
    if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
        footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
    {
      if (makeFootprintFromString(std::string(footprint_xmlrpc),  points))
      {
        writeFootprintToParam(nh,  points);
        return points;
      }
    }
    // 一般是这个, 因为我们定义的是数组
    else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
      points = makeFootprintFromXMLRPC(footprint_xmlrpc,  full_param_name);
      writeFootprintToParam(nh,  points);
      return points;
    }
  }
  // 没有 footprint再读robot_radius
  if (nh.searchParam("robot_radius",  full_radius_param_name))
  {
    double robot_radius;
    nh.param(full_radius_param_name,  robot_radius,  1.234);
    points = makeFootprintFromRadius(robot_radius);
    nh.setParam("robot_radius",  robot_radius);
  }
  // Else neither param was found anywhere this knows about,  so
  // defaults will come from dynamic_reconfigure stuff,  set in
  // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
  return points;
}

std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
{
  std::vector<geometry_msgs::Point> points;
  // Loop over 16 angles around a circle making a point each time
  int N = 16;
  geometry_msgs::Point pt;
  for (int i = 0; i < N; ++i)
  {
    double angle = i * 2 * M_PI / N;
    pt.x = cos(angle) * radius;
    pt.y = sin(angle) * radius;

    points.push_back(pt);
  }
  return points;
}

void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
{
  unpadded_footprint_ = points;
  padded_footprint_ = points;
  padFootprint(padded_footprint_,  footprint_padding_);

  layered_costmap_->setFootprint(padded_footprint_);
}

void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec)
{
  footprint_ = footprint_spec;
  costmap_2d::calculateMinAndMaxDistances(footprint_spec,  inscribed_radius_,  circumscribed_radius_);

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); 
    plugin != plugins_.end();  ++plugin)
  {
    (*plugin)->onFootprintChanged();
  }
}

void Costmap2DROS::updateMap()
{
  if (!stop_updates_)
  {
    // get pose in global frame of costmap
    tf::Stamped < tf::Pose > pose;
    if (getRobotPose (pose))
    {
      double x = pose.getOrigin().x(), 
             y = pose.getOrigin().y(), 
             yaw = tf::getYaw(pose.getRotation());

      layered_costmap_->updateMap(x,  y,  yaw);

      geometry_msgs::PolygonStamped footprint;
      footprint.header.frame_id = global_frame_;
      footprint.header.stamp = ros::Time::now();
      transformFootprint(x,  y,  yaw,  padded_footprint_,  footprint);
      footprint_pub_.publish(footprint);

      initialized_ = true;
    }
  }
}

private_nh.param(topic_param, topic, std::string("footprint"));

footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon,  this);

private_nh.param("footprint_radius", footprint_radius_topic_, std::string("footprint_radius"));

radius_sub_ = private_nh.subscribe(footprint_radius_topic_, 1, &Costmap2DROS::setUnpaddedRobotFootprintRadius,  this);

/*  作用：计算点到线段的距离
*  参数：pX, pY 是中心点坐标
*  x0, y0, x1, y2 线段两端的坐标, 求点到线段的距离
*  用向量点乘 来计算
*/
double distanceToLine(double pX,  double pY,  double x0,  double y0,  double x1,  double y1)