前言
这是一个系列小文章,主要介绍在ROS-Gazebo中如何更好地使用SDF格式建模与仿真。众所周知,URDF是ROS的原生支持格式,但在某些情况下(尤其是Gazebo仿真时),使用SDF格式会更加合理。鉴于中文网络上几乎没有成体系的SDF介绍博文,因此我将自己在硕士期间关于SDF模型的使用经验稍作总结。如有谬误,还望友好地指出。
本系列规划如下内容,建议按顺序阅读。尤其是第一篇,以确认你是否有必要使用SDF建模:
- 为什么选择SDF?
- SDF规范文档与模型资源
- SDF的建模与使用
- ERB:模块化生成SDF模型
- SDF与URDF的相互转换
- 基于SDF的多机器人仿真
- 使用与编写自定义插件——以PID控制插件为例
1 多机器人仿真的思路
当我们使用URDF模型时,要想进行多机器人仿真,一般是在launch文件中为模型和控制器添加不同的命名空间,让gazebo能够区分不同的机器人。
而对于SDF中的多机器人仿真,我们可以将多个机器人的.sdf建立在同一个.world文件下,在launch中加载这一个文件即可。之所以可以这样做,我们在 SDF规范文档与模型资源 一文中也提到,是因为SDF格式支持<model>元素的嵌套(model中套model),而URDF不具有这一特性。
下面我们用一个例子来说明如何在同时加载3个cart_front_steer小车。这是官方模型库中提供的模型,如何下载官方模型库亦可见 SDF规范文档与模型资源 。
2 建立多机器人.world文件
前面文章中说过,在world中添加单个模型,只需要加入如下<model>元素即可,其中cart_front_steer
是模型名称,模型文件夹必须在gazebo模型环境变量中。
<model name='cart_front_steer'>
<include>
<uri>model://cart_front_steer</uri>
</include>
</model>
效果如下:
而要加载多机器人模型,只需要采用所谓的“嵌套模型”即可,将<model>元素中的代码修改如下。其中<name>元素为每个模型定义了单独的名称,便于为每个模型单独添加插件和关节。<pose>元素定义了每个模型的初始位置。
<model name='cart_front_steer'>
<!-- A testing model -->
<include>
<uri>model://cart_front_steer</uri>
<name>cart_front_steer_1</name>
<pose>0 0 0.1 0 0 0</pose>
</include>
<!-- A testing model -->
<include>
<uri>model://cart_front_steer</uri>
<name>cart_front_steer_2</name>
<pose>1.5 0 0.1 0 0 0</pose>
</include>
<!-- A testing model -->
<include>
<uri>model://cart_front_steer</uri>
<name>cart_front_steer_3</name>
<pose>3.0 0 0.1 0 0 0</pose>
</include>
</model>
效果如下:
插件和关节的添加方法和单机器人一致,只要注意加上每个模型自己的名称即可。例如,我们要用一个<fixed>关节将前两个小车的底盘连在一起,只需要在一级<model>元素下添加如下关节即可:
<joint name="fixed_joint_1" type="fixed">
<parent>cart_front_steer_1::chassis</parent>
<child>cart_front_steer_2::chassis</child>
</joint>
最后,在launch文件中启动本.world文件。
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find mult_robot)/world/mult_robot.world"/>
</include>
</launch>
功能包代码见我的项目: