Gazebo之MyRobot建立

1. 源由

在本章中，将学习如何在 SDFormat 中构建一个简单的两轮机器人。

注：SDFormat（Simulation Description Format），有时简称为 SDF，是一种 XML 格式，用于描述机器人模拟器、可视化和控制的对象和环境。

2. 示例

Step 1: 新建一个简单世界

从构建一个简单的世界开始，然后在其中构建我们的机器人。打开一个名为 empty_world.sdf 的新文件，并将以下代码复制到其中。

<?xml version="1.0" ?>
<sdf version="1.10"><world name="car_world"><physics name="1ms" type="ignored"><max_step_size>0.001</max_step_size><real_time_factor>1.0</real_time_factor></physics><pluginfilename="gz-sim-physics-system"name="gz::sim::systems::Physics"></plugin><pluginfilename="gz-sim-user-commands-system"name="gz::sim::systems::UserCommands"></plugin><pluginfilename="gz-sim-scene-broadcaster-system"name="gz::sim::systems::SceneBroadcaster"></plugin><light type="directional" name="sun"><cast_shadows>true</cast_shadows><pose>0 0 10 0 0 0</pose><diffuse>0.8 0.8 0.8 1</diffuse><specular>0.2 0.2 0.2 1</specular><attenuation><range>1000</range><constant>0.9</constant><linear>0.01</linear><quadratic>0.001</quadratic></attenuation><direction>-0.5 0.1 -0.9</direction></light><model name="ground_plane"><static>true</static><link name="link"><collision name="collision"><geometry><plane><normal>0 0 1</normal></plane></geometry></collision><visual name="visual"><geometry><plane><normal>0 0 1</normal><size>100 100</size></plane></geometry><material><ambient>0.8 0.8 0.8 1</ambient><diffuse>0.8 0.8 0.8 1</diffuse><specular>0.8 0.8 0.8 1</specular></material></visual></link></model></world>
</sdf>

保存文件，导航到保存文件的目录并启动模拟器：

$ gz sim empty_world.sdf

注：一个只有地面和阳光的空世界。

Step 2: 新建一个模型(model)

• 定义了模型的名称 vehicle_blue，它应该在其同级（其他标签或同级模型）中是唯一的。
• 每个模型可以有一个链接被指定为 canonical_link，模型的隐式框架附加到这个链接上。
• 如果未定义，则第一个 <link> 将被选择为 canonical_link。
• <pose> 标签用于定义模型的位置和方向，relative_to 属性用于定义模型相对于任何其他框架的姿态。
• 如果未定义 relative_to，则模型的 <pose> 将相对于世界。
• <pose> 标签内的值如下：<pose>X Y Z R P Y</pose>，其中 X Y Z 表示框架的位置，R P Y 表示横滚、俯仰、偏航的方向。我们将它们设置为零，使两个框架（模型和世界）相同。

<model name='vehicle_blue' canonical_link='chassis'><pose relative_to='world'>0 0 0 0 0 0</pose>

Step 3: 机器人组成链接(Links)

Step 3.1: 新增底盘(Links/Chassis)

定义第一个链接，即我们汽车的底盘，以及它相对于模型的姿态。

    <link name='chassis'><pose relative_to='__model__'>0.5 0 0.4 0 0 0</pose>

Step 3.1.1: 惯性属性(Inertial properties)

在这里，定义了底盘的惯性属性，如 <mass> 和 <inertia> 矩阵。使用此工具可以计算基本形状的惯性矩阵的值。

    <inertial> <!--inertial properties of the link mass, inertia matix--><mass>1.14395</mass><inertia><ixx>0.095329</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.381317</iyy><iyz>0</iyz><izz>0.476646</izz></inertia></inertial>

Step 3.1.2: 视觉(Visual)

