void cb1(const geometry_msgs::PoseStamped::ConstPtr& msg) 
{
    ROS_INFO("uwb_pose x: %f", msg->pose.position.x);
}

void cb2(const geometry_msgs::PoseStamped::ConstPtr& msg) 
{
    sleep(2);
    ROS_INFO("yolo_pose x: %f", msg->pose.position.x);
}

int main(int argc, char **argv) 
{
    ros::init(argc, argv, "node");
    ros::NodeHandle nh;
    setlocale(LC_ALL, "");

    ros::Subscriber uwbSub = nh.subscribe<geometry_msgs::PoseStamped>("uwb_pose", 1, cb1);
    ros::Subscriber yoloSub = nh.subscribe<geometry_msgs::PoseStamped>("yolo_pose", 1, cb2);
    ros::spin();
    return 1;
}

ros::AsyncSpinner spinner(2);
spinner.start();
ros::waitForShutdown();

std::vector<Observation>  observations;
// get the marking observations
getMarkingObservations(observations);

// place the new obstacles into a priority queue
// each with a priority of zero to begin with
for (std::vector<Observation>::const_iterator it = observations.begin(); 
      it != observations.end();     ++it)
{
    const Observation& obs = *it;
    const pcl::PointCloud<pcl::PointXYZ>&  cloud = *(obs.cloud_);
    // obstacle_range_ 其实就是参数 obstacle_range
    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
    // 遍历点云中的每一个点
    for (unsigned int i = 0; i < cloud.points.size(); ++i)
    {
      double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
      // if the obstacle is too high or too far away from the robot we won't add it
      if (pz > max_obstacle_height_)
      {
        ROS_DEBUG("The point is too high");
        continue;
      }
      // compute the squared distance from the hitpoint to the pointcloud's origin
      double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
          + (pz - obs.origin_.z) * (pz - obs.origin_.z);

      // if the point is far enough away... we won't consider it
      if (sq_dist >= sq_obstacle_range)
      {
        ROS_DEBUG("The point is too far away");
        continue;
      }
      // now we need to compute the map coordinates for the observation
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my))
      {
        ROS_DEBUG("Computing map coords failed");
        continue;
      }
      unsigned int index = getIndex(mx, my);
      costmap_[index] = LETHAL_OBSTACLE;
      touch(px, py, min_x, min_y, max_x, max_y);
    }
  }
  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);

void Costmap2DROS::mapUpdateLoop(double frequency)
{
  if (frequency == 0.0)		return;
  ros::NodeHandle  nh;
  ros::Rate  r(frequency);
  while (nh.ok() && !map_update_thread_shutdown_)
  {
    struct timeval start, end;
    double start_t, end_t, t_diff;
    gettimeofday(&start, NULL);

    updateMap();
    // 目前的时间用tv 结构体返回
    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_DEBUG("Map update time: %.9f", t_diff);
    if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
    {
      unsigned int x0, y0, xn, yn;
      layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
       // publisher_ 是 Costmap2DPublisher
      publisher_->updateBounds(x0, xn, y0, yn);

      ros::Time now = ros::Time::now();
      if (last_publish_ + publish_cycle < now)
      {
        publisher_->publishCostmap();
        last_publish_ = now;
      }
    }
    r.sleep();
    // make sure to sleep for the remainder of our cycle time
    if (r.cycleTime() > ros::Duration(1 / frequency))
      ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,  r.cycleTime().toSec());
  }
}

void Costmap2DROS::updateMap()
{
  if (!stop_updates_)
  {
	tf::Stamped < tf::Pose > pose;
	//得到global_frame_ 与 robot_base_frame_ tf
	// 转换信息中的 translation 坐标信息
    if (getRobotPose (pose))
    {
      //获取当前机器人位置，必须要弄清楚这个x，y 代表什么意思
      double x = pose.getOrigin().x(),
      y = pose.getOrigin().y(),
      yaw = tf::getYaw(pose.getRotation());
 
		/*调用LayeredCostmap 类中的updateMap 更新边界和cost值
		* 先依据各层的更新情况，判断地图更新过的范围的边界。
		* 然后用初始值重置全局地图更新边界范围内的地图信息，
		* 并用各层的信息在更新边界内部更新地图信息
		*/
      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;
    }
  }
}

bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
  //local_costmap和global_costmap的robot_base_frame_都是/base_footprint
  robot_pose.frame_id_ = robot_base_frame_;
  try
  {
    /*
    *如果是local_costmap,global_frame_是/map
    * 如果是global_costmap 是/odom_combined
    * 然后根据tf信息，转换的到global_pose，即机器人当前位置 
    *得到global_frame_ 与robot_base_frame_ tf转换信息中的 translation 坐标信息
    */
    tf_.transformPose(global_frame_, robot_pose, global_pose);
  }
  return true;
}

void ObstacleLayer::updateBounds(double robot_x, double robot_y, 
              double robot_yaw, double* min_x, double* min_y, 
              double* max_x,  double* max_y)
{
  if (rolling_window_)
    updateOrigin(robot_x - getSizeInMetersX() / 2, 
    			 robot_y - getSizeInMetersY() / 2);
  if (!enabled_)   return;
  useExtraBounds(min_x, min_y, max_x, max_y);

  bool current = true;
  std::vector<Observation> observations, clearing_observations;

  // get the marking observations
  current = current && getMarkingObservations(observations);

  // get the clearing observations
  current = current && getClearingObservations(clearing_observations);

  // update the global current status
  current_ = current;

  // raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
     raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }

for (size_t i = 0; i < message.ranges.size(); i++)
{
  float range = message.ranges[ i ];
  if ( (!std::isfinite(range) && range > 0)  )
  {
    message.ranges[ i ] = message.range_max - epsilon;
  }
}

for (unsigned int i = 0; i < cloud.points.size(); ++i)
{
    double wx = cloud.points[i].x;
    double wy = cloud.points[i].y;

    //ROS_INFO("laser scan wx = %.2f, wy = %.2f", wx, wy);
    //在检测到的点周围生成6x6的点，
    double inflate_dx = 0.01, inflate_dy = 0.01; //在原来点的位置膨胀的尺度
    std::vector< std::pair<double,double> > inflate_pts;
    inflate_pts.push_back(std::make_pair(wx +    0      , wy +     0     ));
    inflate_pts.push_back(std::make_pair(wx -    0      , wy - inflate_dy));
    inflate_pts.push_back(std::make_pair(wx - inflate_dx, wy -     0     ));
    inflate_pts.push_back(std::make_pair(wx + 0         , wy + inflate_dy));
    inflate_pts.push_back(std::make_pair(wx + inflate_dx, wy +     0      ));
    inflate_pts.push_back(std::make_pair(wx -    0        , wy - 2*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx - 2*inflate_dx, wy -     0     ));
    inflate_pts.push_back(std::make_pair(wx +    0        , wy + 2*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx + 2*inflate_dx, wy +     0      ));
    inflate_pts.push_back(std::make_pair(wx -    0        , wy - 3*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx - 3*inflate_dx, wy -     0     ));
    inflate_pts.push_back(std::make_pair(wx +    0        , wy + 3*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx + 3*inflate_dx, wy +     0      ));  

    // 实质上增加一个循环
    std::vector< std::pair<double,double> >::iterator inflate_iter;
    for(inflate_iter = inflate_pts.begin(); inflate_iter != inflate_pts.end(); inflate_iter++)
    {
      wx = (*inflate_iter).first;
      wy = (*inflate_iter).second;
            ......
      MarkCell marker(costmap_, FREE_SPACE);
      // and finally... we can execute our trace to clear obstacles along that line
      //最终raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 
      //会将所有在(x0,y0)>>(x1,y1)之间的所有cell标记为FREE_SPACE。
      raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
      //而updateRaytraceBounds 会根据测量的距离，更新扩张（min_x, min_y, max_x, max_y）。
      updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
  }
}

if(!planner_->makePlan(start, goal, plan) || plan.empty())
{
  ROS_DEBUG_NAMED("move_base","Failed to find a  plan to point (%.2f, %.2f)", 
    goal.pose.position.x, goal.pose.position.y);

  failed_num++;
  if(failed_num > min_clear_ )
  {
    controller_costmap_ros_->resetLayers();
    planner_costmap_ros_->resetLayers();
    ROS_INFO("\033[44;37m clear costmaps done ! \033[0m");
  }
  return false;
}