【ROS】使用pluginlib自定义costmap地图层

文章目录

文章目录

前言

一、重写地图层

1.包含头文件 

2.onInitialize()

3.updateBounds()

4.updateCosts()

二、向ROS注册

1.插件描述文件

2.向ROS注册插件

3.在costmap配置文件中使用

总结

---

前言

pluginlib是一个 C++ 库，用于从 ROS 包中加载和卸载插件。插件是从运行时库（即共享对象、动态链接库）加载的动态可加载类。使用 pluginlib，人们不必将他们的应用程序显式链接到包含类的库，相反，pluginlib 可以在任何时候打开一个包含导出类的库，而无需应用程序事先知道该库或包含类定义的头文件. 插件可用于在不需要应用程序源代码的情况下扩展/修改应用程序行为。

在costmap中就为用户提供了便捷丰富的地图层接口，用户可以使用C++中的继承操作，继承costmap_2d中的类，然后重写其中的虚函数，以实现自己的地图层功能，本文以添加禁用区域为例说明如何实现一个自定义的costmap地图层。

---

一、重写地图层

1.包含头文件 

为了允许动态加载类，必须将其标记为导出类。这是通过特殊宏PLUGINLIB_EXPORT_CLASS完成的。一般PLUGINLIB_EXPORT_CLASS都写在文件的开头，以虚拟墙地图层为例：

#include 
#include 

PLUGINLIB_EXPORT_CLASS(costmap_prohibition_layer_namespace::CostmapProhibitionLayer, costmap_2d::Layer)

如果想实现基本的地图层插件至少要重写这几个函数：

• void onInitialize()：在costmap执行初始化initialize后会执行这个函数，相当于为用户提供的初始化接口。
• void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)：计算插件图层要更新到主图层区域的大小，每个图层都可以增加这个尺寸，如下图（b）所示
• void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)：将每个图层的代价值更新到主图层，如下图（c）（d）（e）所示

2.onInitialize()

 onInitialize()函数主要执行一些初始化工作，如下面代码所示，下面进行逐行讲解。

void CostmapProhibitionLayer::onInitialize()

{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;

  _dsrv = new dynamic_reconfigure::Server(nh);
  dynamic_reconfigure::Server::CallbackType cb =
      boost::bind(&CostmapProhibitionLayer::reconfigureCB, this, _1, _2);
  _dsrv->setCallback(cb);

  // get a pointer to the layered costmap and save resolution
  costmap_2d::Costmap2D *costmap = layered_costmap_->getCostmap();
  _costmap_resolution = costmap->getResolution();

  // set initial bounds
  _min_x = _min_y = _max_x = _max_y = 0;
  
  // reading the prohibition areas out of the namespace of this plugin!
  // e.g.: "move_base/global_costmap/prohibition_layer/prohibition_areas"
  std::string params = "prohibition_areas";
  if (!parseProhibitionListFromYaml(&nh, params))
    ROS_ERROR_STREAM("Reading prohibition areas from '" << nh.getNamespace() << "/" << params << "' failed!");
  
  _fill_polygons = true;
  nh.param("fill_polygons", _fill_polygons, _fill_polygons);
  
  // compute map bounds for the current set of prohibition areas.
  computeMapBounds();
  
  ROS_INFO("CostmapProhibitionLayer initialized.");
}

创建ros节点句柄：

ros::NodeHandle nh("~/" + name_);
current_ = true;

创建动态配置的服务器和回调函数，并为服务器配置回调函数，这部分在我的其他文章进行过详细说明。

_dsrv = new dynamic_reconfigure::Server(nh);
dynamic_reconfigure::Server::CallbackType cb =
      boost::bind(&CostmapProhibitionLayer::reconfigureCB, this, _1, _2);
_dsrv->setCallback(cb);

获得地图数据的指针和分辨率参数：

// get a pointer to the layered costmap and save resolution
  costmap_2d::Costmap2D *costmap = layered_costmap_->getCostmap();
  _costmap_resolution = costmap->getResolution();

从yaml文件中读取虚拟墙区域：

// reading the prohibition areas out of the namespace of this plugin!
  // e.g.: "move_base/global_costmap/prohibition_layer/prohibition_areas"
  std::string params = "prohibition_areas";
  if (!parseProhibitionListFromYaml(&nh, params))
    ROS_ERROR_STREAM("Reading prohibition areas from '" << nh.getNamespace() << "/" << params << "' failed!");