• 顾名思义，<visual> 标签负责定义链接的外观。
• 首先，在 <geometry> 标签内将链接的形状定义为一个 <box>（长方体），然后在 <size> 标签内指定这个盒子的三个维度（以米为单位）。
• 接着，在 <material> 标签内定义链接的材质。
  • 定义了 <ambient>、<diffuse> 和 <specular> 颜色，每个颜色用一组四个数字表示，分别为红色/绿色/蓝色/透明度，范围在 [0, 1] 之间。

    <visual name='visual'><geometry><box><size>2.0 1.0 0.5</size></box></geometry><!--let's add color to our link--><material><ambient>0.0 0.0 1.0 1</ambient><diffuse>0.0 0.0 1.0 1</diffuse><specular>0.0 0.0 1.0 1</specular></material></visual>

Step 3.1.3: 碰撞(Collision)

<collision> 标签定义了链接的碰撞属性，即链接如何与其他物体发生碰撞以及物理引擎对其的影响。

        <collision name='collision'><geometry><box><size>2.0 1.0 0.5</size></box></geometry></collision></link>
</model>

注：<collision> 可以与视觉属性不同，例如，通常使用更简单的碰撞模型来减少计算时间。

Step 3.2: 新增左轮(Links/Left wheel)

• 为机器人添加左轮。
• 以下代码应放在 标签之后和 标签之前。所有属于同一模型的链接和关节应在 之前定义。
• 定义链接的名称为 left_wheel，然后将其 <pose> 相对于底盘链接进行定义。
• 由于轮子需要放置在底盘的左后方，因此我们选择了 <pose> 的值为 -0.5 0.6 0。
• 轮子是一个圆柱体，但它侧放着。因此我们将方向值定义为 -1.5707 0 0，这是绕 x 轴旋转 -90 度（角度以弧度为单位）。

<link name='left_wheel'><pose relative_to="chassis">-0.5 0.6 0 -1.5707 0 0</pose>

Step 3.2.1: 惯性属性(Inertial properties)

    <inertial><mass>1</mass><inertia><ixx>0.043333</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.043333</iyy><iyz>0</iyz><izz>0.08</izz></inertia></inertial>

Step 3.2.2: 视觉(Visual)

    <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.2</length></cylinder></geometry><material><ambient>1.0 0.0 0.0 1</ambient><diffuse>1.0 0.0 0.0 1</diffuse><specular>1.0 0.0 0.0 1</specular></material></visual>

Step 3.2.3: 碰撞(Collision)

    <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.2</length></cylinder></geometry></collision>

Step 3.3: 新增右轮(Links/Right wheel)

• 为机器人添加右轮。
• 以下代码应放在 标签之后和 标签之前。所有属于同一模型的链接和关节应在 之前定义。
• 定义链接的名称为 left_wheel，然后将其 <pose> 相对于底盘链接进行定义。
• 由于轮子需要放置在底盘的右后方，因此我们选择了 <pose> 的值为 -0.5 -0.6 0。
• 轮子是一个圆柱体，但它侧放着。因此我们将方向值定义为 -1.5707 0 0，这是绕 x 轴旋转 -90 度（角度以弧度为单位）。

<!--The same as left wheel but with different position-->
<link name='right_wheel'><pose relative_to="chassis">-0.5 -0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->

Step 3.3.1: 惯性属性(Inertial properties)

    <inertial><mass>1</mass><inertia><ixx>0.043333</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.043333</iyy><iyz>0</iyz><izz>0.08</izz></inertia></inertial>

Step 3.3.2: 视觉(Visual)

    <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.2</length></cylinder></geometry><material><ambient>1.0 0.0 0.0 1</ambient><diffuse>1.0 0.0 0.0 1</diffuse><specular>1.0 0.0 0.0 1</specular></material></visual>

Step 3.3.3: 碰撞(Collision)

    <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.2</length></cylinder></geometry></collision>

Step 3.4: 添加任意框架

任意框架需要两个属性：

• name: 框架的名称
• attached_to: 该框架附加到的框架或链接的名称

给框架命名为 caster_frame，并将其附加到底盘链接上，然后使用 <pose> 标签来定义框架的位置和方向。

注：没有使用 relative_to 属性，因此姿态是相对于 attached_to 属性中指定的框架，即在我们这个例子中是底盘。

<frame name="caster_frame" attached_to='chassis'><pose>0.8 0 -0.2 0 0 0</pose>
</frame>

