Costmap_2d ROS源码解析

1. move_base:Costmap程序的执行入口

本部分，我们关心如何从move_base导航入口进入到costmap的运行程序，以及如何进行静态地图/障碍物层地图/膨胀层的处理。
代码路径：move_base/src/move_base.cpp
首先获取地图信息，内切圆半径inscribed_radius/外切圆形半径circumscribed_radius等信息。

//we'll assume the radius of the robot to be consistent with what's specified for the costmaps
  private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
  private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
  private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
  private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
  private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
  private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
  private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);

下面通过模块costmap_2d构建了全局代价地图对象，并通过其成员函数pause()暂停运行，将在必要的功能模块初始化结束之后通过成员接口start()开启。

 // 下面通过模块costmap_2d构建了全局代价地图对象，并通过其成员函数pause()暂停运行，将在必要的功能模块初始化结束之后通过成员接口start()开启。
    planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
    planner_costmap_ros_->pause();

至此，costmap准备工作完成，接下来就是初始化与维护。

2. costmap初始化和维护

大家都知道，代价地图是由不同层级的plugins逐层累加的，包括静态地图层、障碍层、膨胀层等等，那么通过LayeredCostmap的对象，创造了一个由指向plugin的共享指针组成的容器，通过配置文件，向里一一添加插件层。

layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

if (!private_nh.hasParam("plugins") )
    resetOldParameters(private_nh);

if (private_nh.hasParam("plugins") )
{
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("Using plugin \"%s\"", pname.c_str()  );

      // plugin_loader_ 类型是　pluginlib::ClassLoader<Layer>，ROS插件机制
      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
      // 添加到容器 plugins_
      layered_costmap_->addPlugin(plugin);
      // 执行 Layer::initialize，它向各层地图通知主地图的存在，并调用
      // oninitialize方法（Layer类中的虚函数，被定义在各层地图中）
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
}

Costmap2DROS调用的接口addPlugin，则是将一个指针放入容器中，仅此而已。
对车体尺寸进行建模，这个文件中均是对不同形状的车体进行外壳的位置计算，然后计算的内切圆半径和外切圆半径同步更新到各个地图layer。

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

void Costmap2DROS::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
{
  setUnpaddedRobotFootprint(toPointVector(footprint));
}

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

这段代码是用于创建一个动态重新配置服务器，用于修改名为Costmap2DConfig的配置文件。动态重新配置服务器允许在运行时修改配置参数，而不需要重启机器人。

代码解释如下：

1.dsrv_=new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));：创建一个动态重新配置服务器，用于处理Costmap2DConfig类型的配置文件。ros::NodeHandle用于创建一个节点，其中包含动态重新配置服务器的地址。name是配置文件的名称，包含在地址中。

2.dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1, _2);：定义一个回调函数类型，用于处理配置更新事件。boost::bind将回调函数绑定到reconfigureCB成员函数，并将传入的参数传递给它。_1和_2是动态重新配置服务器传递的参数，分别对应配置文件的旧值和新值。

3. dsrv_->setCallback(cb);：将回调函数设置为动态重新配置服务器的回调函数，以便在接收到新的配置更新时调用它。

总之，这段代码创建了一个动态重新配置服务器，用于处理Costmap2DConfig类型的配置文件。它还定义了一个回调函数，用于在接收到配置更新时修改配置文件。最后，将回调函数设置为动态重新配置服务器的回调函数。这在机器人导航、控制等领域非常有用，允许在不重启机器人的情况下实时修改配置。

dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
  dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);

然后通过如下函数进行配置参数初始化并创建主线程&Costmap2DROS::mapUpdateLoop。

void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)

map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));

mapUpdateLoop这个成员函数是用于不停刷新代价地图的，它在每个循环通过updateMap函数实现，而Costmap2DROS::updateMap函数是利用LayeredCostmap的updateMap函数进行更新。

