ROS2 使用 URDF 和 Xacro 创建机器人模型

编写完成后，在终端中进入 urdf 目录下，输入以下代码将 URDF 结构可视化，结果如下：

~/chapt6/chapt6_ws/src/fishbot_description/urdf$ urdf_to_graphviz first_robot.urdf first_robot
Created file first_robot.gv
Created file first_robot.pdf

打开生成的 first_robot.pdf 内容如下：

![图：URDF 结构可视化 PDF]

6.2 在 Rviz 中显示机器人

安装依赖：

sudo apt install ros-$ROS_DISTRO-robot-state-publisher
sudo apt install ros-$ROS_DISTRO-joint-state-publisher

为了方便运行，使用 launch 文件组织节点，在 fishbot_description 目录下新建 launch 目录，接着创建 display_robot.launch.py，写入以下代码：

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    urdf_tutorial_path = get_package_share_directory('fishbot_description')
    default_model_path = urdf_tutorial_path + '/urdf/first_robot.urdf'
    declare_model_arg = launch.actions.DeclareLaunchArgument(
        name='model',
        default_value=str(default_model_path),
        description='URDF 的绝对路径')
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(['cat ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str)
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}])
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',)
    rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',)
    return launch.LaunchDescription([
        declare_model_arg,
        joint_state_publisher_node,
        robot_state_publisher_node,
        rviz_node
    ])

代码完成后，在 CMakeLists.txt 中将 urdf 和 launch 文件复制到 install 目录下，代码如下：

install(DIRECTORY launch urdf DESTINATION share/${PROJECT_NAME})

随后在工作空间下构建功能包，source 并启动 launch 文件，启动命令与结果如下：

~/chapt6/chapt6_ws$ ros2 launch fishbot_description display_robot.launch.py
[INFO] [launch]: All log files can be found below...
[INFO] [joint_state_publisher-1]: process started with pid [...]
[INFO] [robot_state_publisher-2]: process started with pid [...]
[INFO] [rviz2-3]: process started with pid [...]
[robot_state_publisher-2]: got segment base_link
[robot_state_publisher-2]: got segment imu_link
[joint_state_publisher-1]: Waiting for robot_description to be published on the robot_description topic...
[rviz2-3]: Stereo is NOT SUPPORTED
[rviz2-3]: OpenGl version: 4.3 (GLSL 4.3)

需要将 Rviz 的 Display 模块中的 Fixed Frame 修改为 base_link，添加 RobotModel 并修改 Description Source 为 Topic，在 Description Topic 处选择 /robot_description。

在 launch 中添加启动 rviz2 命令配置：

def generate_launch_description():
    # ... (previous code)
    default_rviz_config_path = urdf_tutorial_path + '/config/display_model.rviz'
    # ...
    arguments=['-d', default_rviz_config_path]

修改完后，修改 CMakeLists.txt，将 config 目录复制到 install 对应目录下，重新构建功能包并运行 launch 文件，即可显示机器人模型。

6.3 使用 Xacro 简化 URDF

在 urdf 目录下创建 first_robot.urdf.xacro，编写以下代码：

<?xml version="1.0"?>
<!-- 机器人名称：first_robot -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="first_robot">
  <xacro:macro name="base_link" params="length radius">
    <!-- 1. 机器人身体部分（底座） -->
    <link name="base_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <cylinder length="${length}" radius="${radius}" />
        </geometry>
        <material name="white">
          <color rgba="1.0 1.0 1.0 0.5" />
        </material>
      </visual>
    </link>
  </xacro:macro>
  <xacro:macro name="imu_link" params="imu_name xyz">
    <!-- 2. 机器人 IMU 部件（惯性测量单元，用来测姿态） -->
    <link name="${imu_name}_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.02 0.02 0.02" />
        </geometry>
        <material name="black">
          <color rgba="0 0 0 0.5" />
        </material>
      </visual>
    </link>
    <!-- 3. 关节：连接 base_link（底座）和 imu_link（IMU） -->
    <joint name="${imu_name}_joint" type="fixed">
      <parent link="base_link" />
      <child link="${imu_name}_link" />
      <origin xyz="${xyz}" />
    </joint>
  </xacro:macro>
  <xacro:base_link length="0.12" radius="0.1" />
  <xacro:imu_link imu_name="imu_up" xyz="0 0 0.02" />
  <xacro:imu_link imu_name="imu_down" xyz="0 0 -0.02" />
</robot>

在终端中安装 xacro 工具，将 xacro 解析成 URDF 格式：

sudo apt install ros-$ROS_DISTRO-xacro

在终端进入 fishbot_description/urdf 目录，使用以下代码将 Xacro 解析为 URDF：

~/chapt6/chapt6_ws/src/fishbot_description/urdf$ xacro first_robot.urdf.xacro
# Output XML content... (auto-generated)

将 display_robot.launch.py 中的 'cat' 命令替换为 'xacro'。

在工作空间下重新构建和启动 launch 文件，并修改 model 参数为对应的文件地址，即可更新生成机器人模型：

~/chapt6/chapt6_ws$ ros2 launch fishbot_description display_robot.launch.py model:=/home/cwq/chapt6/chapt6_ws/src/fishbot_description/urdf/first_robot.urdf.xacro
# Log output showing segments loaded successfully

6.4 创建机器人及传感器部件

在 fishbot_description/urdf/目录下新建 fishbot 目录，在该目录下新建 base.urdf.xacro，写入以下代码：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="base_xacro" params="length radius">
    <link name="base_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <cylinder length="${length}" radius="${radius}" />
        </geometry>
        <material name="white">
          <color rgba="1.0 1.0 1.0 0.5" />
        </material>
      </visual>
    </link>
  </xacro:macro>
</robot>

在 fishbot 目录下新建 sensor 子目录，在 sensor 下新建 imu.urdf.xacro，写入以下代码：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="imu_xacro" params="xyz">
    <link name="imu_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.02 0.02 0.02" />
        </geometry>
        <material name="black">
          <color rgba="0 0 0 0.8" />
        </material>
      </visual>
    </link>
    <joint name="imu_joint" type="fixed">
      <parent link="base_link" />
      <child link="imu_link" />
      <origin xyz="${xyz}" />
    </joint>
  </xacro:macro>
</robot>

在 sensor 下新建 camera.urdf.xacro，写入以下代码：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="camera_xacro" params="xyz">
    <link name="camera_link">
      <visual>
        <origin xyz="0 0 0.0" rpy="0 0 0" />
        <geometry>
          <box size="0.02 0.10 0.02" />
        </geometry>
        <material name="green">
          <color rgba="0.0 1.0 0.0 0.8" />
        </material>
      </visual>
    </link>
    <joint name="camera_joint" type="fixed">
      <parent link="base_link" />
      <child link="camera_link" />
      <origin xyz="${xyz}" />
    </joint>
  </xacro:macro>
</robot>

在 sensor 下新建 laser.urdf.xacro，写入以下代码：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="laser_xacro" params="xyz">
    <link name="laser_cylinder_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <cylinder length="0.10" radius="0.01" />
        </geometry>
        <material name="green">
          <color rgba="0.0 1.0 0.0 0.8" />
        </material>
      </visual>
    </link>
    <joint name="laser_cylinder_joint" type="fixed">
      <parent link="base_link" />
      <child link="laser_cylinder_link" />
      <origin xyz="${xyz}" />
    </joint>
    <link name="laser_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <cylinder length="0.02" radius="0.02" />
        </geometry>
        <material name="green">
          <color rgba="0.0 1.0 0.0 0.8" />
        </material>
      </visual>
    </link>
    <joint name="laser_joint" type="fixed">
      <parent link="laser_cylinder_link" />
      <child link="laser_link" />
      <origin xyz="0 0 0.05" />
    </joint>
  </xacro:macro>
</robot>

传感器代码完成，现在需要将传感器组装起来，在 urdf/fishbot/下新建 fishbot.urdf.xacro，写入以下代码：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="first_robot">
  <xacro:include filename="$(find fishbot_description)/urdf/fishbot/base.urdf.xacro" />
  <xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/imu.urdf.xacro" />
  <xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/laser.urdf.xacro" />
  <xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/camera.urdf.xacro" />
  <xacro:base_xacro length="0.12" radius="0.1" />
  <xacro:imu_xacro xyz="0 0 0.02" />
  <xacro:laser_xacro xyz="0 0 0.10" />
  <xacro:camera_xacro xyz="0.10 0 0.075" />
</robot>

重新编译并运行 display_robot.launch.py(需将路径改为 fishbot.urdf.xacro)，代码及显示结果如下：

ros2 launch fishbot_description display_robot.launch.py model:=/home/cwq/chapt6/chapt6_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro
# Log output showing segments loaded successfully

![图：包含传感器的机器人模型]

6.5 完善机器人执行器部件

在 fishbot 目录下新建 actuator 子目录，然后在 actuator 下新建 wheel.urdf.xacro，编写以下代码：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="wheel_xacro" params="wheel_name xyz">
    <link name="${wheel_name}_wheel_link">
      <visual>
        <origin xyz="0 0 0" rpy="1.57079 0 0" />
        <geometry>
          <cylinder length="0.04" radius="0.032" />
        </geometry>
        <material name="yellow">
          <color rgba="1.0 1.0 0.0 0.8" />
        </material>
      </visual>
    </link>
    <joint name="${wheel_name}_wheel_joint" type="continuous">
      <parent link="base_link" />
      <child link="${wheel_name}_wheel_link" />
      <origin xyz="${xyz}" />
      <axis xyz="0.0 1.0 0.0"/>
    </joint>
  </xacro:macro>
</robot>

完成驱动轮的定义，接着定义万向轮部分，在 actuator 中新建 caster.urdf.xacro 文件，代码如下：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="caster_xacro" params="caster_name xyz">
    <link name="${caster_name}_caster_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <sphere radius="0.016" />
        </geometry>
        <material name="yellow">
          <color rgba="1.0 1.0 0.0 0.8" />
        </material>
      </visual>
    </link>
    <joint name="${caster_name}_caster_joint" type="fixed">
      <parent link="base_link" />
      <child link="${caster_name}_caster_link" />
      <origin xyz="${xyz}" />
      <axis xyz="0 0 0"/>
    </joint>
  </xacro:macro>
</robot>

完成后再次修改 fishbot.urdf.xacro，添加代码如下：

<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/caster.urdf.xacro" />
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/wheel.urdf.xacro" />
<xacro:wheel_xacro wheel_name="left" xyz="0 0.10 -0.06" />
<xacro:wheel_xacro wheel_name="right" xyz="0 -0.10 -0.06" />
<xacro:caster_xacro caster_name="front" xyz="0.08 0.0 -0.076" />
<xacro:caster_xacro caster_name="back" xyz="-0.08 0.0 -0.076" />

在 rviz 中观察机器人，发现机器人轮子在地面以下，通过添加虚拟关节来解决这一问题，在 src/fishbot_description/urdf/fishbot/base.urdf.xacro 中添加以下代码：

<link name="base_footprint" />
<joint name="base_joint" type="fixed">
  <parent link="base_footprint" />
  <child link="base_link" />
  <origin xyz="0.0 0.0 ${length/2.0+0.032-0.001}" rpy="0 0 0" />
</joint>

保存好后构建运行，修改 fixed frame 为 base_footprint，代码及结果展示如下：

ros2 launch fishbot_description display_robot.launch.py model:=/home/cwq/chapt6/chapt6_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro
# Log output showing base_footprint and other segments loaded

![图：修正后的机器人底盘模型]

6.6 添加物理属性让机器人更真实

6.6.1 为机器人添加碰撞属性

为每个部件都添加一份碰撞属性，与 visual 同级，添加 <collision>，将 visual 的内容直接复制粘贴即可，base, laser, wheel, caster 都添加一次。

</visual>
<collision>
  <origin xyz="0 0 0" rpy="0 0 0" />
  <geometry>
    <sphere radius="0.016" />
  </geometry>
  <material name="yellow">
    <color rgba="1.0 1.0 0.0 0.8" />
  </material>
</collision>
</link>

在终端运行，打开 rviz 后，修改 RobotModel 配置，去掉 Visual Enabled 的勾选，再勾选上 Collision Enabled 即可显示出碰撞模型。

![图：碰撞模型视图]

6.6.2 为机器人部件添加质量与惯性

使用 xacro 结合惯性公式，编写一个惯性和质量专用的宏定义，在 urdf/fishbot 目录下新建 common_inertia.xacro 文件，编写以下代码：

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:macro name="box_inertia" params="m w h d">
    <inertial>
      <mass value="${m}" />
      <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}" />
    </inertial>
  </xacro:macro>
  <xacro:macro name="cylinder_inertia" params="m r h">
    <inertial>
      <mass value="${m}" />
      <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy="0" ixz="0" iyy="${(m/12) * (3*r*r + h*h)}" iyz="0" izz="${(m/2) * (r*r)}" />
    </inertial>
  </xacro:macro>
  <xacro:macro name="sphere_inertia" params="m r">
    <inertial>
      <mass value="${m}" />
      <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}" />
    </inertial>
  </xacro:macro>
</robot>

编写完成后，就可以在其它 xacro 文件导入并使用该宏，修改 base.urdf.xacro，添加导入对该宏的调用：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
  <xacro:macro name="base_xacro" params="length radius">
    <link name="base_footprint" />
    <joint name="base_joint" type="fixed">
      <parent link="base_footprint" />
      <child link="base_link" />
      <origin xyz="0.0 0.0 ${length/2.0+0.032-0.001}" rpy="0 0 0" />
    </joint>
    <link name="base_link">
      <visual>
        <origin xyz="0 0 0.0" rpy="0 0 0" />
        <geometry>
          <cylinder length="${length}" radius="${length}" />
        </geometry>
        <material name="blue">
          <color rgba="0.1 0.1 1.0 0.5"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0.0" rpy="0 0 0" />
        <geometry>
          <cylinder length="${length}" radius="${length}" />
        </geometry>
        <material name="blue">
          <color rgba="0.1 0.1 1.0 0.5"/>
        </material>
      </collision>
      <xacro:cylinder_inertia m="1.0" r="${radius}" h="${length}"/>
    </link>
  </xacro:macro>
</robot>

这里给出的完整代码，读者可对照修改。再次构建并运行 launch，在 rviz 中显示机器人模型。

接着修改 robotmodel 配置，取消 visual enabled 的勾选，然后勾选 mass properties 选项中的 mass，即可看到质量配置视图，取消 mass 勾选，勾选 inertia 视图，即可看到惯性视图。