要想在地图中添加障碍,用virtual_layer
和costmap_prohibition_layer
都可以,效果一样,区别就是前者的yaml里需要加上朝向,其实是没必要的。下面以virtual_layer
为例向代价地图加入新的障碍。
进入costmap_2d
这个包,查看costmap_2d\costmap_plugins.xml
文件,可以看到目前所拥有的层,基类都是costmap_2d::Layer
。
配置代价地图
launch
里加入1
2<rosparam file="$(find navigation2d)/param/virtual_layer.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find navigation2d)/param/virtual_layer.yaml" command="load" ns="local_costmap" />
在global_costmap_params.yaml
和 local_costmap_params.yaml
里加入 - {name: virtual_layer, type: "virtual_costmap_layer::VirtualLayer"}
最后是设置禁止区域坐标参数,首先需要在参数配置文件夹(global_costmap_params.yaml
所在的文件夹)中创建新的文档,命名为prohibition_areas.yaml
,然后输入:1
2
3
4
5
6
7costmap_prohibition_layer:
prohibition_areas:
- [[-1.3, -2.98],[-1.3, -16.77]]
- [[13.31, -26.77],[2.88, -3.45]]
- [[-4.96, -37.78],[-32.67, -37.90]]
- [[-23.39, -33.43],[-11.89, -33.20]]
- [[0.0, 6.89],[0.0, 38.05]]
如果是virtual_layer.yaml
,设置类似如下:1
2
3
4
5
6virtual_layer:
zone_topics: ["/zone"]
obstacle_topics: ["/obstacle"]
static_forms: [[[-18.46,-12.18,0.0],[-13.21,-12.18,0.0],[-13.21,-13.4,0.0],[-18.46,-13.4,0.0]],
[[-13.99,-0.17,0.0],[-11.87,-0.14,0.0]],
[[-13.18,-11.15,0.0],[-11.81,-11.15,0.0]]]
此时加载virtual_layer
成功,但是障碍没有被膨胀,只有静态障碍和代价值
查来查去,怎么也找不到原因,最后是在两个代价地图的yaml里,把- {name: virtual_layer, type: "virtual_costmap_layer::VirtualLayer"}
放到膨胀层之前 ,问题解决,看来膨胀层的源码不完善。
注意事项:
- 一定要严格按照上述格式来设置坐标,格式错误导致不能识别禁止区域坐标情形有: (1)坐标前的短横线没对齐 (2) 定义禁止区域或者禁止线,两坐标之间缺少了逗号
- 可以同时定义多个点障碍/线障碍/障碍区域。对于3个及以上的点,形成的是封闭的区域障碍。
- 禁止区域的坐标是map坐标系的
- 由于Bresenham算法,形成的线可能看起来不够直
costmap_prohibition_layer
CMakeLists
里可以看到add_library(costmap_prohibition_layer src/costmap_prohibition_layer.cpp)
, 结果在lib/libcostmap_prohibition_layer.so
看文件package.xml
,最后有一句:1
2
3<export>
<costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
</export>
costmap_plugins.xml
如下:1
2
3
4
5<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>
以上几项缺一不可,放到工作空间编译后,在终端中输入rospack plugins —attrib=plugin costmap_2d ,这个命令是显示当前costmap_2d
能接受的所有插件,如果出现1
costmap_prohibition_layer /home/user/plan_ws/src/costmap_prohibition_layer/costmap_plugins.xml
说明prohibition_layer已经是一个可供使用的地图插件了。