void Costmap2DROS::mapUpdateLoop(double frequency)
{
  //mapUpdateLoop这个成员函数是用于不停刷新代价地图的，它在每个循环通过updateMap函数实现
  //，而Costmap2DROS::updateMap函数是利用LayeredCostmap的updateMap函数进行更新，在前面已经分析过。
  // the user might not want to run the loop every cycle
 
    updateMap();//更新地图

更新地图信息，这个函数在Costmap2DROS的地图更新线程中被循环调用。它分为两步：第一步：更新bound，即确定地图更新的范围；第二步：更新cost，更新每层地图cell对应的cost值后整合到主地图上。

void Costmap2DROS::updateMap()
{
  if (!stop_updates_)
  {
    // get global pose
    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函数，参数是机器人位姿
      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);//make coordiates to global coordiates
      footprint_pub_.publish(footprint);

      initialized_ = true;
    }
  }
}

layered_costmap_->updateMap(x, y, yaw)来自于如下代码：

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
  // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
  // implement thread unsafe updateBounds() functions.
  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));

  // if we're using a rolling buffer costmap... we need to update the origin using the robot's position
  if (rolling_window_)
  {
    double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
    double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
    costmap_.updateOrigin(new_origin_x, new_origin_y);
  }

  if (plugins_.size() == 0)
    return;

  minx_ = miny_ = 1e30;
  maxx_ = maxy_ = -1e30;

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    double prev_minx = minx_;
    double prev_miny = miny_;
    double prev_maxx = maxx_;
    double prev_maxy = maxy_;
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
    {
      ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
                        "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
                        prev_minx, prev_miny, prev_maxx , prev_maxy,
                        minx_, miny_, maxx_ , maxy_,
                        (*plugin)->getName().c_str());
    }
  }

  int x0, xn, y0, yn;
  costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
  costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);

  x0 = std::max(0, x0);
  xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);

  ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);

  if (xn < x0 || yn < y0)
    return;

  costmap_.resetMap(x0, y0, xn, yn);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
  }

  bx0_ = x0;
  bxn_ = xn;
  by0_ = y0;
  byn_ = yn;

  initialized_ = true;
}

rolling_window_默认为false，如果开启的话，地图是时刻跟随机器人中心移动的，这里需要根据机器人当前位置和地图大小计算出地图的新原点，设置给主地图。

// if we're using a rolling buffer costmap... we need to update the origin using the robot's position
  //rolling_window_默认为false，如果开启的话，地图是时刻跟随机器人中心移动的，
  //这里需要根据机器人当前位置和地图大小计算出地图的新原点，设置给主地图。
  if (rolling_window_)
  {
    double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
    double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
    costmap_.updateOrigin(new_origin_x, new_origin_y);
  }

设置好minx_、miny_、maxx_、maxy_的初始值，然后对每一层的子地图调用其updateBounds函数，传入minx_、miny_、maxx_、maxy_，函数将新的bound填充进去。updateBounds函数在Layer类中声明，在各层地图中被重载，第二步使用到的updateCosts函数也是如此。

for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    double prev_minx = minx_;
    double prev_miny = miny_;
    double prev_maxx = maxx_;
    double prev_maxy = maxy_;
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
    {
      ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
                        "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
                        prev_minx, prev_miny, prev_maxx , prev_maxy,
                        minx_, miny_, maxx_ , maxy_,
                        (*plugin)->getName().c_str());
    }
  }

需要指出的是，(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);成员函数updateBounds可能来自于三个类：
(1)costmap_2d/plugins/inflation_layer.cpp
(2)costmap_2d/plugins/obstacle_layer.cpp
(3) costmap_2d/plugins/static_layer.cpp

使用到了C++虚函数的多态！子类重写基类的方法，然后子类实例化指向基类即可使用。

这里以obstacle_layer.cpp中的updateBounds函数为例，看一下是怎么更新的

ObstacleLayer::updateBounds函数是ObstacleLayer类的一个成员函数，它的作用是更新障碍物层的边界信息，并根据机器人的位置和雷达/传感器数据更新地图上的障碍物。以下是对该函数的逐步解释：

更新滚动窗口原点：
如果rolling_window_标志为true，则调用updateOrigin函数来更新障碍物层的原点位置。这是为了在滚动窗口模式下跟随机器人的移动。

使用额外边界：
调用useExtraBounds函数，传入边界指针min_x、min_y、max_x、max_y。这个函数确保障碍物层的边界在指定的额外边界内。

初始化当前状态标志：
将current变量设置为true，这个布尔变量用来标记当前是否有有效的观测数据。

