costmap_2d包介绍

文章目录

- 一. costmap_2d包介绍
- 二. Costmap包的执行入口-- move_base中调用
- 三. Costmap包的初始化以及维护
- - 3.1 Costmap2DROS类
  - - 3.1.1 构造函数 Costmap2DROS::Costmap2DROS
    - 3.1.2 地图更新线程 Costmap2DROS::mapUpdateLoop
    - 3.1.3 地图更新 Costmap2DROS::updateMap()
    - 3.1.4 激活各层地图 Costmap2DROS::start()
  - 3.2 Costmap2D类
  - 3.3 LayeredCostmap类
  - 3.4 CostmapLayer类

关于Move_base的框架梳理参考：此链接

一. costmap_2d包介绍

costmap_2d包提供了一种可配置框架来维护机器人在代价地图上应该如何导航的信息。实际使用的时候，可以分为global_costmap和local_costmap两部分，每一种都可以配置多个图层，包括以下几种：

static_layer :静态地图层，基本不变的地图层，通常都是SLAM建图完成的静态地图，用于路径规划
obstacle_layer：障碍地图层，用于动态的记录传感器感知的障碍物信息，用于路径规划和避障。
inflation_layer: 膨胀层，在以上两层地图进行膨胀，以避免机器人的撞上障碍物，用于路径规划。
Other Layers：可以通过插件的形式自己实现costmap，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。

costmap_2d包提供的ROS化功能接口：costmap_2d::Costmap2DROS。它使用costmap_2d::LayeredCostmap 来跟踪每一层。每一层在Costmap2DROS中以插件方式被实例化，并被添加到LayeredCostmap。每一层可以独立编译，且可使用C++接口实现对代价地图的随意修改。costmap_2d::Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构。其中包含或者涉及到的类如下：

Costmap2DROS类

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

LayeredCostmap类

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

Layer类

含有子地图层用到的一些函数，如更新size、更新bound、和主地图合并等；

Costmap2D类

存储该层维护的地图数据,包括地图的x,y方向的尺寸，地图的分辨率，地图原点位姿。

CostmapLayer类

派生自Layer类和Costmap2D类，里面提供了用主地图尺寸设置该层地图尺寸matchSize、用当前子地图数据更新对应区域updateWithMax等函数，相当于函数模版，在相关层进行继承和调用。

Staticlayer类和Obsaclelayer类

由CostmapLayer类派生出Staticlayer类和Obsaclelayer类，即静态层和障碍层，前者获取静态地图，后者通过传感器数据不断更新，获得能反应障碍物信息的子地图。

InflationLayer类

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

类与类的继承关系如下图：

在这里插入图片描述

二. Costmap包的执行入口-- move_base中调用

代码路径：move_base/src/move_base.cpp

首先获取地图信息，内切圆半径inscribed_radius/外接圆形半径circumscribed_radius等信息。

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()开启。

planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();

加载的全局代价地图是由静态地图层、障碍层和膨胀层构成，如下面下边的代码片段所示。而局部代价地图只有障碍层和膨胀层。

# global_costmap_params.yaml
    - {name: static_layer,
       type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,
       type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,
       type: "costmap_2d::InflationLayer"}
# lobal_costmap_params.yaml
    - {name: obstacle_layer,
       type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,
       type: "costmap_2d::InflationLayer"}

构建了必要的功能模块之后，就可以开启代价地图的计算了。

planner_costmap_ros_->start();

三. Costmap包的初始化以及维护

3.1 Costmap2DROS类

代码路径：costmap_2d/src/costmap_2d_ros.cpp

大致的运行关系如下图：

在这里插入图片描述

3.1.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_，作为规划器使用的主地图，并通过它管理各层地图。

通过LayeredCostmap的对象创造了一个由指向plugin的共享指针组成的容器，通过配置文件，向里逐个添加插件层。

//初始化一个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);
}

3.1.2 地图更新线程 Costmap2DROS::mapUpdateLoop

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

第一步：更新bound，即确定地图更新的范围；
第二步：更新cost，更新每层地图cell对应的cost值后整合到主地图上。

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

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

3.2 Costmap2D类

Costmap2D类是记录地图数据的底层，它记录地图的x、y方向的尺寸，地图的分辨率，地图原点位置，以及用unsigned char类型记录地图内容。并且，Costmap2D类提供了一些对地图进行基本操作的函数，如：地图复制、用index/点坐标来设置/获取地图上该点的cost值、地图坐标和世界坐标之间的转换、获取地图大小/分辨率/原点、设置多边形边缘及内部点的cost值。

它充当一种以模版形式存在的类，Layer类则是调用它的工具人形式。

代码路径：costmap_2d/src/costmap_2d.cpp

大致的运行关系如下图：

在这里插入图片描述