【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)所示

【ROS】使用pluginlib自定义costmap地图层_第1张图片

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.一定要严格按照上述格式来设置坐标,可能出现情况:
  (1)坐标前的短横线没对齐
  (2)定义禁止区域或者禁止线,两坐标之间缺少了逗号
2.你可以同时定义多个禁止点/多个禁止线/多个禁止区域,或者混合定义多个点/线/区域.


总结

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

你可能感兴趣的:(ROS,ros,slam,机器人,c++)