获取标记和清理观测结果：
创建两个Observation类型的向量observations和clearing_observations。
调用getMarkingObservations函数获取标记观测结果，并将其存储在observations向量中。同时更新current标志。
调用getClearingObservations函数获取清理观测结果，并将其存储在clearing_observations向量中。同时更新current标志。

更新全局当前状态：
将current_成员变量设置为current变量的值，表示当前是否有有效的观测数据。

对清理观测结果进行光线追踪：
遍历clearing_observations向量，对每个清理观测结果调用raytraceFreespace函数。这个函数通过光线追踪技术来确定哪些区域是可通行的，并更新边界。

处理障碍物观测结果：
遍历observations向量，处理每个观测点。
获取点云sensor_msgs::PointCloud2对象。
计算障碍物范围的平方sq_obstacle_range。
遍历点云中的每个点，使用迭代器获取点的x、y、z坐标。
如果点的高度pz超过max_obstacle_height_，则忽略该点。
计算点到点云原点的平方距离sq_dist，如果点太远，则忽略。
使用worldToMap函数将世界坐标系中的点转换为地图坐标系中的单元格坐标mx和my。
如果转换失败，则忽略该点。
计算地图中的索引index。
将对应的costmap_单元格标记为致命障碍物LETHAL_OBSTACLE。
调用touch函数更新边界信息。

更新足迹：
最后，调用updateFootprint函数，传入机器人的位置和朝向，以及边界指针。这个函数更新机器人足迹的表示，确保足迹在正确的位置。
输入参数robot_x、robot_y、robot_yaw用于获取机器人的位置和朝向，这些信息用于更新滚动窗口原点和处理传感器数据。min_x、min_y、max_x、max_y是指向边界坐标的指针，用于存储更新后的边界信息。

总的来说，updateBounds函数负责处理传感器数据，更新障碍物层的内部状态，并确保层的边界信息是最新的。

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);
  
  // 使用额外的边界
  useExtraBounds(min_x, min_y, max_x, max_y);

  // 标记当前状态为 true
  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);
  }

  // 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 sensor_msgs::PointCloud2& cloud = *(obs.cloud_);

    // 障碍物范围的平方
    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;

    // 获取点云的 x、y、z 坐标迭代器
    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");

    // 遍历点云中的每个点
    for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
    {
      double px = *iter_x, py = *iter_y, pz = *iter_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 单元格标记为致命障碍物
      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);
}

接下来调用Costmap2D类的worldToMapEnforceBounds函数，将得到的bound转换到地图坐标系。这个函数可以防止转换后的坐标超出地图范围。

  //接下来调用Costmap2D类的worldToMapEnforceBounds函数，
  //将得到的bound转换到地图坐标系。这个函数可以防止转换后的坐标超出地图范围。
  int x0, xn, y0, yn;
  costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
  costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);

  x0 = std::max(0, x0);
  xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
  //范围更新
  ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);

接下来，调用resetMap，将主地图上bound范围内的cell的cost恢复为默认值（track_unknown：255 / 否则：0），再对每层子地图调用updateCosts函数。updateCosts也是来源于不同的文件的（

(1)costmap_2d/plugins/inflation_layer.cpp
(2)costmap_2d/plugins/obstacle_layer.cpp
(3) costmap_2d/plugins/static_layer.cpp

），也是虚函数实现的多态。updateCosts实现了对costmap_主地图的合并。

下面以obstacle_layer.cpp为例，介绍updateCosts过程

//接下来，调用resetMap，将主地图上bound范围内的cell的cost恢复为默认值（track_unknown：255 / 否则：0），
  //再对每层子地图调用updateCosts函数。
  costmap_.resetMap(x0, y0, xn, yn);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
  }

其实上述完成了地图的初始化和循环更新地图的代码逻辑。通过上述的分析，我们知道有两个比较重要的接口：所有层都是继承layer类，这个类比较简单，是一个纯虚基函数，定定义了两个重要接口updateBounds、updateCosts，updateBounds用于计算栅格的权重，updateCosts用于将计算的地图合并到主地图上。

参考链接：
https://mp.weixin.qq.com/s/kAtx-R121hIhHQRizn41vg