从launch文件中读取参数，是否完全更新多边形区域，然后computeMapBounds()函数用来计算更新区域的最大最小值。

_fill_polygons = true;
nh.param("fill_polygons", _fill_polygons, _fill_polygons);
  
// compute map bounds for the current set of prohibition areas.
computeMapBounds();
  
ROS_INFO("CostmapProhibitionLayer initialized.");

3.updateBounds()

updateBounds()函数用来根据刚才computeMapBounds()计算得到的最大最小区域更新costmap中定义的最大最小区域变量double *min_x, double *min_y, double *max_x, double *max_y：

void CostmapProhibitionLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, 
                                           double *min_x, double *min_y, double *max_x, double *max_y)
{
    if (!enabled_)
        return;
    
    std::lock_guard l(_data_mutex);
    
    if (_prohibition_points.empty() && _prohibition_polygons.empty())
        return;

    *min_x = std::min(*min_x, _min_x);
    *min_y = std::min(*min_y, _min_y);
    *max_x = std::max(*max_x, _max_x);
    *max_y = std::max(*max_y, _max_y);

}

这里面主要就是进行了一个赋值操作

4.updateCosts()

updateCosts()是最主要的功能，用来将地图层的代价更新到主图层，这里面主要有两个部分，第一个循环是更新禁止通行的区域，第二个循环是更新禁止通行的点（因为这个图层约定的yaml文件格式可以选择禁用点或直线或区域，所以这里分开处理）

void CostmapProhibitionLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  std::lock_guard l(_data_mutex);
  
  // set costs of polygons
  for (int i = 0; i < _prohibition_polygons.size(); ++i)
  {
      setPolygonCost(master_grid, _prohibition_polygons[i], LETHAL_OBSTACLE, min_i, min_j, max_i, max_j, _fill_polygons);
  }
      
  // set cost of points
  for (int i = 0; i < _prohibition_points.size(); ++i)
  {
    unsigned int mx;
    unsigned int my;
    if (master_grid.worldToMap(_prohibition_points[i].x, _prohibition_points[i].y, mx, my))
    {
      master_grid.setCost(mx, my, LETHAL_OBSTACLE);
    }
  }
}

先调用了setPolygonCost()函数将禁用区域的代价值设置为“致命障碍”（LETHAL_OBSTACLE）

  // set costs of polygons
  for (int i = 0; i < _prohibition_polygons.size(); ++i)
  {
      setPolygonCost(master_grid, _prohibition_polygons[i], LETHAL_OBSTACLE, min_i, min_j, max_i, max_j, _fill_polygons);
  }

setPolygonCost()函数如下：

void CostmapProhibitionLayer::setPolygonCost(costmap_2d::Costmap2D &master_grid, const std::vector& polygon, unsigned char cost,
                                             int min_i, int min_j, int max_i, int max_j, bool fill_polygon)
{
    std::vector map_polygon;
    for (unsigned int i = 0; i < polygon.size(); ++i)
    {
        PointInt loc;
        master_grid.worldToMapNoBounds(polygon[i].x, polygon[i].y, loc.x, loc.y);
        map_polygon.push_back(loc);
    }

    std::vector polygon_cells;

    // get the cells that fill the polygon
    rasterizePolygon(map_polygon, polygon_cells, fill_polygon);

    // set the cost of those cells
    for (unsigned int i = 0; i < polygon_cells.size(); ++i)
    {
        int mx = polygon_cells[i].x;
        int my = polygon_cells[i].y;
        // check if point is outside bounds
        if (mx < min_i || mx >= max_i)
            continue;
        if (my < min_j || my >= max_j)
            continue;
        master_grid.setCost(mx, my, cost);
    }
}

先通过这几行代码，将多边形区域的世界坐标转化为地图边界，存储在loc变量中（作者定义的结构体，有两个变量int x和int y），然后通过rasterizePolygon函数获得这个区域内所有单元格的地图坐标

std::vector map_polygon;
for (unsigned int i = 0; i < polygon.size(); ++i)
{
    PointInt loc;
    master_grid.worldToMapNoBounds(polygon[i].x, polygon[i].y, loc.x, loc.y);
    map_polygon.push_back(loc);
}

std::vector polygon_cells;

// get the cells that fill the polygon
rasterizePolygon(map_polygon, polygon_cells, fill_polygon);