Step 3.5: 添加滑轮

其姿态是相对于我们上面定义的 caster_frame 框架。

• <pose> 标签而没有定义位置或方向；在这种情况下，链接的姿态与 relative_to 框架的姿态相同（即为单位姿态）。

<!--caster wheel-->
<link name='caster'><pose relative_to='caster_frame'/>

Step 3.5.1 惯性属性(Inertial properties)

    <inertial><mass>1</mass><inertia><ixx>0.016</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.016</iyy><iyz>0</iyz><izz>0.016</izz></inertia></inertial>

Step 3.5.2 视觉(Visual)

    <visual name='visual'><geometry><sphere><radius>0.2</radius></sphere></geometry><material><ambient>0.0 1 0.0 1</ambient><diffuse>0.0 1 0.0 1</diffuse><specular>0.0 1 0.0 1</specular></material></visual>

Step 3.5.3 碰撞(Collision)

    <collision name='collision'><geometry><sphere><radius>0.2</radius></sphere></geometry></collision>

Step 4: 链接关节(links)

最后需要将这些链接连接在一起，这就需要用到 标签。 标签将两个链接连接在一起，并定义它们相互之间的运动方式。在 标签内，我们需要定义要连接的两个链接及其关系（运动方式）。

Step 4.1: 添加 Left wheel joint

第一个关节是 left_wheel_joint。它有两个属性：name='left_wheel_joint' 和 type='revolute'。revolute 类型提供一个具有关节限制的旋转自由度。关节的姿态与子链接框架相同，即 left_wheel 框架。

<joint name='left_wheel_joint' type='revolute'><pose relative_to='left_wheel'/>

Step 4.1.1: 链接连接

将两个链接（实体）连接在一起。在这里，我们将底盘与左轮连接。底盘是父链接，左轮是子链接。

    <parent>chassis</parent><child>left_wheel</child>

Step 4.1.2: 定义旋转轴

• 旋转轴可以是任何框架，不仅仅是父链接或子链接。
• 我们选择相对于模型框架的 y 轴，因此我们在 y 元素中放置 1，在其他元素中放置 0。对于旋转关节，我们需要在 <lower> 和 <upper> 标签中定义旋转角度的 <limits>。

    <axis><xyz expressed_in='__model__'>0 1 0</xyz> <!--can be defined as any frame or even arbitrary frames--><limit><lower>-1.79769e+308</lower>    <!--negative infinity--><upper>1.79769e+308</upper>     <!--positive infinity--></limit></axis>
</joint>

Step 4.2: 添加 Right wheel joint

right_wheel_joint 非常相似，不同之处在于关节的姿态。这个关节将右轮与底盘连接在一起。

<joint name='right_wheel_joint' type='revolute'><pose relative_to='right_wheel'/><parent>chassis</parent><child>right_wheel</child><axis><xyz expressed_in='__model__'>0 1 0</xyz><limit><lower>-1.79769e+308</lower>    <!--negative infinity--><upper>1.79769e+308</upper>     <!--positive infinity--></limit></axis>
</joint>

Step 4.3: 添加 Caster wheel joint

对于万向轮，需要不同类型的关节（连接）。这里使用了 type='ball'，它提供三个旋转自由度。

<joint name='caster_wheel' type='ball'><parent>chassis</parent><child>caster</child>
</joint>

Step 5: 简单验证

$ gz sim building_robot.sdf

