代码流程比较复杂: Costmap2DROS::mapUpdateLoop —— Costmap2DROS::updateMap() —— LayeredCostmap::updateMap—— 每一层的 updateBounds

mapUpdateLoop

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

updateMap

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

这里就只看障碍层的updateBounds函数。这个函数主要完成 clearing, marking以及确定bound。和静态地图类似，同样也是先判断是否是rolling地图，若是则更新地图原点。

点云数据最终传到const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_); 点云z坐标在计算sq_dist之后就不处理了。

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

清除和添加障碍在后面的文章继续