【ROS-Navigation】Costmap2D代价地图源码解读-1

记录学习阅读ROS Navigation源码的理解，本文为Costmap2D代价地图源码学习记录，以文字总结、绘制结构图说明、代码注释为主。仍在学习过程中，有错误欢迎指正，共同进步。

Costmap通过各层地图订阅话题、接收传感器数据，维护各层地图数据，并最终整合出一张用于路径规划的主地图。

【结构示意图】

【相关文件】

• costmap_2d/src/costmap_2d_ros.cpp
• costmap_2d/src/costmap_2d.cpp
• costmap_2d/src/layered_costmap.cpp
• costmap_2d/src/costmap_layer.cpp
• costmap_2d/plugins/static_layer.cpp
• costmap_2d/plugins/obstale_layer.cpp
• costmap_2d/plugins/inflation_layer.cpp

Costmap2DROS类是对整个代价地图内容的封装。

LayeredCostmap类是Costmap2DROS的类成员，它是“主地图”，也能够管理各层地图，因为它含有指向各层子地图的指针，能够调用子地图的类方法，开启子地图的更新。并且，各层子地图最后都会合并到主地图上，提供给规划器的使用。它含有Costmap2D类成员，这个类就是底层地图，用于记录地图数据。

CostmapLayer类派生自Layer类和Costmap2D类。Layer类中含有子地图层用到的一些函数，如更新size、更新bound、和主地图合并等；Costmap2D类存储该层维护的地图数据。由CostmapLayer类派生出StaticLayer类和ObstacleLayer类，即静态层和障碍层，前者获取静态地图，后者通过传感器数据不断更新，获得能反映障碍物信息的子地图。

由Layer类单独派生出InflationLayer类，即膨胀层，用它来反映障碍物在地图上向周边的膨胀。由于它的父类中不含Costmap2D类，所以其实膨胀层自身没有栅格地图要维护，这一点和另外两层有区别。

本篇记录对Costmap2DROS类和Costmap2D类的阅读和理解。

【代码分析】

1. Costmap2DROS类

这里关注一下Costmap2DROS类的构造函数以及它在Movebase中被调用的函数。

<1> 构造函数 Costmap2DROS::Costmap2DROS

首先是一些参数的获取。循环等待直到获得机器人底盘坐标系和global系之间的坐标转换。
并获取rolling_window、track_unknown_space、always_send_full_costmap的参数，默认为false。

Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :
    layered_costmap_(NULL),
    name_(name),
    tf_(tf),
    transform_tolerance_(0.3),
    map_update_thread_shutdown_(false),
    stop_updates_(false),
    initialized_(true),
    stopped_(false),
    robot_stopped_(false),
    map_update_thread_(NULL),
    last_publish_(0),
    plugin_loader_("costmap_2d", "costmap_2d::Layer"),
    publisher_(NULL),
    dsrv_(NULL),
    footprint_padding_(0.0)
{
  //初始化old pose
  old_pose_.setIdentity();
  old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));

  ros::NodeHandle private_nh("~/" + name);
  ros::NodeHandle g_nh;

  //获取tf前缀
  ros::NodeHandle prefix_nh;
  std::string tf_prefix = tf::getPrefixParam(prefix_nh);

  //两个坐标系
  private_nh.param("global_frame", global_frame_, std::string("/map"));
  private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));

  //确保基于tf前缀正确设置了坐标系
  //注：tf::resolve或者TransformListener::resolve方法的作用就是使用tf_prefix参数将frame_name解析为frame_id
  global_frame_ = tf::resolve(tf_prefix, global_frame_);
  robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);

  ros::Time last_error = ros::Time::now();
  std::string tf_error;
  
  //确保机器人底盘坐标系和世界坐标系之间的有效转换
  //否则一直停留在这里阻塞
  while (ros::ok()
      && !tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), ros::Duration(0.01),
                               &tf_error))
  {
    ros::spinOnce();
    if (last_error + ros::Duration(5.0) < ros::Time::now())
    {
      ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
               robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
      last_error = ros::Time::now();
    }
    // The error string will accumulate and errors will typically be the same, so the last
    // will do for the warning above. Reset the string here to avoid accumulation.
    tf_error.clear();
  }
  //检测是否需要“窗口滚动”，从参数服务器获取以下参数
  bool rolling_window, track_unknown_space, always_send_full_costmap;
  private_nh.param("rolling_window", rolling_window, false);
  private_nh.param("track_unknown_space", track_unknown_space, false);
  private_nh.param("always_send_full_costmap", always_send_full_costmap, false);

接下来创建一个LayeredCostmap类实例layered_costmap_，作为规划器使用的主地图，并通过它管理各层地图。

  //初始化一个layered_costmap
  layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

接着，在参数服务器上获取“plugins”参数，这里得到的插件即为各层子地图。每层子地图使用Pluginlib（ROS插件机制）来实例化，各个层可以被独立的编译，且允许使用C++接口对Costmap做出任意的改变。循环将每个plugin即每层子地图添加进layered_costmap_类的指针组对象中，并对每层地图调用其initialize类方法，初始化各层地图。这个函数定义在Layer类中，它向各层地图“通知”主地图的存在，并调用oninitialize类方法（Layer类中的虚函数，被定义在各层地图中）。

  //如果没有这一项参数，重新设置旧参数
  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)
    { 
      //从my_list里获取地图名称和种类
      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());

      //这行会创建一个以type为类类型的实例变量，然后让plugin这个指针指向这个实例
      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
      layered_costmap_->addPlugin(plugin);
      //实际执行的是plugin调用的父类Layer的方法initialize
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
  }

订阅footprint话题，回调函数为setUnpaddedRobotFootprintPolygon。当话题上收到footprint时，回调函数会将接收到的footprint根据参数footprint_padding_的值进行“膨胀”，得到“膨胀”后的padded_footprint_，传递给各级地图。

创建另一个话题，该话题上将发布的内容是根据机器人当前位置计算出来的实时footprint的位置。

  std::string topic_param, topic;
  if (!private_nh.searchParam("footprint_topic", topic_param))
  {
    topic_param = "footprint_topic";
  }
  private_nh.param(topic_param, topic, std::string("footprint"));
  footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);

  if (!private_nh.searchParam("published_footprint_topic", topic_param))
  {
    topic_param = "published_footprint";
  }
  private_nh.param(topic_param, topic, std::string("oriented_footprint"));
  footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);

  setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));

创建地图的发布器实例，Costmap2DPublisher类是用于地图发布的封装类。

  //发布Costmap2D
  publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
                                      always_send_full_costmap);

  //创建地图更新线程的控制量
  stop_updates_ = false;
  initialized_ = true;
  stopped_ = false;

  //创建一个timer去检测机器人是否在移动
  robot_stopped_ = false;
  //回调函数movementCB通过比较前后两个pose的差来判断机器人是否在移动
  timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);

开启动态参数配置，它的回调函数Costmap2DROS::reconfigureCB会在节点启动时运行一次，加载.cfg文件的配置参数。这个函数给对应参数赋值，并创建了一个Costmap2DROS::mapUpdateLoop函数的线程，即地图更新线程。

  //开启参数动态配置
  dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
  //回调函数reconfigureCB 除了对一些类成员的配置值做赋值以外，还会开启一个更新map的线程 
  dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,
                                                                              _2);
  dsrv_->setCallback(cb);
}

<2> 地图更新线程 Costmap2DROS::mapUpdateLoop

这个函数循环调用UpdateMap函数，更新地图。并以publish_cycle为周期，发布更新后的地图。

void Costmap2DROS::mapUpdateLoop(double frequency)
{
  // the user might not want to run the loop every cycle
  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();

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

<3> 地图更新 Costmap2DROS::updateMap()

这个函数首先调用类内getRobotPose函数，通过tf转换，将机器人底盘系的坐标转换到global系，得到机器人的位姿。然后通过layered_costmap_调用LayeredCostmap类的updateMap函数，更新地图。

void Costmap2DROS::updateMap()
{
  if (!stop_updates_)
  {
    //获取机器人在地图上的位置和姿态
    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);

这里更新机器人的实时足迹，通过footprint_pub_发布。

      //更新机器人足迹
      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;
    }
  }
}

<4> 激活各层地图 Costmap2DROS::start()

start函数在Movebase中被调用，这个函数对各层地图插件调用activate函数，激活各层地图。

void Costmap2DROS::start()
{
  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
  //检查地图是否暂停或者停止
  if (stopped_)
  {
    //如果停止，需要重新订阅话题
    //“Layer”是一个类，active是其中一个类方法
    for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
        ++plugin)
    {
      (*plugin)->activate();
    }
    stopped_ = false;
  }
  stop_updates_ = false;

  //在costmap被重新初始化前阻塞..meaning one update cycle has run
  ros::Rate r(100.0);
  while (ros::ok() && !initialized_)
    r.sleep();
}

2. Costmap2D类

Costmap2D类是记录地图数据的底层，它记录地图的x、y方向的尺寸，地图的分辨率，地图原点位置，以及用unsigned char类型记录地图内容。

protected:
  unsigned int size_x_;
  unsigned int size_y_;
  double resolution_;
  double origin_x_;
  double origin_y_;
  unsigned char* costmap_;

并且，Costmap2D类提供了一些对地图进行基本操作的函数，如：地图复制、用index/点坐标来设置/获取地图上该点的cost值、地图坐标和世界坐标之间的转换、获取地图大小/分辨率/原点、设置多边形边缘及内部点的cost值。

前面的函数内容都很简单，这里关注一下多边形cost的设置，它主要是用来设置机器人足迹内cell的cost

<1> 设置多边形cost Costmap2D::setConvexPolygonCost

先将世界系下的多边形顶点转换到地图坐标系，并存放进map_polygon数组中。

bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
{
  std::vector<MapLocation> map_polygon;
  for (unsigned int i = 0; i < polygon.size(); ++i)
  {
    MapLocation loc;
    if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
    {
      // ("Polygon lies outside map bounds, so we can't fill it");
      return false;
    }
    map_polygon.push_back(loc);
  }

接着调用convexFillCells函数，通过机器人顶点坐标数组map_polygon得到多边形边缘及内部的全部cell，存放在polygon_cells中，并通过循环对多边形边缘及内部各cell的cost赋值。

  std::vector<MapLocation> polygon_cells;
  // get the cells that fill the polygon
  convexFillCells(map_polygon, polygon_cells);

  //把多边形边缘及内部的所有cell的cost设置为cost_value
  for (unsigned int i = 0; i < polygon_cells.size(); ++i)
  {
    unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
    costmap_[index] = cost_value;
  }
  return true;
}

<2> 获取多边形边缘及内部cell Costmap2D::convexFillCells

首先确保给定的多边形顶点不少于3个，接着调用类内polygonOutlineCells函数，通过给定的顶点提取多边形边上的cell。

void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
  // we need a minimum polygon of a triangle
  if (polygon.size() < 3)
    return;

  //首先获得轮廓点之间连线的列表，存放在polygon_cells中
  polygonOutlineCells(polygon, polygon_cells);

对边上的cell点的x做排序，使其按x坐标升序排列。

  MapLocation swap;
  unsigned int i = 0;
  while (i < polygon_cells.size() - 1)
  {
    if (polygon_cells[i].x > polygon_cells[i + 1].x)
    {
      swap = polygon_cells[i];
      polygon_cells[i] = polygon_cells[i + 1];
      polygon_cells[i + 1] = swap;

      if (i > 0)
        --i;
    }
    else
      ++i;
  }

遍历所有x，对每个相同的x，检查y，获得y最大和最小的polygon cell，将范围内的所有cell填充进polygon_cells，获得多边形边缘及内部的所有cell。

  i = 0;
  MapLocation min_pt;
  MapLocation max_pt;
  unsigned int min_x = polygon_cells[0].x;
  unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;

  for (unsigned int x = min_x; x <= max_x; ++x)
  {
    if (i >= polygon_cells.size() - 1)
      break;

    if (polygon_cells[i].y < polygon_cells[i + 1].y)
    {
      min_pt = polygon_cells[i];
      max_pt = polygon_cells[i + 1];
    }
    else
    {
      min_pt = polygon_cells[i + 1];
      max_pt = polygon_cells[i];
    }

    i += 2;
    while (i < polygon_cells.size() && polygon_cells[i].x == x)
    {
      if (polygon_cells[i].y < min_pt.y)
        min_pt = polygon_cells[i];
      else if (polygon_cells[i].y > max_pt.y)
        max_pt = polygon_cells[i];
      ++i;
    }

    MapLocation pt;
    //填充
    for (unsigned int y = min_pt.y; y < max_pt.y; ++y)
    {
      pt.x = x;
      pt.y = y;
      polygon_cells.push_back(pt);
    }
  }
}

<3> 获取多边形边上的cell Costmap2D::polygonOutlineCells

这个函数循环调用raytraceLine函数，不断获取相邻之间的连线，最终组成多边形边上的cell，需要注意的是需要将最后一点和第一点连接起来，形成闭合。

void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
  PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
  for (unsigned int i = 0; i < polygon.size() - 1; ++i)
  {
    raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
  }
  if (!polygon.empty())
  {
    unsigned int last_index = polygon.size() - 1;
    // we also need to close the polygon by going from the last point to the first
    raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
  }
}

<4> 两点连线上的cell raytraceLine

对于离散的平面点，指定两个点，这个函数可以找到两个点之间的其他点，使得这些中间组成一个尽可能趋近直线的点集。

  template<class ActionType>
    inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
                             unsigned int max_length = UINT_MAX)
    {
      int dx = x1 - x0;
      int dy = y1 - y0;

      unsigned int abs_dx = abs(dx);
      unsigned int abs_dy = abs(dy);

      int offset_dx = sign(dx);
      int offset_dy = sign(dy) * size_x_;

      unsigned int offset = y0 * size_x_ + x0;

      // we need to chose how much to scale our dominant dimension, based on the maximum length of the line
      double dist = hypot(dx, dy);
      double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);

      // if x is dominant
      if (abs_dx >= abs_dy)
      {
        int error_y = abs_dx / 2;
        bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
        return;
      }

      // otherwise y is dominant
      int error_x = abs_dy / 2;
      bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
    }

Neo 2020.3