测试资料：[SnapLearnGazebo/lesson_02_sensor}(https://github.com/SnapDragonfly/SnapLearnGazebo/tree/main/lesson_02_sensor)

Step 6: 移动机器人

Step 6.1: 添加 Diff_drive plugin

diff_drive插件帮助控制机器人，特别是可以差速驱动的机器人。让我们在机器人上设置这个插件。

在building_robot.sdf文件基础上，新建一个moving_robot.sdf文件，并在vehicle_blue模型标签内添加以下代码。

<pluginfilename="gz-sim-diff-drive-system"name="gz::sim::systems::DiffDrive"><left_joint>left_wheel_joint</left_joint><right_joint>right_wheel_joint</right_joint><wheel_separation>1.2</wheel_separation><wheel_radius>0.4</wheel_radius><odom_publish_frequency>1</odom_publish_frequency><topic>cmd_vel</topic>
</plugin>

<plugin>标签有两个属性：

• filename表示库文件的名称
• name表示插件的名称

在<left_joint>和<right_joint>标签中，我们定义了将左轮和右轮连接到机器人主体的关节，在我们的例子中分别是left_wheel_joint和right_wheel_joint。

• <wheel_separation>表示两个轮子之间的距离。我们的机器人左轮位于相对于底盘y轴0.6米处，右轮位于-0.6米处，因此轮间距是1.2米。
• <wheel_radius>表示轮子的半径，该半径在轮链接下的<radius>标签中定义。
• <odom_publish_frequency>设置在/model/vehicle_blue/odometry上发布里程计的频率。
• cmd_vel是DiffDrive插件的输入<topic>。

Step 6.2: 测试键值

1. 在一个终端启动机器人仿真环境：

$ gz sim building_robot.sdf

2. 在仿真界面右上角查找Key publisher插件；并添加到右侧列表中。

3. 在另一个终端打印topic值

$ gz topic -e -t /keyboard/keypress

可以获得按键与键值的对应关系：

• Left ← : 16777234
• Up ↑ : 16777235
• Right → : 16777236
• Down ↓ : 16777237

Step 6.3: 添加 Triggered publisher plugin

将每个箭头键（按键）与所需的消息（运动）进行映射：

• Left ➞ 16777234 ➞ linear: {x: 0.0}, angular: {z: 0.5}
• Up ➞ 16777235 ➞ linear: {x: 0.5}, angular: {z: 0.0}
• Right ➞ 16777236 ➞ linear: {x: 0.0}, angular: {z: -0.5}
• Down ➞ 16777237 ➞ linear: {x: -0.5}, angular: {z: 0.0}

<!-- Moving Forward--><!-- Moving Left--><plugin filename="gz-sim-triggered-publisher-system"name="gz::sim::systems::TriggeredPublisher"><input type="gz.msgs.Int32" topic="/keyboard/keypress"><match field="data">16777234</match></input><output type="gz.msgs.Twist" topic="/cmd_vel">linear: {x: 0.0}, angular: {z: 0.5}</output></plugin><!-- Moving Forward--><plugin filename="gz-sim-triggered-publisher-system"name="gz::sim::systems::TriggeredPublisher"><input type="gz.msgs.Int32" topic="/keyboard/keypress"><match field="data">16777235</match></input><output type="gz.msgs.Twist" topic="/cmd_vel">linear: {x: 0.5}, angular: {z: 0.0}</output></plugin><!-- Moving Right--><plugin filename="gz-sim-triggered-publisher-system"name="gz::sim::systems::TriggeredPublisher"><input type="gz.msgs.Int32" topic="/keyboard/keypress"><match field="data">16777236</match></input><output type="gz.msgs.Twist" topic="/cmd_vel">linear: {x: 0.0}, angular: {z: -0.5}</output></plugin><!-- Moving Backward--><plugin filename="gz-sim-triggered-publisher-system"name="gz::sim::systems::TriggeredPublisher"><input type="gz.msgs.Int32" topic="/keyboard/keypress"><match field="data">16777237</match></input><output type="gz.msgs.Twist" topic="/cmd_vel">linear: {x: -0.5}, angular: {z: 0.0}</output></plugin>

Step 7: 简单测试

$ gz sim moving_robot.sdf

测试资料：[SnapLearnGazebo/lesson_02_sensor}(https://github.com/SnapDragonfly/SnapLearnGazebo/tree/main/lesson_02_sensor)

3. 参考资料

【1】ArduPilot开源代码之ROS2Humble+CartographerSLAM+SITL+Gazebo
【2】ArduPilot飞控之Gazebo + SITL + MP的Jetson Orin环境搭建
【3】ArduPilot飞控之ubuntu22.04-Gazebo模拟
【4】PX4模块设计之七：Ubuntu 20.04搭建Gazebo模拟器