这个循环是根据获得的序号设置代价值，关键函数是setCost函数，根据每一个单元格的地图坐标，将其代价值设置为“致命障碍”

// set the cost of those cells
for (unsigned int i = 0; i < polygon_cells.size(); ++i)
{
    int mx = polygon_cells[i].x;
    int my = polygon_cells[i].y;
    // check if point is outside bounds
    if (mx < min_i || mx >= max_i)
        continue;
    if (my < min_j || my >= max_j)
        continue;
    master_grid.setCost(mx, my, cost);
}

然后这个循环就更简单了，直接使用worldToMap()函数将世界坐标转化为地图坐标，然后根据获得的地图坐标使用setCost()函数设置代价值。

  // set cost of points
  for (int i = 0; i < _prohibition_points.size(); ++i)
  {
    unsigned int mx;
    unsigned int my;
    if (master_grid.worldToMap(_prohibition_points[i].x, _prohibition_points[i].y, mx, my))
    {
      master_grid.setCost(mx, my, LETHAL_OBSTACLE);
    }
  }

---

 

二、向ROS注册

1.插件描述文件

插件描述文件是一个 XML 文件，用于以机器可读格式存储有关插件的所有重要信息。它包含有关插件所在的库、插件名称、插件类型等的信息。对于虚拟墙禁用图层，它的描述性文件是这样的：

  
    ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration.
  

library标签定义插件类所在的库。一个库可能包含多个不同类类型的插件。

class标签描述了库提供的类。

属性：

• name ：类的查找名称。由pluginlib工具用作插件的标识符。

• type ：完全限定的类的类型。
• base_class_type ：基类的完全限定类型
• description ：类及其作用的描述。

更详细的描述可以查看这个文档。

2.向ROS注册插件

为了让 pluginlib 查询系统上所有 ROS 包中的所有可用插件，每个包必须明确指定它导出的插件以及哪些包库包含这些插件。插件提供者必须在其导出标记块内的package.xml中指向其插件描述文件，需要在package.xml文件最后添加这样一个标签：

当然需要在前面添加对基类(costmap_2d)的依赖：

costmap_2d

然后在工作空间中使用catkin_make或catkin_make_isolated编译即可，然后使用以下命令查看：

rospack plugins --attrib=plugin costmap_2d

如果出现以下结果，则说明插件可用：

costmap_prohibition_layer /home/lyh/catkin_acad/src/costmap_prohibition_layer-repush3/costmap_plugins.xml
costmap_2d /opt/ros/kinetic/share/costmap_2d/costmap_plugins.xml

3.在costmap配置文件中使用

在参数配置文件夹中找到　global_costmap_params.yaml　和/或　local_costmap_params.yaml，在末尾添加或修改：

 plugins:
      - {name: static_map,       type: "costmap_2d::StaticLayer"}
      - {name: obstacles,        type: "costmap_2d::VoxelLayer"}
      - {name: inflation_layer,        type: "costmap_2d::InflationLayer"}
      - {name: costmap_prohibition_layer,        type: "costmap_prohibition_layer_namespace::CostmapProhibitionLayer"}    

对于虚拟墙地图层，还需要在param文件夹中自己配置一个设置禁止区域的参数文件，在参数配置文件夹（就是和　global_costmap_params.yaml　以及　local_costmap_params.yaml　相同位置的文件夹）中创建新的文档，命名为　＂prohibition_areas.yaml＂，然后在prohibition_areas.yaml文档中输入:

prohibition_areas:
#定义一个禁止点
 - [17.09, -6.388]
# 定义一个禁止通行的线
 - [[8.33, 2.11],
    [8.26, 5.11]]
# 定义一个禁止通行的区域
 - [[-11.15, -15.614],
    [-12.35, -13.89],
    [-10.05, -12.218]]

注意事项：
１.一定要严格按照上述格式来设置坐标，可能出现情况：
　　(1)坐标前的短横线没对齐
　　(2)定义禁止区域或者禁止线，两坐标之间缺少了逗号
２.你可以同时定义多个禁止点／多个禁止线／多个禁止区域，或者混合定义多个点／线／区域．

---

总结

本文以虚拟墙禁用层为例，详细介绍了如何实现一个自定义的costmap地图层，以及如何在ROS中使用pluginlib制作一个插件，插件机制应用广泛，使用C++的类继承为用户提供了极大的便利。