【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 <costmap_prohibition_layer/costmap_prohibition_layer.h>
#include <pluginlib/class_list_macros.h>
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<CostmapProhibitionLayerConfig>(nh);
dynamic_reconfigure::Server<CostmapProhibitionLayerConfig>::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<CostmapProhibitionLayerConfig>(nh);
dynamic_reconfigure::Server<CostmapProhibitionLayerConfig>::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<std::mutex> 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<std::mutex> 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<geometry_msgs::Point>& polygon, unsigned char cost,
int min_i, int min_j, int max_i, int max_j, bool fill_polygon)
{
std::vector<PointInt> 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<PointInt> 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<PointInt> 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<PointInt> 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 文件，用于以机器可读格式存储有关插件的所有重要信息。它包含有关插件所在的库、插件名称、插件类型等的信息。对于虚拟墙禁用图层，它的描述性文件是这样的：
<library path="lib/libcostmap_prohibition_layer">
<class type="costmap_prohibition_layer_namespace::CostmapProhibitionLayer" base_class_type="costmap_2d::Layer">
<description>ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration.</description>
</class>
</library>library标签定义插件类所在的库。一个库可能包含多个不同类类型的插件。
class标签描述了库提供的类。
属性：
-
name ：类的查找名称。由pluginlib工具用作插件的标识符。
- type ：完全限定的类的类型。
- base_class_type ：基类的完全限定类型
- description ：类及其作用的描述。
更详细的描述可以查看这个文档。
2.向ROS注册插件
为了让 pluginlib 查询系统上所有 ROS 包中的所有可用插件，每个包必须明确指定它导出的插件以及哪些包库包含这些插件。插件提供者必须在其导出标记块内的package.xml中指向其插件描述文件，需要在package.xml文件最后添加这样一个标签：
<export>
<costmap_2d plugin="${prefix}/costmap_plugins.xml" />
</export>当然需要在前面添加对基类(costmap_2d)的依赖：
<depend>costmap_2d</depend>然后在工作空间中使用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++的类继承为用户提供了极大的便利。