本专栏旨在通过对ROS的系统学习,掌握ROS底层基本分布式原理,并具有机器人建模和应用ROS进行实际项目的开发和调试的工程能力。
详情:《ROS从入门到精通》
在ROS从入门到精通6-1:图文详解代价地图costmap原理与配置中,我们指出代价地图就是ROS定义的用于动态导航的地图数据结构,其在静态地图基础上添加了一些辅助信息,机器人导航必须依赖于地图,而静态地图在导航中一般不可以直接使用,因为导航过程中障碍信息是可变的,地图信息需要实时更新。
在costmap_2d
中,默认实现了代价地图的以下图层:
然而,这些默认的图层可能并不能满足实际需要。举例而言,社交地图层(Social Costmap Layer)用于机器人在导航过程中考虑与人类之间的社交交互,这个层级允许机器人更加智能地导航,并遵循一些社交规则,以更好地与人类共享空间,避免产生不适或危险的行为;禁区地图层(Prohibition Costmap Layer)用于标记和表示机器人不能进入的区域。这个层级的作用是在机器人的导航过程中限制其进入特定的区域(例如高压区、悬崖边缘、深水区等),从而确保机器人在导航时遵守特定的规则和限制,防止可能的事故和损坏。
本文就以实际案例介绍如何编写一个代价地图插件,关于插件开发请参考ROS从入门到精通5-3:ROS插件库与开发
首先创建功能包prohibition_layer
用于生成自定义代价地图插件,接着按以下步骤执行
构造基类:由于代价地图插件继承于costmap_2d
功能包的BaseGlobalPlanner
类,因此无需构造
构造插件类:在prohibition_layer/include
中新建prohibition_layer.h
,继承自基类costmap_2d::Layer
,重点实现其中的onInitialize
、updateBounds
与updateCosts
接口
class ProhibitionLayer : public costmap_2d::Layer
{
public:
ProhibitionLayer();
virtual ~ProhibitionLayer();
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw,
double *min_x, double *min_y, double *max_x, double *max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
int max_i, int max_j);
其中onInitialize
是代价地图为用户提供的初始化接口;updateBounds
计算插件图层要更新到主图层区域的大小;updateCosts
将每个图层的代价值更新到主图层。
注册插件:在prohibition_layer/src
中新建prohibition_layer.cpp
使用PLUGINLIB_EXPORT_CLASS
宏注册插件并实现上述接口,限于篇幅不列出完整代码
#include
#include
PLUGINLIB_EXPORT_CLASS(prohibition_layer::ProhibitionLayer, costmap_2d::Layer)
using costmap_2d::LETHAL_OBSTACLE;
namespace prohibition_layer
{
void ProhibitionLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_);
current_ = true;
...
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("ProhibitionLayer initialized.");
}
...
构建插件库.so
:编译此功能包prohibition_layer
将会在根目录devel/lib
中生成插件libprohibition_layer.so
集成插件库到ROS:在功能包prohibition_layer
下创建prohibition_layer_plugin.xml
描述插件信息和库路径
<library path="lib/libprohibition_layer">
<class type="prohibition_layer::ProhibitionLayer" 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>
使用插件:在参数配置文件夹中找到global_costmap_params.yaml
和local_costmap_params.yaml
,在末尾添加或修改
plugins:
- {name: static_map, type: "costmap_2d::StaticLayer"}
- {name: obstacles, type: "costmap_2d::VoxelLayer"}
- {name: prohibition_layer, type: "prohibition_layer::ProhibitionLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
在config
目录新建prohibition_areas.yaml
prohibition_areas:
#定义一个禁止点
- [6.09, -1.388]
# 定义一个禁止通行的线
- [[7.33, 2.11],
[7.26, 0.11]]
# 定义一个禁止通行的区域
- [[5.33, 1.11],
[5.26, -1.11],
[3, 1]]
这里我们设置了一个点、一条线和一个三角形
接着在move_base.launch
文件加载参数
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
...
<rosparam file="$(find gazebo_scene)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find gazebo_scene)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find gazebo_scene)/config/prohibition_areas.yaml" command="load" ns="global_costmap/prohibition_layer" />
...
node>
假设我们的实际场景如下
本文完整工程代码请通过下方名片联系博主获取
更多精彩专栏: