在 Gazebo Fortress 中加载 ROS 2 发布的 URDF 模型（一）

概述

本文使用使用的ROS2版本是Humble，仿真Gazbeo官方建议的是Fortress，进行AGV小车的仿真搭建。

 Gazebo Fortress 使用的是ignition，网上很多资料都是使用Gazebo老版本而不是ignition。比如老版本Gazebo启动是使用gazebo命令，而Fortress版本使用的是ign gazebo。网上使用ROS2 Humble结合Gazebo Fortress的教材比较少。

仿真效果展示：

gazebo仿真如下图所示：

对应的rviz数据展示如下图所示：

下面详细介绍各个文件的内容

详细代码

机器人外形描述文件定义：

<?xml version="1.0"?>
<!-- 该文件主要定义机器人外形描述 -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 导入机器人惯性计算 -->
    <xacro:include filename="inertial_macros.xacro"/>

    <xacro:property name="chassis_length" value="0.335"/>
    <xacro:property name="chassis_width" value="0.265"/>
    <xacro:property name="chassis_height" value="0.138"/>
    <xacro:property name="chassis_mass" value="1.0"/>
    <xacro:property name="wheel_radius" value="0.033"/>
    <xacro:property name="wheel_thickness" value="0.026"/>
    <xacro:property name="wheel_mass" value="0.05"/>
    <xacro:property name="wheel_offset_x" value="0.226"/>
    <xacro:property name="wheel_offset_y" value="0.1485"/>
    <xacro:property name="wheel_offset_z" value="0.01"/>
    <xacro:property name="caster_wheel_radius" value="0.01"/>
    <xacro:property name="caster_wheel_mass" value="0.01"/>
    <xacro:property name="caster_wheel_offset_x" value="0.075"/>
    <xacro:property name="caster_wheel_offset_z" value="${wheel_offset_z - wheel_radius + caster_wheel_radius}"/>

    <material name="white">
        <color rgba="1 1 1 1" />
    </material>

    <material name="orange">
        <color rgba="1 0.3 0.1 1"/>
    </material>

    <material name="blue">
        <color rgba="0.2 0.2 1 1"/>
    </material>

    <material name="black">
        <color rgba="0 0 0 1"/>
    </material>

    <material name="red">
        <color rgba="1 0 0 1"/>
    </material>

    <!-- BASE LINK -->

    <link name="base_link">

    </link>

    <!-- BASE_FOOTPRINT LINK -->

    <joint name="base_footprint_joint" type="fixed">
        <parent link="base_link"/>
        <child link="base_footprint"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>
    </joint>

    <link name="base_footprint">
    </link>


    <!-- CHASSIS LINK -->

    <joint name="chassis_joint" type="fixed">
        <parent link="base_link"/>
        <child link="chassis"/>
        <origin xyz="${-wheel_offset_x} 0 ${-wheel_offset_z}"/>
    </joint>

    <link name="chassis">
        <visual>
            <origin xyz="${chassis_length/2} 0 ${chassis_height/2}"/>
            <geometry>
                <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
            </geometry>
            <material name="orange"/>
        </visual>
        <collision>
            <origin xyz="${chassis_length/2} 0 ${chassis_height/2}"/>
            <geometry>
                <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
            </geometry>
        </collision>
        <xacro:inertial_box mass="0.5" x="${chassis_length}" y="${chassis_width}" z="${chassis_height}">
            <origin xyz="${chassis_length/2} 0 ${chassis_height/2}" rpy="0 0 0"/>
        </xacro:inertial_box>
    </link>

    <gazebo reference="chassis">
        <material>Gazebo/Orange</material>
    </gazebo>

    <!-- LEFT WHEEL LINK -->

    <joint name="left_wheel_joint" type="continuous">
        <parent link="base_link"/>
        <child link="left_wheel"/>
        <origin xyz="0 ${wheel_offset_y} 0" rpy="-${pi/2} 0 0" />
        <axis xyz="0 0 1"/>
    </joint>

    <link name="left_wheel">
        <visual>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="${wheel_mass}" length="${wheel_thickness}" radius="${wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="left_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>


    <!-- RIGHT WHEEL LINK -->

    <joint name="right_wheel_joint" type="continuous">
        <parent link="base_link"/>
        <child link="right_wheel"/>
        <origin xyz="0 ${-wheel_offset_y} 0" rpy="${pi/2} 0 0" />
        <axis xyz="0 0 -1"/>
    </joint>

    <link name="right_wheel">
        <visual>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="${wheel_mass}" length="${wheel_thickness}" radius="${wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="right_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>


    <!-- CASTER WHEEL LINK -->

    <joint name="caster_wheel_joint" type="fixed">
        <parent link="chassis"/>
        <child link="caster_wheel"/>
        <origin xyz="${caster_wheel_offset_x} 0 ${caster_wheel_offset_z}"/>
    </joint>


    <link name="caster_wheel">
        <visual>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_sphere mass="${caster_wheel_mass}" radius="${caster_wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_sphere>
    </link>

    <gazebo reference="caster_wheel">
        <material>Gazebo/White</material>
        <mu1 value="0.001"/>
        <mu2 value="0.001"/>
    </gazebo>

</robot>

机器人控制ros2_control定义

当使用Gazebo Fortress进行仿真时，需要使用ign-ros2-control这个插件。首先需要安装该插件：

sudo apt-get install ros-humble-ign-ros2-control

然后在xacro中添加该插件：

<hardware>
    <plugin>ign_ros2_control/IgnitionSystem</plugin>
</hardware>

配置gazebo中的控制插件

<gazebo>
    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
        <parameters>$(find agv_sim)/config/my_controllers.yaml</parameters>
    </plugin>
</gazebo>

 其中my_controllers.yaml为ros2_control的配置文件，如下：

controller_manager:
  ros__parameters:
    update_rate: 30  # 控制器管理器的更新频率，单位为赫兹（Hz）。
    use_sim_time: true  # 使用仿真时间，通常在仿真环境中设置为true。
    diff_cont:
      type: diff_drive_controller/DiffDriveController  # 定义差速驱动控制器的类型。
    joint_broad:
      type: joint_state_broadcaster/JointStateBroadcaster  # 定义关节状态广播器的类型。

diff_cont:
  ros__parameters:
    publish_rate: 50.0  # 发布频率，表示差速驱动控制器的数据发布频率，单位为赫兹（Hz）。
    base_frame_id: base_link  # 基坐标系
    left_wheel_names: ['left_wheel_joint']  # 左侧车轮关节的名称列表。
    right_wheel_names: ['right_wheel_joint']  # 右侧车轮关节的名称列表。
    wheel_separation: 0.297  # 车轮之间的距离（车轮轴距），单位为米（m）。
    wheel_radius: 0.033  # 车轮半径，单位为米（m）。
    use_stamped_vel: false  # 是否使用带时间戳的速度消息，false表示不使用。

 完整的ros2_control.xacro文件如下：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> 

    <!-- 仿真环境配置 -->
    <ros2_control name="IgnitionSystem" type="system"> <!-- 定义一个ROS2控制系统，名称为IgnitionSystem -->
        <hardware>
            <plugin>ign_ros2_control/IgnitionSystem</plugin> <!-- 硬件插件，用于Ignition Gazebo仿真系统 -->
        </hardware>
        <joint name="left_wheel_joint"> <!-- 定义左轮关节 -->
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
            <state_interface name="velocity"/>
            <state_interface name="position"/>
        </joint>
        <joint name="right_wheel_joint"> <!-- 定义右轮关节 -->
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
                <state_interface name="velocity"/>
                <state_interface name="position"/>
        </joint>
    </ros2_control>

    <!-- Gazebo仿真插件配置 -->
    <gazebo>
        <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin"> <!-- Gazebo插件配置，用于将ROS2控制系统与Ignition Gazebo集成 -->
            <parameters>$(find agv_sim)/config/my_controllers.yaml</parameters> <!-- 控制器参数文件 -->
        </plugin>
    </gazebo>
</robot>

相机传感器定义 

具体定义xacro文件如下，注意其中<gazebo>仿真的部分，这个是Gazebo Fortress官方的写法，网上很多是老版本Gazebo的写法。参考官方的例子为：gz-sim/examples/worlds/camera_sensor.sdf at ign-gazebo5 · gazebosim/gz-sim (github.com)

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 摄像头的固定关节 -->
    <joint name="camera_joint" type="fixed">  <!-- 定义了一个名为camera_joint的固定类型的关节 -->
        <parent link="chassis"/>  <!-- 父链接为底盘(chassis) -->
        <child link="camera_link"/>  <!-- 子链接为摄像头链接(camera_link) -->
        <origin xyz="0.276 0 0.181" rpy="0 0.18 0"/>  <!-- 定义了关节的位置和姿态 -->
    </joint>

    <!-- 摄像头链接 -->
    <link name="camera_link"> 
        <visual>  <!-- 定义在仿真中的外观 -->
            <geometry>
                <box size="0.010 0.03 0.03"/>  <!-- 使用一个长方体作为几何形状 -->
            </geometry>
            <material name="black"/>  <!-- 黑色材质 -->
        </visual>
        <visual>  <!-- 外观细节 -->
            <origin xyz="0 0 -0.05"/>  <!-- 定义了视觉元素的位置偏移 -->
            <geometry>
                <cylinder radius="0.002" length="0.1"/>  <!-- 使用柱体作为几何形状 -->
            </geometry>
            <material name="black"/>  <!-- 黑色材质 -->
        </visual>
    </link>

    <!-- 摄像头关节 -->
    <joint name="camera_optical_joint" type="fixed">  
        <parent link="camera_link"/>  <!-- 父链接为camera_link -->
        <child link="camera_link_optical"/>  <!-- 子链接为camera_link_optical -->
        <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>  <!-- 定义了光学关节的姿 -->
    </joint>

    <!-- 摄像头的光学链接 -->
    <link name="camera_link_optical"></link>  

    <!-- Gazebo仿真配置 -->
    <gazebo reference="camera_link">  <!-- Gazebo仿真部分的配置，参考链接为camera_link -->
        <material>Gazebo/Black</material>  <!-- 定义材质为Gazebo内置的黑色 -->
        <pose> 0 0 0 0 0 0 </pose>  <!-- 定义了该对象的姿态 -->
        <sensor type="camera" name="camera">  <!-- 定义了一个类型为camera的传感器 -->
            <always_on>true</always_on>  <!-- 始终开启 -->
            <ignition_frame_id>camera_link</ignition_frame_id>  <!-- Ignition坐标系的ID -->
            <visualize>true</visualize>  <!-- 使传感器在仿真中可见 -->
            <topic>/camera</topic>  <!-- 传感器数据发布的主题 -->
            <update_rate>15.0</update_rate>  <!-- 传感器数据的更新频率，单位为赫兹 -->
            <camera name="camera">
                <horizontal_fov>1.089</horizontal_fov>  <!-- 水平视场角，弧度 -->
                <image>
                    <width>640</width>  <!-- 图像的宽度素 -->
                    <height>480</height>  <!-- 图像的高度 -->
                    <format>R8G8B8</format>  <!-- 图像的格式 -->
                </image>
                <clip>
                    <near>0.05</near>  <!-- 最近距离 -->
                    <far>10.0</far>  <!-- 最远距离 -->
                </clip>
            </camera>
        </sensor>
    </gazebo>

    <!-- Gazebo中的插件配置 -->
    <gazebo>
        <!-- 相机插件 -->
        <plugin filename="ignition-gazebo-sensors-system" name="gz::sim::systems::Sensors">
            <render_engine>ogre2</render_engine>  <!-- 渲染引擎类型 -->
        </plugin>
    </gazebo>

</robot>

激光雷达传感器配置

参考官方资料：ros_gz/ros_gz_point_cloud/examples/gpu_lidar.sdf at ros2 · gazebosim/ros_gz (github.com)

最后定义的lidar.xacro文件内容如下：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> 
    <!-- 激光雷达传感器的固定关节 -->
    <joint name="laser_joint" type="fixed">
        <parent link="chassis"/> <!-- 父链接为底盘(chassis) -->
        <child link="laser_frame"/> <!-- 子链接为激光雷达(laser_frame) -->
        <origin xyz="0.122 0 0.212" rpy="0 0 0"/> <!-- 关节的位姿 -->
    </joint>

    <!-- 激光雷达的链接 -->
    <link name="laser_frame"> 
        <visual> <!-- 仿真中的外观 -->
            <geometry>
                <cylinder radius="0.05" length="0.04"/> <!-- 圆柱体作为几何形状 -->
            </geometry>
            <material name="black"/> <!-- 黑色材质 -->
        </visual>
        <visual> <!-- 第二个视觉外观 -->
            <origin xyz="0 0 -0.05"/> <!-- 位置偏移 -->
            <geometry>
                <cylinder radius="0.01" length="0.1"/> <!-- 圆柱体作为几何形状 -->
            </geometry>
            <material name="black"/> <!-- 黑色材质 -->
        </visual>
        <collision> <!-- 碰撞元素，用于定义链接在物理引擎中的碰撞形状 -->
            <geometry>
                <cylinder radius="0.05" length="0.04"/> <!-- 与视觉相同的圆柱体 -->
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05"> <!-- 惯性属性 -->
            <origin xyz="0 0 0" rpy="0 0 0"/> <!-- 惯性元素的位姿 -->
        </xacro:inertial_cylinder>
    </link>

    <!-- Gazebo仿真配置 -->
    <gazebo reference="laser_frame">
        <material>Gazebo/Black</material> <!-- 材质为Gazebo内置的黑色 -->

        <!-- 激光雷达传感器 -->
        <sensor name='gpu_lidar' type='gpu_lidar'>
            <topic>lidar</topic> <!-- 激光雷达数据发布的主题 -->
            <update_rate>10</update_rate> <!-- 传感器数据的更新频率，单位为赫兹 -->
            <gz_frame_id>laser_frame</gz_frame_id> 
            <lidar>
                <scan> 
                    <horizontal> <!-- 水平扫描参数 -->
                        <samples>640</samples> <!-- 水平扫描的样本数量 -->
                        <resolution>1</resolution> <!-- 水平扫描的分辨率 -->
                        <min_angle>-3.141593</min_angle> <!-- 最小扫描角度（-180度） -->
                        <max_angle>3.141593</max_angle> <!-- 最大扫描角度（180度） -->
                    </horizontal>
                    <vertical> <!-- 垂直扫描参数 -->
                        <samples>1</samples> <!-- 垂直扫描的样本数量 -->
                        <resolution>1</resolution> <!-- 垂直扫描的分辨率 -->
                        <min_angle>0</min_angle> <!-- 最小垂直角度 -->
                        <max_angle>0</max_angle> <!-- 最大垂直角度 -->
                    </vertical>
                </scan>
                <range> <!-- 测距配置 -->
                    <min>0.08</min> <!-- 最小测距距离 -->
                    <max>10.0</max> <!-- 最大测距距离 -->
                    <resolution>0.01</resolution> <!-- 测距的分辨率 -->
                </range>
            </lidar>
            <always_on>true</always_on> <!-- 传感器始终开启 -->
            <visualize>true</visualize> <!-- 在仿真中可视化传感器数据 -->
            
            <!-- 激光雷达点云数据插件 -->
            <plugin filename="RosGzPointCloud" name="ros_gz_point_cloud::PointCloud">
                <namespace>custom_params</namespace> <!-- 自定义命名空间 -->
                <topic>pc2</topic> <!-- 点云数据发布的主题 -->
                <frame_id>laser_frame</frame_id> <!-- 点云数据的参考框架ID -->
            </plugin>
        </sensor>
    </gazebo>

</robot>

 机器人总体urdf.xacro

把上面的几个xacro添加到机器人总体urdf.xacro文件：robot.urdf.xacro具体内容如下：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot">
    <xacro:arg name="use_ros2_control" default="true"/> <!-- 定义参数use_ros2_control，默认为true -->
    <xacro:arg name="sim_mode" default="true"/> <!-- 定义参数sim_mode，默认为true -->

    <!-- 机器人描述文件 -->
    <xacro:include filename="robot_core.xacro" />

    <!-- 使用ros2_control -->
    <xacro:include filename="ros2_control.xacro" /> <!-- 包含ros2_control配置文件 -->

    <!-- 传感器描述文件 -->
    <xacro:include filename="camera.xacro" /> <!-- 包含相机描述文件 -->
    <xacro:include filename="lidar.xacro" /> <!-- 包含激光雷达描述文件 -->
</robot>

 launch.py文件

机器人描述发布launch.py文件:

核心代码如下：

def generate_launch_description()
    use_sim_time = LaunchConfiguration("use_sim_time")  # 使用仿真时间

    # 处理URDF文件的路径
    pkg_path = os.path.join(get_package_share_directory("agv_sim"))  # 获取包的目录
    xacro_file = os.path.join(pkg_path, "description", "robot.urdf.xacro")  # xacro文件路径

    # 使用xacro命令生成机器人描述
    robot_description_config = Command(
        [
            "xacro ",  # 使用xacro命令来处理xacro文件
            xacro_file,
            " sim_mode:=",
            use_sim_time,
        ]
    )

    # 创建机器人状态发布节点的参数
    params = {
        "robot_description": robot_description_config,  # 将处理后的机器人描述作为参数
        "use_sim_time": use_sim_time,  # 使用仿真时间
    }

    # 创建robot_state_publisher节点，用于发布机器人状态
    node_robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[params],  # 传入参数
    )

    # 创建并返回启动描述符
    return LaunchDescription(
        [
            DeclareLaunchArgument(
                "use_sim_time",
                default_value="true",
                description="Use sim time if true",
            ),
            node_robot_state_publisher,  # 启动robot_state_publisher节点
        ]
    )

gazebo仿真启动launch.py文件

核心代码如下：

# 启动gazebo
world = path.join(
    get_package_share_directory(package_name),
    "worlds",
    "empty.sdf",
)
gazebo = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        PathJoinSubstitution(
            [
                FindPackageShare("ros_gz_sim"),
                "launch",
                "gz_sim.launch.py",
            ]
        )
    ),
    launch_arguments=[("gz_args", [world, " -r -v ", "3"])],
)

# 在gazebo中创建机器人
ros_gz_sim_create = Node(
    package="ros_gz_sim",
    executable="create",
    output="log",
    arguments=[
        "-x",
        "0",
        "-y",
        "0",
        "-z",
        "1",
        "-topic",
        "robot_description",
        "--ros-args",
        "--log-level",
        "warn",
    ],
    parameters=[{"use_sim_time": True}],
)

ros2_control控制启动launch.py文件

关键代码如下：

# 驱动控制器
diff_drive_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["diff_cont"],
    output="screen",
    parameters=[{"use_sim_time": True}],
)

joint_broad_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["joint_broad"],
    output="screen",
    parameters=[{"use_sim_time": True}],
)

return LaunchDescription(
    [
        diff_drive_spawner,
        joint_broad_spawner,
    ]
)

其中diff_cont和joint_broad分别对应my_controllers.yaml中定义的diff_cont和joint_broad。

最后使用ros2 launch agv_sim launch_robot.launch.py启用得到文章开头仿真效果展示的效果。

待续

关于gazebo和ros2之间的数据通信在后续内容中补充。