ROS2之仿真

机器人的建模和仿真一般用于实机部署之前进行算法的虚拟测试

机器人建模的核心文件：URDF

URDF 基本组成

• <link>：刚体部分（如底盘、机械臂连杆）

• <joint>：连接关系（如旋转、滑动）

• <inertial>：惯性参数（质量、质心、转动惯量）

• <visual>：视觉外观（mesh 文件，如 .dae、.stl）

• <collision>：碰撞检测模型

• <transmission>：定义执行器与 joint 的关系

创建一个urdf的功能包无需其余依赖，使用ament_cmake方式进行构建生成可执行文件，在下面新建一个urdf文件夹用于存放urdf文件

<?xml version="1.0"?>
<!-- 声明 XML 文件头，URDF 是一种 XML 格式的描述语言 -->

<robot name="my_robot">
<!-- 定义一个机器人模型，名字为 "my_robot" -->
    <link name="base_link">
        <!-- link 是机器人中的刚体（不发生形变的物理部分） -->
        <visual>
            <!-- visual 定义用于可视化（显示）的几何信息 -->
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <!-- origin 表示几何形状相对于 link 坐标系的位姿（位置+姿态） -->
            <!-- xyz 表示平移（x, y, z），rpy 表示旋转（roll, pitch, yaw） -->
            <geometry>
                <!-- geometry 定义该 link 的形状 -->
                <cylinder radius="0.10" length="0.12"/>
                <!-- 定义一个圆柱体，半径为 0.10m，高度为 0.12m -->
            </geometry>
        </visual>
        <material>
            <!-- material 定义外观颜色或纹理 -->
            <color rgba="1.0 1.0 1.0 0.5"/>
            <!-- rgba: 红、绿、蓝、透明度（0=透明, 1=不透明） -->
        </material>
    </link>

    <link name="imu_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <!-- IMU 的外观几何原点在自身坐标系的原点 -->
            <geometry>
                <box size="0.02 0.02 0.02"/>
                <!-- 定义一个 2cm 立方体，用于表示 IMU 模块 -->
            </geometry>
        </visual>
        <material>
            <color rgba="0.0 0.0 0.0 0.5"/>
            <!-- 颜色为半透明黑色 -->
        </material>
    </link>

    <joint name="imu_joint" type="fixed">
        <!-- joint 定义两个 link 之间的连接关系 -->
        <!-- name 为关节名称，type 为关节类型 -->
        <!-- type="fixed" 表示两个 link 之间无相对运动 -->
        <parent link="base_link"/>
        <!-- 父连杆（主结构体） -->
        <child link="imu_link"/>
        <!-- 子连杆（被附着的模块） -->
        <origin xyz="0.03 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <!-- 表示 imu_link 坐标系相对于 base_link 坐标系的位姿 -->
    </joint>

</robot>

urdf_to_graphviz：一个 ROS 工具，用于将 URDF 模型的结构（link-joint 关系）可视化为拓扑图，作用为读取 .urdf 文件，分析 link 与 joint 的连接关系，使用 Graphviz 生成 .gv（graphviz 源文件）和 .pdf（图形文件）。

urdf_to_graphviz robot.urdf将会生成两个文件，这两个文件的名字和xml里面name属性的赋值一致

使用rviz2打开可视化工具，读取urdf文件，可视化展示，我们选择从文件中获取同时设置Fixed Frame（固定坐标系）为base_link

安装两个工具包，sudo apt install ros-$ROS_DISTRO-robot-state-publisher和sudo apt install ros-$ROS_DISTRO-joint-state-publisher

包名	节点名	功能作用	输入	输出
robot_state_publisher	/robot_state_publisher	根据 URDF 模型和 joint 状态生成 TF 树（坐标变换关系）	/joint_states（joint 角度） /robot_description（URDF）	/tf、/tf_static
joint_state_publisher	/joint_state_publisher	发布机器人各关节的角度状态，用于驱动 URDF 中的关节	——（手动或 GUI 输入）	/joint_states

这两个包涉及的节点和话题入下图所示

相关节点

节点名称	角色	主要功能
/robot_state_publisher	机器人状态发布节点（核心）	根据 URDF 模型（robot_description）和关节角度（joint_states）计算各个 link 的坐标变换（TF），并发布到 /tf 和 /tf_static
/joint_state_publisher	关节状态发布节点	向话题 /joint_states 发布各关节的角度、速度、位移等信息（一般由 GUI 或真实传感器产生）
/transform_listener_impl_...	TF 监听节点（例如 RViz 或 TF 工具）	订阅 /tf 与 /tf_static，以便获取机器人各个部分的空间位置关系（用于 3D 显示或坐标变换）
/robot_description	参数（非真正节点）	存储机器人的 URDF 模型（XML 字符串），供 robot_state_publisher 读取

相关话题

话题名称	发布者	订阅者	作用
/robot_description	launch 或 parameter server	/robot_state_publisher	提供 URDF 模型内容（机器人结构）
/joint_states	/joint_state_publisher	/robot_state_publisher	提供关节角度、速度、位移信息
/tf	/robot_state_publisher	/transform_listener_impl_xxxxx	动态 TF（随时间变化的坐标关系）
/tf_static	/robot_state_publisher	/transform_listener_impl_xxxxx	静态 TF（固定坐标变换，如 base_link→imu_link）

创建launch文件启动这些节点

from launch import LaunchDescription                    # ROS2 启动文件的核心类，用于定义要启动的所有节点/动作
from launch.actions import DeclareLaunchArgument        # 声明可在命令行传入的启动参数
from launch.substitutions import Command, LaunchConfiguration  # Launch 变量替换机制（动态获取参数或命令结果）
from launch_ros.actions import Node                     # ROS2 节点启动动作
from launch_ros.descriptions import ParameterValue       # ROS 参数值类型
from ament_index_python.packages import get_package_share_directory  # 获取包路径的工具函数
import os


def generate_launch_description():
    # 获取 URDF 模型路径
    urdf_package_path = get_package_share_directory('robot_description')
    default_urdf_path = os.path.join(urdf_package_path,'urdf','robot.urdf')
    # 声明 Launch 参数
    # 区分launch参数和节点参数，launch 参数管 “启动怎么配”（启动阶段、全局控制），节点参数管 “运行怎么算”（运行阶段、节点内部配置）
    action_declare_arg_mode_path=DeclareLaunchArgument('model',default_value=str(default_urdf_path))
    # 读取 URDF 文件内容,cat有空格
    command_result = Command(['cat ',LaunchConfiguration('model')])
    # 告诉 ROS 这是一个字符串类型的参数值（即 URDF 模型文本）
    robot_description_value = ParameterValue(command_result,value_type=str)
    '''
      启动节点:
   ├─ joint_state_publisher → 发布 JointState
   ├─ robot_state_publisher → 发布 TF 树
   └─ rviz2 → 显示模型
    '''
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}],
    )
    robot_joint_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
    )
    robot_rviz_node = Node(
        package='rviz2',
        executable='rviz2',
    )
    return LaunchDescription(
        [action_declare_arg_mode_path,
        robot_state_publisher,
        robot_joint_publisher,
        robot_rviz_node,]
    )

在robot_description下新建目录config，将配置保存到该目录下得到.rviz文件然后修改代码和cmakelist.txt

# 复制config目录到install
install(DIRECTORY launch urdf config
 DESTINATION share/${PROJECT_NAME}
)

from launch import LaunchDescription                    # ROS2 启动文件的核心类，用于定义要启动的所有节点/动作
from launch.actions import DeclareLaunchArgument        # 声明可在命令行传入的启动参数
from launch.substitutions import Command, LaunchConfiguration  # Launch 变量替换机制（动态获取参数或命令结果）
from launch_ros.actions import Node                     # ROS2 节点启动动作
from launch_ros.descriptions import ParameterValue       # ROS 参数值类型
from ament_index_python.packages import get_package_share_directory  # 获取包路径的工具函数
import os


def generate_launch_description():
    # 获取 URDF 模型路径
    urdf_package_path = get_package_share_directory('robot_description')
    default_urdf_path = os.path.join(urdf_package_path,'urdf','robot.urdf')
    default_rviz_path = os.path.join(urdf_package_path,'config','display_robot.rviz')
    # 声明 Launch 参数
    action_declare_arg_mode_path=DeclareLaunchArgument('model',default_value=str(default_urdf_path))
    # 读取 URDF 文件内容,cat有空格
    command_result = Command(['cat ',LaunchConfiguration('model')])
    # 告诉 ROS 这是一个字符串类型的参数值（即 URDF 模型文本
    robot_description_value = ParameterValue(command_result,value_type=str)
    '''
      启动节点:
   ├─ joint_state_publisher → 发布 JointState
   ├─ robot_state_publisher → 发布 TF 树
   └─ rviz2 → 显示模型
    '''
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}],
    )
    robot_joint_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
    )
    robot_rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d',default_rviz_path], 
    )
    return LaunchDescription(
        [action_declare_arg_mode_path,
        robot_state_publisher,
        robot_joint_publisher,
        robot_rviz_node,]
    )

Xacro

Xacro是 ROS 中定义机器人模型最常用的增强版 URDF 语言，它是 URDF 的宏扩展语言，让 URDF 文件可以使用变量、宏、条件、循环等高级功能。

项目	URDF	Xacro
文件后缀	.urdf	.xacro
是否支持变量	❌ 不支持	✅ 支持
是否支持复用结构（宏）	❌ 不支持	✅ 支持
是否支持条件语句	❌ 不支持	✅ 支持
是否支持数学计算	❌ 不支持	✅ 支持
解析方式	直接加载	必须先转成 URDF 再加载

引入xacro的命名空间

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">

常见属性

标签	功能	示例
<xacro:property>	定义变量	<xacro:property name="r" value="0.1"/>
<xacro:macro>	定义宏	<xacro:macro name="wheel" params="name r">...</xacro:macro>
<xacro:include>	引入文件	<xacro:include filename="wheel.xacro"/>
<xacro:arg>	定义参数	<xacro:arg name="use_lidar" default="true"/>
<xacro:if> / <xacro:unless>	条件判断	<xacro:if value="$(arg use_lidar)">...</xacro:if>
<xacro:insert_block>	插入块	<xacro:insert_block name="link_content"/>
${}	表达式或变量引用	${r/2}

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

  <!-- ========= 全局颜色属性定义 ========= -->
  <xacro:property name="base_color_rgba" value="0.2 0.2 0.2 1.0"/>   <!-- 深灰底座 -->
  <xacro:property name="imu1_color_rgba" value="0.1 0.6 1.0 1.0"/>   <!-- 明亮蓝色 IMU1 -->
  <xacro:property name="imu2_color_rgba" value="1.0 0.5 0.0 1.0"/>   <!-- 明亮橙色 IMU2 -->
  <xacro:property name="imu3_color_rgba" value="0.2 1.0 0.2 1.0"/>   <!-- 明亮绿色 IMU3 -->
  <xacro:property name="imu4_color_rgba" value="1.0 0.2 0.6 1.0"/>   <!-- 粉红 IMU4 -->
  <xacro:property name="imu5_color_rgba" value="1.0 1.0 0.1 1.0"/>   <!-- 明亮黄色 IMU5 -->

  <!-- ========= 定义底座 link 宏 ========= -->
  <xacro:macro name="base_link" params="base_name xyz rpy radius length rgba">
    <link name="${base_name}">
      <!-- 可视部分 -->
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="base_color">
          <color rgba="${rgba}"/>
        </material>
      </visual>
    </link>
  </xacro:macro>

  <!-- ========= 定义 IMU link 宏 ========= -->
  <xacro:macro name="imu_link" params="imu_name xyz rpy size rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${imu_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <box size="${size}"/>
        </geometry>
        <material name="${imu_name}_color">
          <color rgba="${rgba}"/>
        </material>
      </visual>
    </link>

    <!-- joint: 连接 IMU 与底座 -->
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
    </joint>
  </xacro:macro>

  <!-- ========= 生成实际模型 ========= -->
  <!-- 底座 -->
  <xacro:base_link 
    base_name="base_link"
    xyz="0 0 0"
    rpy="0 0 0"
    radius="0.1"
    length="0.12"
    rgba="${base_color_rgba}"/>

  <!-- IMU 1（蓝色） 正上方中心 -->
  <xacro:imu_link 
    imu_name="imu_link_1"
    xyz="0 0 0.15"
    rpy="0 0 0"
    size="0.02 0.02 0.02"
    rgba="${imu1_color_rgba}"
    joint_name="base_to_imu1"
    joint_type="fixed"
    parent_name="base_link"
    child_name="imu_link_1"
    joint_xyz="0 0 0.15"
    joint_rpy="0 0 0"/>

  <!-- IMU 2（橙色） 前方 -->
  <xacro:imu_link 
    imu_name="imu_link_2"
    xyz="0.1 0 0.12"
    rpy="0 0 0"
    size="0.02 0.02 0.02"
    rgba="${imu2_color_rgba}"
    joint_name="base_to_imu2"
    joint_type="fixed"
    parent_name="base_link"
    child_name="imu_link_2"
    joint_xyz="0.1 0 0.12"
    joint_rpy="0 0 0"/>

  <!-- IMU 3（绿色） 后方 -->
  <xacro:imu_link 
    imu_name="imu_link_3"
    xyz="-0.1 0 0.12"
    rpy="0 0 0"
    size="0.02 0.02 0.02"
    rgba="${imu3_color_rgba}"
    joint_name="base_to_imu3"
    joint_type="fixed"
    parent_name="base_link"
    child_name="imu_link_3"
    joint_xyz="-0.1 0 0.12"
    joint_rpy="0 0 0"/>

  <!-- IMU 4（粉红） 左方 -->
  <xacro:imu_link 
    imu_name="imu_link_4"
    xyz="0 0.1 0.12"
    rpy="0 0 0"
    size="0.02 0.02 0.02"
    rgba="${imu4_color_rgba}"
    joint_name="base_to_imu4"
    joint_type="fixed"
    parent_name="base_link"
    child_name="imu_link_4"
    joint_xyz="0 0.1 0.12"
    joint_rpy="0 0 0"/>

  <!-- IMU 5（黄色） 右方 -->
  <xacro:imu_link 
    imu_name="imu_link_5"
    xyz="0 -0.1 0.12"
    rpy="0 0 0"
    size="0.02 0.02 0.02"
    rgba="${imu5_color_rgba}"
    joint_name="base_to_imu5"
    joint_type="fixed"
    parent_name="base_link"
    child_name="imu_link_5"
    joint_xyz="0 -0.1 0.12"
    joint_rpy="0 0 0"/>

</robot>

from launch import LaunchDescription                    # ROS2 启动文件的核心类，用于定义要启动的所有节点/动作
from launch.actions import DeclareLaunchArgument        # 声明可在命令行传入的启动参数
from launch.substitutions import Command, LaunchConfiguration  # Launch 变量替换机制（动态获取参数或命令结果）
from launch_ros.actions import Node                     # ROS2 节点启动动作
from launch_ros.descriptions import ParameterValue       # ROS 参数值类型
from ament_index_python.packages import get_package_share_directory  # 获取包路径的工具函数
import os


def generate_launch_description():
    # 获取 URDF 模型路径
    urdf_package_path = get_package_share_directory('robot_description')
    default_urdf_path = os.path.join(urdf_package_path,'urdf','robot.xacro')
    default_rviz_path = os.path.join(urdf_package_path,'config','display_robot.rviz')
    # 声明 Launch 参数
    action_declare_arg_mode_path=DeclareLaunchArgument('model',default_value=str(default_urdf_path))
    # 读取 xacro 文件内容,xacro有空格,通过命令转换为urdf让rviz工具可以分析
    command_result = Command(['xacro ',LaunchConfiguration('model')])
    # 告诉 ROS 这是一个字符串类型的参数值（即 URDF 模型文本
    robot_description_value = ParameterValue(command_result,value_type=str)
    '''
      启动节点:
   ├─ joint_state_publisher → 发布 JointState
   ├─ robot_state_publisher → 发布 TF 树
   └─ rviz2 → 显示模型
    '''
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}],
    )
    robot_joint_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
    )
    robot_rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d',default_rviz_path], 
    )
    return LaunchDescription(
        [action_declare_arg_mode_path,
        robot_state_publisher,
        robot_joint_publisher,
        robot_rviz_node,]
    )

通过上面的例子可以看到xacro的出现本质上是面向对象的思想运用到urdf文件中，通过定义宏将部件简化为一个模板，对于有相似属性的部件，通过模板可以创建多个极大的简化了代码量，如果是urdf原始写法，上面五个imu部件就要写五给link标签，而xacro就简化为了一个模板，传入不同的值就可以定义多个不同的imu部件。

注意有些名称的定义都是静态的，例如宏的名称是静态的，不能用变量插入，比如 <xacro:macro name="${var}" params="${var}"> ，<xacro:property name="${var}"/> 和 <xacro:${macro_name}/>这些都不能动态变化，而其余的标签里面的属性可以进行动态插入。

创建一个自己的机器人

该机器人以mybot整合，包含底盘，传感器和执行器三大块

 底盘部件为机器人的主体几何

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
      <xacro:macro name="base_xacro" params="base_name xyz rpy radius length base_color rgba">
    <link name="${base_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${base_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
    </link>
    <!-- 虚拟部件用于贴合地面 -->
    <link name="base_footprint"> </link>
    <joint name="joint_name" type="fixed">
      <parent link="base_footprint"/>
      <child link="${base_name}"/>
      <origin xyz="0.0 0.0 ${length/2.0+0.032-0.01}" rpy="0.0 0.0 0.0"/>
    </joint>
  </xacro:macro>
</robot>

 传感器包括相机，imu和雷达三大传感器

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="camera_xacro" params="camera_name xyz rpy size camera_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${camera_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <box size="${size}"/>
        </geometry>
        <material name="${camera_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
    </link>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
    </joint>
  </xacro:macro>
</robot>

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="imu_xacro" params="imu_name xyz rpy size imu_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${imu_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <box size="${size}"/>
        </geometry>
        <material name="${imu_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
    </link>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
    </joint>
  </xacro:macro>
</robot>

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

  <!-- 激光雷达组件宏 -->
  <xacro:macro name="laser_xacro" params="
      laser_name
      parent_name
      joint1_name joint1_type joint1_xyz joint1_rpy
      joint2_name joint2_type joint2_xyz joint2_rpy
      cylinder_xyz cylinder_rpy cylinder_radius cylinder_length cylinder_color cylinder_rgba
      sensor_xyz sensor_rpy sensor_radius sensor_length sensor_color sensor_rgba">

    <!-- 支撑杆 -->
    <link name='${laser_name}_cylinder_link'>
      <visual>
        <origin xyz='${cylinder_xyz}' rpy='${cylinder_rpy}'/>
        <geometry>
          <cylinder radius='${cylinder_radius}' length='${cylinder_length}'/>
        </geometry>
        <material name='${cylinder_color}'>
          <color rgba='${cylinder_rgba}'/>
        </material>
      </visual>
    </link>

    <!-- 激光雷达主体 -->
    <link name='${laser_name}_sensor_link'>
      <visual>
        <origin xyz='${sensor_xyz}' rpy='${sensor_rpy}'/>
        <geometry>
          <cylinder radius='${sensor_radius}' length='${sensor_length}'/>
        </geometry>
        <material name='${sensor_color}'>
          <color rgba='${sensor_rgba}'/>
        </material>
      </visual>
    </link>

    <!-- 关节1：底座连接支撑杆 -->
    <joint name='${joint1_name}' type='${joint1_type}'>
      <parent link='${parent_name}'/>
      <child link='${laser_name}_cylinder_link'/>
      <origin xyz='${joint1_xyz}' rpy='${joint1_rpy}'/>
    </joint>

    <!-- 关节2：支撑杆连接激光雷达 -->
    <joint name='${joint2_name}' type='${joint2_type}'>
      <parent link='${laser_name}_cylinder_link'/>
      <child link='${laser_name}_sensor_link'/>
      <origin xyz='${joint2_xyz}' rpy='${joint2_rpy}'/>
    </joint>

  </xacro:macro>

</robot>

执行器有左右两个轮子和前后的万向轮

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="caster_xacro" params="caster_name xyz rpy radius caster_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${caster_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <sphere radius="${radius}"/>
        </geometry>
        <material name="${caster_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
    </link>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>
</robot>

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="wheel_xacro" params="wheel_name xyz rpy radius length wheel_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${wheel_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${wheel_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
    </link>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>
</robot>

集中部署到机器人上

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

  <!-- ========== Include 部分 ========== -->
  <xacro:include filename="$(find robot_description)/urdf/base.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/camera.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/imu.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/laser.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/actuator/wheel.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/actuator/caster.urdf.xacro" />

  <!-- ========== Base ========== -->
  <xacro:base_xacro
      base_name="base_link"
      xyz="0 0 0"
      rpy="0 0 0"
      radius="0.1"
      length="0.12"
      base_color="white"
      rgba="1 1 1 0.5" />

  <!-- ========== IMU ========== -->
  <xacro:imu_xacro
      imu_name="imu_link"
      xyz="0 0 0"
      rpy="0 0 0"
      size="0.02 0.02 0.02"
      imu_color="black"
      rgba="0 0 0 0.5"
      joint_name="imu_joint"
      joint_type="fixed"
      parent_name="base_link"
      child_name="imu_link"
      joint_xyz="0 0 0.02"
      joint_rpy="0 0 0" />

  <!-- ========== Camera ========== -->
  <xacro:camera_xacro
      camera_name="camera_link"
      xyz="0 0 0"
      rpy="0 0 0"
      size="0.02 0.02 0.02"
      camera_color="black"
      rgba="0 0 0 0.5"
      joint_name="camera_joint"
      joint_type="fixed"
      parent_name="base_link"
      child_name="camera_link"
      joint_xyz="0.1 0 0.075"
      joint_rpy="0 0 0" />

  <!-- ========== Laser  ========== -->
  <xacro:laser_xacro
      laser_name="laser"
      parent_name="base_link"

      joint1_name="laser_stand_joint"
      joint1_type="fixed"
      joint1_xyz="0 0 0.1"
      joint1_rpy="0 0 0"

      joint2_name="laser_mount_joint"
      joint2_type="fixed"
      joint2_xyz="0 0 0.05"
      joint2_rpy="0 0 0"

      cylinder_xyz="0 0 0"
      cylinder_rpy="0 0 0"
      cylinder_radius="0.01"
      cylinder_length="0.1"
      cylinder_color="black"
      cylinder_rgba="0 0 0 1.0"

      sensor_xyz="0 0 0"
      sensor_rpy="0 0 0"
      sensor_radius="0.02"
      sensor_length="0.02"
      sensor_color="black"
      sensor_rgba="0 0 0 1.0" />

  <!-- ========== Wheel ========== -->
  <xacro:wheel_xacro
      wheel_name="wheel_left"
      xyz="0 0 0"
      rpy="1.57079 0 0"
      radius="0.032"
      length="0.04"
      wheel_color="yellow"
      rgba="1 1 0 0.8"
      joint_name="wheel_left_joint"
      joint_type="continuous"
      parent_name="base_link"
      child_name="wheel_left"
      joint_xyz="0 0.1 -0.06"
      joint_rpy="0 0 0" />
    <xacro:wheel_xacro
      wheel_name="wheel_right"
      xyz="0 0 0"
      rpy="1.57079 0 0"
      radius="0.032"
      length="0.04"
      wheel_color="yellow"
      rgba="1 1 0 0.8"
      joint_name="wheel_right_joint"
      joint_type="continuous"
      parent_name="base_link"
      child_name="wheel_right"
      joint_xyz="0 -0.1 -0.06"
      joint_rpy="0 0 0" />

    <!-- ========== Caster ========== -->
    <xacro:caster_xacro
    caster_name="caster_front"
    xyz="0 0 0"
    rpy="0 0 0"
    radius="0.016"
    caster_color="yellow"
    rgba="1 1 0 0.8"
    joint_name="caster_front_joint"
    joint_type="fixed"
    parent_name="base_link"
    child_name="caster_front"
    joint_xyz="0.08 0 -0.076"
    joint_rpy="0 0 0" />
    <xacro:caster_xacro
    caster_name="caster_back"
    xyz="0 0 0"
    rpy="0 0 0"
    radius="0.016"
    caster_color="yellow"
    rgba="1 1 0 0.8"
    joint_name="caster_back_joint"
    joint_type="fixed"
    parent_name="base_link"
    child_name="caster_back"
    joint_xyz="-0.08 0 -0.076"
    joint_rpy="0 0 0" />

</robot>

简单回顾一下之前的TF，一般由机器人底盘作为整体的父坐标系，而其底盘的原点坐标为下为相对世界坐标系下的坐标，坐标系的关系通过joint进行转换，这样才能将所有不同坐标系下的部件整合到一个大坐标系下统一控制。

所在位置	含义	坐标关系	影响对象
link/visual/origin	模型几何中心（例如立方体或圆柱）在 link 坐标系下的位置与姿态	几何体 ← 相对 ← link	决定 link 的外观模型
link/collision/origin	碰撞体在 link 坐标系下的位置与姿态	碰撞体 ← 相对 ← link	用于仿真碰撞检测
joint/origin	子 link 坐标系在父 link 坐标系下的位置与姿态	child_link ← 相对 ← parent_link	决定 link 之间的装配关系

刚体

刚体是一种理想化的物理模型，其特征是在任何情况下，物体内部任意两点之间的距离始终保持不变，也就是说，无论受到多大的外力或产生何种运动，刚体都不会发生形变、压缩或拉伸。在机器人仿真过程中，为了简化计算与提高仿真效率，我们通常将各个部件理想化为刚体进行建模和分析。

属性	说明
name	刚体的名称（link 名称需唯一）
visual	用于显示的几何外观（仅视觉效果，不参与物理仿真）
collision	用于物理碰撞检测的几何体（通常比 visual 更简单）
inertial	表示刚体的质量和惯性张量（影响物体运动特性）

碰撞属性

<collision> 元素用于定义刚体在物理仿真中的真实形状，不影响 RViz 可视化，决定“仿真怎么动、怎么撞”和<visual>分工不同

如以下代码所示给部件加入碰撞属性上面为可视化部件，下面为添加碰撞属性，部件位于link坐标系下，其余部件添加碰撞属性也是如此

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
      <xacro:macro name="base_xacro" params="base_name xyz rpy radius length base_color rgba">
    <link name="${base_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${base_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${base_color}">
          <color rgba="${rgba}"/>
        </material>
      </collision>
    </link>
    <link name="base_footprint"> </link>
    <joint name="joint_name" type="fixed">
      <parent link="base_footprint"/>
      <child link="${base_name}"/>
      <origin xyz="0.0 0.0 ${length/2.0+0.032-0.01}" rpy="0.0 0.0 0.0"/>
    </joint>
  </xacro:macro>
</robot>

关闭可视化，打开碰撞属性可视化

质量和惯性

真实环境下的机器人的各个部件肯定是有质量和惯性，由于上面我们已经加入的碰撞属性，因此对于碰撞的之后物体会发生哪些变化还需借助质量和关系进行一个判断。

惯性矩阵：描述刚体对不同旋转轴的惯性阻力的一个对称矩阵，它表示物体的质量分布对旋转运动的影响，相当于“质量在旋转意义上的分布。

添加系新的xacro文件储存物体们的惯性矩阵

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

  <!-- ========== 长方体惯性矩阵 ========== -->
  <!-- 参数: m=质量, w=宽, h=高, d=深 -->
  <xacro:macro name="box_inertia" params="m w h d">
    <inertial>
      <!-- 惯性中心位姿 -->
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${m}"/>
      <inertia
        ixx="${(m/12.0)*(h*h + d*d)}"
        ixy="0.0"
        ixz="0.0"
        iyy="${(m/12.0)*(w*w + d*d)}"
        iyz="0.0"
        izz="${(m/12.0)*(w*w + h*h)}"/>
    </inertial>
  </xacro:macro>


  <!-- ========== 圆柱体惯性矩阵 ========== -->
  <!-- 参数: m=质量, r=半径, h=高度 -->
  <xacro:macro name="cylinder_inertia" params="m r h">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${m}"/>
      <inertia
        ixx="${(m/12.0)*(3*r*r + h*h)}"
        ixy="0.0"
        ixz="0.0"
        iyy="${(m/12.0)*(3*r*r + h*h)}"
        iyz="0.0"
        izz="${(m/2.0)*(r*r)}"/>
    </inertial>
  </xacro:macro>


  <!-- ========== 球体惯性矩阵 ========== -->
  <!-- 参数: m=质量, r=半径 -->
  <xacro:macro name="sphere_inertia" params="m r">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${m}"/>
      <inertia
        ixx="${(2.0/5.0)*m*(r*r)}"
        ixy="0.0"
        ixz="0.0"
        iyy="${(2.0/5.0)*m*(r*r)}"
        iyz="0.0"
        izz="${(2.0/5.0)*m*(r*r)}"/>
    </inertial>
  </xacro:macro>

</robot>

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:include filename="$(find robot_description)/urdf/common_inert.urdf.xacro" />
     <xacro:macro name="caster_xacro" params="caster_name xyz rpy radius caster_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${caster_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <sphere radius="${radius}"/>
        </geometry>
        <material name="${caster_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <sphere radius="${radius}"/>
        </geometry>
        <material name="${caster_color}">
          <color rgba="${rgba}"/>
        </material>
      </collision>
      <!--- 添加对应的惯性矩阵 -->
      <xacro:sphere_inertia m="0.05" r="${radius}"/>
    </link>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>
</robot>

其余的部件都是通过这种方式进行添加的

Gazebo

Gazebo 是一个功能强大的 三维机器人仿真平台，可以在虚拟环境中模拟 物理特性 和 传感器数据，它常与 ROS 配合使用，用于在电脑上测试和验证机器人控制、导航与感知算法，而无需真实硬件。

sudo apt install gazebo  # 下载功能模块和一些基础模型

cd ~/.gazebo/

git clone https://gitee.com/ohhuo/gazebo_models.git
rm -rf gazebo_models/.git/  # 删除版本控制信息，变成本地独立文件夹

使用gazebo创建一个房间

退出构建模式并保存模型

保存世界文件，前面我们只是保存了模型文件，并不包含家具等物品，只是单纯的墙面，后面我还在该房屋内添加了一个正方体障碍物

下载工具包，机器人模型文件为urdf而仿真模型文件为sdf，二者如果要集成的话需要进行文件类型的转换，ros2自带工具进行二者的文件转换，和xacro将xacro文件转换为urdf文件是同一个道理=

sudo apt install ros-$ROS_DISTRO-gazebo-ros-pkgs

创建launch在 gazebo加载机器人模型

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import Command, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterValue
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    # ==============================
    # 1️⃣ 获取必要的路径
    # ==============================
    # 获取 robot_description 包的共享路径（即 install/robot_description/share/robot_description）
    urdf_package_path = get_package_share_directory('robot_description')

    # 设定默认的 URDF(Xacro) 文件路径
    default_xacro_path = os.path.join(urdf_package_path, 'urdf', 'mybot.urdf.xacro')

    # 设定默认的 Gazebo 仿真世界文件路径
    default_gazebo_path = os.path.join(urdf_package_path, 'world', 'custom_room.world')


    # ==============================
    # 2️⃣ 声明 Launch 参数
    # ==============================
    # 声明一个参数 `model`，默认值为我们定义的 Xacro 文件路径
    # 这样可以在命令行中通过 model:=xxx.xacro 指定其他模型
    action_declare_arg_mode_path = DeclareLaunchArgument(
        'model',
        default_value=str(default_xacro_path)
    )


    # ==============================
    # 3️⃣ 使用 xacro 生成 URDF
    # ==============================
    # 使用 Command 执行 xacro 命令，把 xacro 文件动态转换为 URDF 内容字符串
    command_result = Command(['xacro ', LaunchConfiguration('model')])

    # 将转换后的 URDF 结果传递给 robot_state_publisher
    robot_description_value = ParameterValue(command_result, value_type=str)


    # ==============================
    # 4️⃣ 启动 robot_state_publisher 节点
    # ==============================
    # 该节点负责发布机器人模型（TF 树）到 /tf 和 /robot_description
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description_value}],
    )


    # ==============================
    # 5️⃣ 启动 Gazebo 仿真环境
    # ==============================
    # IncludeLaunchDescription 表示嵌套启动另一个 launch 文件
    # 这里启动 gazebo_ros 包内置的 gazebo.launch.py 文件
    action_launch_gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            get_package_share_directory('gazebo_ros'),
            '/launch', '/gazebo.launch.py'
        ]),
        # 向 Gazebo 传入 world 文件路径和 verbose 模式参数
        launch_arguments=[
            ('world', default_gazebo_path),
            ('verbose', 'true')
        ],
    )


    # ==============================
    # 6️⃣ 向 Gazebo 中生成机器人实体
    # ==============================
    # spawn_entity.py 脚本会读取 /robot_description 主题中的 URDF 数据，
    # 并在 Gazebo 世界中生成对应的机器人模型（即“实体”）
    action_spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic', '/robot_description', '-entity', 'mybot'],
    )


    # ==============================
    # 7️⃣ 返回 LaunchDescription（启动描述）
    # ==============================
    # LaunchDescription 是 ROS2 Launch 系统的核心，它定义了要启动的所有组件。
    return LaunchDescription([
        action_declare_arg_mode_path,   # 声明 model 参数
        robot_state_publisher,          # 启动 robot_state_publisher
        action_launch_gazebo,           # 启动 Gazebo 仿真环境
        action_spawn_entity,            # 向 Gazebo 中生成机器人模型
    ])

Cmakelists.txtx不要忘了修改

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

 <gazebo> 标签扩展原urdf文件

标签名	作用
<material>	指定 Gazebo 中显示的材质（颜色/纹理）
<mu1>, <mu2>	摩擦系数
<plugin>	加载插件（控制器、传感器、ROS接口等）
<sensor>	定义传感器（摄像头、雷达、IMU 等）
<kp>, <kd>	物理弹性、阻尼参数
<self_collide>	是否允许自碰撞
<max_contacts>	最大接触点数量

 一般使用格式

<gazebo reference="link_name">
  <!-- Gazebo 相关配置 -->
</gazebo>
或不指定 reference（应用到整个模型）：

<gazebo>
  <!-- 全局 Gazebo 配置 -->
</gazebo>

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:include filename="$(find robot_description)/urdf/common_inert.urdf.xacro" />
     <xacro:macro name="caster_xacro" params="caster_name xyz rpy radius caster_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${caster_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <sphere radius="${radius}"/>
        </geometry>
        <material name="${caster_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <sphere radius="${radius}"/>
        </geometry>
        <material name="${caster_color}">
          <color rgba="${rgba}"/>
        </material>
      </collision>
      <xacro:sphere_inertia m="0.05" r="${radius}"/>
    </link>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
      <axis xyz="0 1 0"/>
    </joint>
      <gazebo reference="${caster_name}">
      <!-- Gazebo 材质（对应 Gazebo 材质库） -->
      <material>Gazebo/${caster_color}</material>

      <!-- 物理特性：摩擦、恢复系数等 -->
      <mu1>0.2</mu1>           <!-- 摩擦系数1 -->
      <mu2>0.2</mu2>           <!-- 摩擦系数2 -->
      <kp>100000.0</kp>        <!-- 弹性系数 -->
      <kd>10.0</kd>            <!-- 阻尼系数 -->
      <maxVel>0.1</maxVel>     <!-- 最大接触速度 -->
      <minDepth>0.001</minDepth> <!-- 最小接触深度 -->
    </gazebo>

  </xacro:macro>
</robot>

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:include filename="$(find robot_description)/urdf/common_inert.urdf.xacro" />
     <xacro:macro name="wheel_xacro" params="wheel_name xyz rpy radius length wheel_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${wheel_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${wheel_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${wheel_color}">
          <color rgba="${rgba}"/>
        </material>
      </collision>
      <xacro:cylinder_inertia m="1.0" r="${radius}" h="${length}"/>
    </link>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
      <axis xyz="0 1 0"/>
    </joint>
        <!-- 为 Gazebo 指定 link 的物理、视觉、摩擦等属性 -->
    <gazebo reference="${wheel_name}">
      <!-- Gazebo 中的视觉材质 -->
      <material>Gazebo/${wheel_color}</material>

      <!-- 摩擦参数，决定轮胎打滑程度 -->
      <mu1>20.0</mu1>
      <mu2>20.0</mu2>

      <!-- 弹性与阻尼系数 -->
      <kp>100000.0</kp>
      <kd>10.0</kd>

      <!-- 避免轮子“穿透”地面 -->
      <maxVel>0.1</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>
    <gazebo>
    
    </gazebo>
  </xacro:macro>
</robot>

注意mybot传入的颜色名称首字母要大写才会生效

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

  <!-- ========== Include 部分 ========== -->
  <xacro:include filename="$(find robot_description)/urdf/base.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/camera.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/imu.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/laser.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/actuator/wheel.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/actuator/caster.urdf.xacro" />

  <!-- ========== Base ========== -->
  <xacro:base_xacro
      base_name="base_link"
      xyz="0 0 0"
      rpy="0 0 0"
      radius="0.1"
      length="0.12"
      base_color="White"
      rgba="1 1 1 0.5" />

  <!-- ========== IMU ========== -->
  <xacro:imu_xacro
      imu_name="imu_link"
      xyz="0 0 0"
      rpy="0 0 0"
      size="0.02 0.02 0.02"
      imu_color="Black"
      rgba="0 0 0 0.5"
      joint_name="imu_joint"
      joint_type="fixed"
      parent_name="base_link"
      child_name="imu_link"
      joint_xyz="0 0 0.02"
      joint_rpy="0 0 0" />

  <!-- ========== Camera ========== -->
  <xacro:camera_xacro
      camera_name="camera_link"
      xyz="0 0 0"
      rpy="0 0 0"
      size="0.02 0.1 0.02"
      camera_color="Black"
      rgba="0 0 0 0.5"
      joint_name="camera_joint"
      joint_type="fixed"
      parent_name="base_link"
      child_name="camera_link"
      joint_xyz="0.1 0 0.075"
      joint_rpy="0 0 0" />

  <!-- ========== Laser  ========== -->
  <xacro:laser_xacro
      laser_name="laser"
      parent_name="base_link"

      joint1_name="laser_stand_joint"
      joint1_type="fixed"
      joint1_xyz="0 0 0.1"
      joint1_rpy="0 0 0"

      joint2_name="laser_mount_joint"
      joint2_type="fixed"
      joint2_xyz="0 0 0.05"
      joint2_rpy="0 0 0"

      cylinder_xyz="0 0 0"
      cylinder_rpy="0 0 0"
      cylinder_radius="0.01"
      cylinder_length="0.1"
      cylinder_color="Black"
      cylinder_rgba="0 0 0 1.0"

      sensor_xyz="0 0 0"
      sensor_rpy="0 0 0"
      sensor_radius="0.02"
      sensor_length="0.02"
      sensor_color="Black"
      sensor_rgba="0 0 0 1.0" />

  <!-- ========== Wheel ========== -->
  <xacro:wheel_xacro
      wheel_name="wheel_left"
      xyz="0 0 0"
      rpy="1.57079 0 0"
      radius="0.032"
      length="0.04"
      wheel_color="Yellow"
      rgba="1 1 0 0.8"
      joint_name="wheel_left_joint"
      joint_type="continuous"
      parent_name="base_link"
      child_name="wheel_left"
      joint_xyz="0 0.1 -0.06"
      joint_rpy="0 0 0" />
    <xacro:wheel_xacro
      wheel_name="wheel_right"
      xyz="0 0 0"
      rpy="1.57079 0 0"
      radius="0.032"
      length="0.04"
      wheel_color="Yellow"
      rgba="1 1 0 0.8"
      joint_name="wheel_right_joint"
      joint_type="continuous"
      parent_name="base_link"
      child_name="wheel_right"
      joint_xyz="0 -0.1 -0.06"
      joint_rpy="0 0 0" />

    <!-- ========== Caster ========== -->
    <xacro:caster_xacro
    caster_name="caster_front"
    xyz="0 0 0"
    rpy="0 0 0"
    radius="0.016"
    caster_color="Yellow"
    rgba="1 1 0 0.8"
    joint_name="caster_front_joint"
    joint_type="fixed"
    parent_name="base_link"
    child_name="caster_front"
    joint_xyz="0.08 0 -0.076"
    joint_rpy="0 0 0" />
    <xacro:caster_xacro
    caster_name="caster_back"
    xyz="0 0 0"
    rpy="0 0 0"
    radius="0.016"
    caster_color="Yellow"
    rgba="1 1 0 0.8"
    joint_name="caster_back_joint"
    joint_type="fixed"
    parent_name="base_link"
    child_name="caster_back"
    joint_xyz="-0.08 0 -0.076"
    joint_rpy="0 0 0" />

</robot>

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:include filename="$(find robot_description)/urdf/common_inert.urdf.xacro" />
     <xacro:macro name="imu_xacro" params="imu_name xyz rpy size imu_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${imu_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <box size="${size}"/>
        </geometry>
        <material name="${imu_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
      <collision>
      <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <box size="${size}"/>
        </geometry>
        <material name="${imu_color}">
          <color rgba="${rgba}"/>
        </material>
      </collision>
      <xacro:box_inertia m="0.05" w="0.02" h="0.02" d="0.02"/>
    </link>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
    </joint>
    <gazebo reference="${imu_name}">
      <material>Gazebo/${imu_color}</material>
    </gazebo>
  </xacro:macro>
</robot>

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:include filename="$(find robot_description)/urdf/common_inert.urdf.xacro" />
     <xacro:macro name="imu_xacro" params="imu_name xyz rpy size imu_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${imu_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <box size="${size}"/>
        </geometry>
        <material name="${imu_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
      <collision>
      <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <box size="${size}"/>
        </geometry>
        <material name="${imu_color}">
          <color rgba="${rgba}"/>
        </material>
      </collision>
      <xacro:box_inertia m="0.05" w="0.02" h="0.02" d="0.02"/>
    </link>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
    </joint>
    <gazebo reference="${imu_name}">
      <material>Gazebo/${imu_color}</material>
    </gazebo>
  </xacro:macro>
</robot>

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find robot_description)/urdf/common_inert.urdf.xacro" />

  <!-- 激光雷达组件宏 -->
  <xacro:macro name="laser_xacro" params="
      laser_name
      parent_name
      joint1_name joint1_type joint1_xyz joint1_rpy
      joint2_name joint2_type joint2_xyz joint2_rpy
      cylinder_xyz cylinder_rpy cylinder_radius cylinder_length cylinder_color cylinder_rgba
      sensor_xyz sensor_rpy sensor_radius sensor_length sensor_color sensor_rgba">

    <!-- 支撑杆 -->
    <link name="${laser_name}_cylinder_link">
      <visual>
        <origin xyz="${cylinder_xyz}" rpy="${cylinder_rpy}"/>
        <geometry>
          <cylinder radius="${cylinder_radius}" length="${cylinder_length}"/>
        </geometry>
        <material name="${cylinder_color}">
          <color rgba="${cylinder_rgba}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="${cylinder_xyz}" rpy="${cylinder_rpy}"/>
        <geometry>
          <cylinder radius="${cylinder_radius}" length="${cylinder_length}"/>
        </geometry>
        <material name="${cylinder_color}">
          <color rgba="${cylinder_rgba}"/>
        </material>
      </collision>
    </link>

    <!-- 激光雷达主体 -->
    <link name="${laser_name}_sensor_link">
      <visual>
        <origin xyz="${sensor_xyz}" rpy="${sensor_rpy}"/>
        <geometry>
          <cylinder radius="${sensor_radius}" length="${sensor_length}"/>
        </geometry>
        <material name="${sensor_color}">
          <color rgba="${sensor_rgba}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="${sensor_xyz}" rpy="${sensor_rpy}"/>
        <geometry>
          <cylinder radius="${sensor_radius}" length="${sensor_length}"/>
        </geometry>
        <material name="${sensor_color}">
          <color rgba="${sensor_rgba}"/>
        </material>
      </collision>
      <xacro:cylinder_inertia m="0.1" r="${cylinder_radius}" h="${cylinder_length}"/>
      <xacro:cylinder_inertia m="0.1" r="${sensor_radius}" h="${sensor_length}"/>
    </link>

    <!-- 关节1：底座连接支撑杆 -->
    <joint name="${joint1_name}" type="${joint1_type}">
      <parent link="${parent_name}"/>
      <child link="${laser_name}_cylinder_link"/>
      <origin xyz="${joint1_xyz}" rpy="${joint1_rpy}"/>
    </joint>

    <!-- 关节2：支撑杆连接激光雷达 -->
    <joint name="${joint2_name}" type="${joint2_type}">
      <parent link="${laser_name}_cylinder_link"/>
      <child link="${laser_name}_sensor_link"/>
      <origin xyz="${joint2_xyz}" rpy="${joint2_rpy}"/>
    </joint>

    <gazebo reference="${laser_name}">
      <material>Gazebo/${laser_color}</material>
    </gazebo>

  </xacro:macro>

</robot>

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
      <xacro:include filename="$(find robot_description)/urdf/common_inert.urdf.xacro" />
      <xacro:macro name="base_xacro" params="base_name xyz rpy radius length base_color rgba">
    <link name="${base_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${base_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${base_color}">
          <color rgba="${rgba}"/>
        </material>
      </collision>
      <xacro:cylinder_inertia m="1.0" r="${radius}" h="${length}"/>
    </link>
    <link name="base_footprint"> </link>
    <joint name="joint_name" type="fixed">
      <parent link="base_footprint"/>
      <child link="${base_name}"/>
      <origin xyz="0.0 0.0 ${length/2.0+0.032-0.01}" rpy="0.0 0.0 0.0"/>
    </joint>
    <gazebo reference="${base_name}">
      <material>Gazebo/${base_color}</material>
    </gazebo>
  </xacro:macro>
</robot>

差速驱动插件

该插件在Gazebo 的物理模型 和 ROS 的控制话题 之间建立了桥梁，让两轮差速小车在 Gazebo 里能被 ROS 控制移动

新建文件夹plugins，然后在下面新建插件xacro文件

<?xml version="1.0"?>
<!-- 定义一个 ROS xacro 文件，用于在 Gazebo 中加载差速驱动插件 -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- 定义一个宏：gazebo_control_plugin -->
    <!-- 这个宏可以在主 URDF 文件中通过 <xacro:gazebo_control_plugin/> 调用 -->
    <xacro:macro name="gazebo_control_plugin">

        <!-- Gazebo 仿真相关的标签 -->
        <gazebo>
            <!-- 插件定义 -->
            <!-- libgazebo_ros_diff_drive.so 是 Gazebo 官方的 ROS 2 插件，用于实现差速驱动（左右轮控制） -->
            <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>

                <!-- ROS 2 相关配置 -->
                <ros>
                    <!-- 命名空间（namespace）：决定节点、话题等在 ROS 图中的层级 -->
                    <!-- 默认为根命名空间“/” -->
                    <namespace>/</namespace>

                    <!-- 话题重映射 -->
                    <!-- 将 Gazebo 插件内部使用的 cmd_vel、odom 话题映射为 ROS 系统中对应的名称 -->
                    <remapping>cmd_vel:=cmd_vel</remapping>
                    <remapping>odom:=odom</remapping>
                </ros>

                <!-- 更新频率（Hz） -->
                <!-- 插件内部更新控制与里程计发布的速率 -->
                <update_rate>30</update_rate>

                <!-- ========== 轮子设置 ========== -->
                <!-- 左右轮的关节名（必须与 URDF 中 joint 名称完全一致） -->
                <left_joint>wheel_left_joint</left_joint>
                <right_joint>wheel_right_joint</right_joint>

                <!-- ========== 运动学参数 ========== -->
                <!-- 轮间距（两轮中心之间的距离，单位：米） -->
                <wheel_separation>0.2</wheel_separation>

                <!-- 轮子直径（单位：米） -->
                <wheel_diameter>0.064</wheel_diameter>

                <!-- ========== 动力学限制 ========== -->
                <!-- 最大扭矩（影响机器人加速能力，单位：N·m） -->
                <max_wheel_torque>20</max_wheel_torque>

                <!-- 最大加速度（单位：m/s²） -->
                <max_wheel_acceleration>1.0</max_wheel_acceleration>

                <!-- ========== 输出与发布设置 ========== -->
                <!-- 是否发布里程计数据 -->
                <publish_odom>true</publish_odom>

                <!-- 是否发布 TF（odom -> base_link 或 base_footprint） -->
                <publish_odom_tf>true</publish_odom_tf>

                <!-- 是否发布轮子 TF（wheel_link） -->
                <publish_wheel_tf>true</publish_wheel_tf>

                <!-- ========== 坐标系定义 ========== -->
                <!-- 里程计参考坐标系 -->
                <odometry_frame>odom</odometry_frame>

                <!-- 机器人底盘坐标系（与 URDF 中定义一致） -->
                <robot_base_frame>base_footprint</robot_base_frame>

            </plugin>
        </gazebo>
   </xacro:macro>
</robot>

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

  <!-- ========== Include 部分 ========== -->
  <xacro:include filename="$(find robot_description)/urdf/base.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/camera.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/imu.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/laser.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/actuator/wheel.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/actuator/caster.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/plugins/gazebo_control_plugin.xacro" />

  <!-- ========== Base ========== -->
  <xacro:base_xacro
      base_name="base_link"
      xyz="0 0 0"
      rpy="0 0 0"
      radius="0.1"
      length="0.12"
      base_color="White"
      rgba="1 1 1 0.5" />

  <!-- ========== IMU ========== -->
  <xacro:imu_xacro
      imu_name="imu_link"
      xyz="0 0 0"
      rpy="0 0 0"
      size="0.02 0.02 0.02"
      imu_color="Black"
      rgba="0 0 0 0.5"
      joint_name="imu_joint"
      joint_type="fixed"
      parent_name="base_link"
      child_name="imu_link"
      joint_xyz="0 0 0.02"
      joint_rpy="0 0 0" />

  <!-- ========== Camera ========== -->
  <xacro:camera_xacro
      camera_name="camera_link"
      xyz="0 0 0"
      rpy="0 0 0"
      size="0.02 0.1 0.02"
      camera_color="Black"
      rgba="0 0 0 0.5"
      joint_name="camera_joint"
      joint_type="fixed"
      parent_name="base_link"
      child_name="camera_link"
      joint_xyz="0.1 0 0.075"
      joint_rpy="0 0 0" />

  <!-- ========== Laser  ========== -->
  <xacro:laser_xacro
      laser_name="laser"
      parent_name="base_link"

      joint1_name="laser_stand_joint"
      joint1_type="fixed"
      joint1_xyz="0 0 0.1"
      joint1_rpy="0 0 0"

      joint2_name="laser_mount_joint"
      joint2_type="fixed"
      joint2_xyz="0 0 0.05"
      joint2_rpy="0 0 0"

      cylinder_xyz="0 0 0"
      cylinder_rpy="0 0 0"
      cylinder_radius="0.01"
      cylinder_length="0.1"
      cylinder_color="Black"
      cylinder_rgba="0 0 0 1.0"

      sensor_xyz="0 0 0"
      sensor_rpy="0 0 0"
      sensor_radius="0.02"
      sensor_length="0.02"
      sensor_color="Black"
      sensor_rgba="0 0 0 1.0" />

  <!-- ========== Wheel ========== -->
  <xacro:wheel_xacro
      wheel_name="wheel_left"
      xyz="0 0 0"
      rpy="1.57079 0 0"
      radius="0.032"
      length="0.04"
      wheel_color="Yellow"
      rgba="1 1 0 0.8"
      joint_name="wheel_left_joint"
      joint_type="continuous"
      parent_name="base_link"
      child_name="wheel_left"
      joint_xyz="0 0.1 -0.06"
      joint_rpy="0 0 0" />
    <xacro:wheel_xacro
      wheel_name="wheel_right"
      xyz="0 0 0"
      rpy="1.57079 0 0"
      radius="0.032"
      length="0.04"
      wheel_color="Yellow"
      rgba="1 1 0 0.8"
      joint_name="wheel_right_joint"
      joint_type="continuous"
      parent_name="base_link"
      child_name="wheel_right"
      joint_xyz="0 -0.1 -0.06"
      joint_rpy="0 0 0" />

    <!-- ========== Caster ========== -->
    <xacro:caster_xacro
    caster_name="caster_front"
    xyz="0 0 0"
    rpy="0 0 0"
    radius="0.016"
    caster_color="Yellow"
    rgba="1 1 0 0.8"
    joint_name="caster_front_joint"
    joint_type="fixed"
    parent_name="base_link"
    child_name="caster_front"
    joint_xyz="0.08 0 -0.076"
    joint_rpy="0 0 0" />
    <xacro:caster_xacro
    caster_name="caster_back"
    xyz="0 0 0"
    rpy="0 0 0"
    radius="0.016"
    caster_color="Yellow"
    rgba="1 1 0 0.8"
    joint_name="caster_back_joint"
    joint_type="fixed"
    parent_name="base_link"
    child_name="caster_back"
    joint_xyz="-0.08 0 -0.076"
    joint_rpy="0 0 0" />
    <xacro:gazebo_control_plugin />
</robot>

控制小车进行一个移动

注意先通过launch启动gazebo然后再使用命令启动rviz2而不是通过之前编写的launch去启动，因为二者启动得到的TF是各自独立的，博主一开始没有注意这个问题搞得得到的rviz2的TF中左右两轮和虚拟地面之间的TF一直没找到，折腾了好久才发现这个问题

添加三个传感器的仿真插件

IMU是移动机器人、无人车、无人机中非常关键的一个传感器，它的主要功能是：测量和估计机器人在三维空间中的姿态、角速度和线加速度。

需要修改一下相机的TF，传统环境下的相机是Z轴向前的，即Z轴垂直于相机坐标系

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:include filename="$(find robot_description)/urdf/common_inert.urdf.xacro" />
     <xacro:macro name="camera_xacro" params="camera_name xyz rpy size camera_color rgba joint_name joint_type parent_name child_name joint_xyz joint_rpy">
    <link name="${camera_name}">
      <visual>
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <box size="${size}"/>
        </geometry>
        <material name="${camera_color}">
          <color rgba="${rgba}"/>
        </material>
      </visual>
       <collision>
       <origin xyz="${xyz}" rpy="${rpy}"/>
        <geometry>
          <box size="${size}"/>
        </geometry>
        <material name="${camera_color}">
          <color rgba="${rgba}"/>
        </material>
    </collision>
    </link>
     <link name="camera_optical_link"></link>
    <xacro:box_inertia m="0.1" w="0.02" h="0.1" d="0.02"/>
    <joint name="${joint_name}" type="${joint_type}">
      <parent link="${parent_name}"/>
      <child link="${child_name}"/>
      <origin xyz="${joint_xyz}" rpy="${joint_rpy}"/>
    </joint>
     <joint name="camera_optical_joint" type="fixed">
        <parent link="${camera_name}" />
        <child link="camera_optical_link" />
        <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
      </joint>
    <gazebo reference="${camera_name}">
      <material>Gazebo/${camera_color}</material>
    </gazebo>
  </xacro:macro>
</robot>

模拟真实环境下的相机坐标系

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

  <!-- 定义一个宏：gazebo_sensor_plugin
       用于在机器人URDF中插入激光雷达、IMU、深度相机的Gazebo仿真插件 -->
  <xacro:macro name="gazebo_sensor_plugin">

    <!-- ====================== 激光雷达（LaserScan）配置 ====================== -->
    <gazebo reference="laser_sensor_link"> <!-- 将以下传感器挂载到 laser_link 链接上 -->
      <sensor name="laserscan" type="ray"> <!-- 定义一个射线型雷达传感器 -->
        
        <!-- 使用 Gazebo-ROS 插件，将雷达数据发布到 ROS 中 -->
        <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
          <ros>
            <namespace>/</namespace> <!-- ROS命名空间设置为根目录 -->
            <remapping>~/out:=scan</remapping> <!-- 输出话题重映射为 /scan -->
          </ros>
          <output_type>sensor_msgs/LaserScan</output_type> <!-- 输出消息类型 -->
          <frame_name>laser_sensor_link</frame_name> <!-- TF坐标系名称 -->
        </plugin>

        <!-- 传感器运行参数 -->
        <always_on>true</always_on> <!-- 始终开启 -->
        <visualize>true</visualize> <!-- 在Gazebo界面中显示扫描 -->
        <update_rate>5</update_rate> <!-- 更新频率 5Hz -->
        <pose>0 0 0 0 0 0</pose> <!-- 相对于laser_link的位姿 -->

        <!-- 激光扫描参数设置 -->
        <ray>
          <scan>
            <horizontal>
              <samples>360</samples> <!-- 水平扫描样本数 -->
              <resolution>1.000000</resolution> <!-- 角分辨率 -->
              <min_angle>0.000000</min_angle> <!-- 最小角度（弧度） -->
              <max_angle>6.280000</max_angle> <!-- 最大角度（2π，360°） -->
            </horizontal>
          </scan>

          <!-- 测距范围配置 -->
          <range>
            <min>0.120000</min> <!-- 最小测距0.12m -->
            <max>8.0</max> <!-- 最大测距8m -->
            <resolution>0.015000</resolution> <!-- 距离分辨率0.015m -->
          </range>

          <!-- 添加高斯噪声以模拟真实环境测量误差 -->
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
        </ray>
      </sensor>
    </gazebo>


    <!-- ====================== IMU惯性测量单元配置 ====================== -->
    <gazebo reference="imu_link"> <!-- 将IMU挂载到imu_link链接上 -->
      <sensor name="imu_sensor" type="imu"> <!-- 传感器类型为IMU -->
        
        <!-- 加载Gazebo ROS IMU插件 -->
        <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
          <ros>
            <namespace>/</namespace> <!-- ROS命名空间为根目录 -->
            <remapping>~/out:=imu</remapping> <!-- 输出话题重映射为 /imu -->
          </ros>
          <initial_orientation_as_reference>false</initial_orientation_as_reference> <!-- 不将初始姿态作为基准 -->
        </plugin>

        <update_rate>100</update_rate> <!-- 100Hz高频率输出 -->
        <always_on>true</always_on> <!-- 始终开启 -->

        <!-- IMU噪声模型定义（角速度+线加速度） -->
        <imu>
          <!-- 角速度噪声（x/y/z三轴） -->
          <angular_velocity>
            <x>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </z>
          </angular_velocity>

          <!-- 线加速度噪声（x/y/z三轴） -->
          <linear_acceleration>
            <x>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </z>
          </linear_acceleration>
        </imu>
      </sensor>
    </gazebo>


    <!-- ====================== 深度相机（Depth Camera）配置 ====================== -->
    <gazebo reference="camera_link"> <!-- 挂载在camera_link链接 -->
      <sensor type="depth" name="camera_sensor"> <!-- 定义一个深度相机传感器 -->
        
        <!-- 使用Gazebo-ROS相机插件 -->
        <plugin name="depth_camera" filename="libgazebo_ros_camera.so">
          <frame_name>camera_optical_link</frame_name> <!-- 相机坐标帧名称 -->
        </plugin>

        <!-- 基本参数 -->
        <always_on>true</always_on> <!-- 始终开启 -->
        <update_rate>10</update_rate> <!-- 10Hz帧率 -->

        <!-- 相机内参设置 -->
        <camera name="camera">
          <horizontal_fov>1.5009831567</horizontal_fov> <!-- 水平视场角约85° -->
          <image>
            <width>800</width> <!-- 图像宽度 -->
            <height>600</height> <!-- 图像高度 -->
            <format>R8G8B8</format> <!-- RGB格式 -->
          </image>

          <!-- 畸变参数（全0表示理想无畸变相机） -->
          <distortion>
            <k1>0.0</k1>
            <k2>0.0</k2>
            <k3>0.0</k3>
            <p1>0.0</p1>
            <p2>0.0</p2>
            <center>0.5 0.5</center> <!-- 光心位置（归一化坐标） -->
          </distortion>
        </camera>
      </sensor>
    </gazebo>

  </xacro:macro>
</robot>

不要忘了在mybot.urdf.xacro里面添加引用

ros2_control

ros2_control 是 ROS 2 官方推荐的机器人控制框架，其核心思想是把控制算法（如差速控制器、关节位置控制器）与硬件接口（电机、执行器、Gazebo 模拟等）分离，底层为多个服务共同实现。

ros2_control本身就是真实环境/仿真环境和机器人行为交互之间的一个中间媒介，帮上层决策逻辑（比如根据环境避障、规划路径）和底层硬件（或仿真模型）高效对接，即控制算法-ros2_control-外部环境，这样一个三层关系，将抽象算法转为实际环境中的响应同时将实际的环境转为算法所需的数据。

sudo apt install ros-$ROS_DISTRO-ros2-controllers

ros2_control 的架构可概括为 “1 个中枢 + 3 类核心组件 + 标准化接口”，各部分各司其职、协同工作：

组成部分	核心角色	关键实例
1. 中枢：Controller Manager（控制器管理器）	整个框架的 “调度中心”，负责组件生命周期管理、资源协调、动态控制	提供 ros2 control 命令行工具（加载 / 切换控制器）
2. 控制逻辑：Controllers（控制器）	实现具体控制算法（如轨迹跟踪、PID），接收指令并计算控制信号	官方预实现：JointTrajectoryController（轨迹控制）、PIDController（PID 调节）；支持自定义
3. 硬件适配：Hardware Components（硬件组件）	对接真实硬件 / 仿真环境，将标准化指令转换为硬件信号，同时反馈状态	真实硬件：串口电机驱动、CAN 舵机驱动；仿真：gazebo_ros2_control 提供的虚拟硬件接口
4. 状态发布：Broadcasters（广播器）	读取硬件状态，通过 ROS 2 标准话题发布，供上层可视化 / 算法使用	JointStateBroadcaster（发布 /joint_states 话题）、TFBroadcaster（发布坐标系变换）
5. 标准化接口：Hardware Interfaces	组件间的 “数据协议”，确保交互一致性	StateInterface（状态反馈）、CommandInterface（指令下发）

其中最核心的就是控制器了，控制器是 “执行控制算法的模块”，通过读取硬件状态、计算控制指令，实现对机器人的运动控制（如关节轨迹跟踪、PID 调节），需继承 ros2_control 提供的基类（如 controller_interface::ControllerInterface）开发，或使用官方预实现的控制器。

接下来我们会使用到三个官方的控制器

控制器名称	核心功能	适用场景
joint_trajectory_controller	接收关节轨迹指令（trajectory_msgs/msg/JointTrajectory），控制关节按轨迹运动	机械臂、多关节机器人的轨迹跟踪（如 MoveIt! 规划后执行）
diff_drive_controller	差速轮式机器人运动控制（接收 geometry_msgs/msg/Twist 指令，解算左右轮速度）	轮式移动机器人（如 AGV、小车）
effort_controller	关节力矩 / 力控制（适用于力控场景）	协作机械臂、需要力反馈的场景

控制器需要在配置文件内声明，同时还需要激活才能有效

编写<ros2_control>的xacro文件，通过 <ros2_control> 标签关联 ros2_control 的硬件组件和接口（状态 / 指令接口），即关联硬件和ros2_control 接口

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

  <!-- 定义一个名为 mybot_ros2_control 的 xacro 宏 -->
  <!-- 调用此宏即可为 mybot 加载 ros2_control 与 gazebo_ros2_control 插件 -->
  <xacro:macro name="mybot_ros2_control">

    <!-- ==========================
         ROS2 Control 系统定义部分
         ========================== -->
    <!-- 定义一个 ros2_control 系统接口，名字为 mybotGazeboSystem -->
    <!-- type="system" 表示这是一个系统级控制器，而不是单个传感器或执行器 -->
    <ros2_control name="mybotGazeboSystem" type="system">

      <!-- 指定使用的硬件接口插件 -->
      <!-- gazebo_ros2_control/GazeboSystem 表示这是一个 Gazebo 模拟的硬件系统 -->
      <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      </hardware>

      <!-- ========== 左轮关节控制定义 ========== -->
      <joint name="wheel_left_joint">

        <!-- 定义速度控制接口（常用于差速驱动机器人） -->
        <command_interface name="velocity">
          <param name="min">-1</param>   <!-- 最小速度（单位：rad/s） -->
          <param name="max">1</param>    <!-- 最大速度 -->
        </command_interface>

        <!-- 定义力矩（扭矩）控制接口 -->
        <command_interface name="effort">
          <param name="min">-0.1</param>
          <param name="max">0.1</param>
        </command_interface>

        <!-- 定义状态接口，用于反馈轮子的实际状态 -->
        <state_interface name="position" />  <!-- 轮子位置（角度） -->
        <state_interface name="velocity" />  <!-- 轮子速度 -->
        <state_interface name="effort" />    <!-- 轮子受力情况 -->
      </joint>

      <!-- ========== 右轮关节控制定义 ========== -->
      <joint name="wheel_right_joint">
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>

        <command_interface name="effort">
          <param name="min">-0.1</param>
          <param name="max">0.1</param>
        </command_interface>

        <state_interface name="position" />
        <state_interface name="velocity" />
        <state_interface name="effort" />
      </joint>

    </ros2_control>

    <!-- ==========================
         Gazebo ros2_control 插件定义
         ========================== -->
    <!-- 此插件将 ros2_control 框架与 Gazebo 仿真世界连接起来 -->
    <gazebo>
      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">

        <!-- 指定控制器配置文件（yaml）路径 -->
        <!-- 该文件定义了哪些控制器（如 diff_drive_controller）会被加载 -->
        <parameters>$(find robot_description)/config/mybot_ros2_controller.yaml</parameters>

        <!-- ROS2 remapping，用于话题名映射 -->
        <ros>
          <!-- 将控制器的 /cmd_vel_unstamped 重映射到通用的 /cmd_vel -->
          <remapping>/mybot_diff_drive_controller/cmd_vel_unstamped:=/cmd_vel</remapping>

          <!-- 将控制器发布的 /odom 重映射为全局的 /odom -->
          <remapping>/mybot_diff_drive_controller/odom:=/odom</remapping>
        </ros>

      </plugin>
    </gazebo>

  </xacro:macro>
</robot>

编写配置文件，配置 Controller Manager、硬件组件、控制器、广播器 的参数

# ==============================================================================
# 整体说明：ROS 2 ros2_control 控制器配置文件（适用于 mybot 差速驱动机器人）
# 核心功能：实现 "cmd_vel 速度指令 → 差速解算 → 关节力矩驱动 → odom/关节状态反馈" 全链路
# 依赖组件：需配合 URDF/SDF 模型（关节名需与配置一致）、Gazebo 仿真（use_sim_time=true）
# ==============================================================================

# ------------------------------
# 1. 全局控制器管理器（核心统筹模块）
# 作用：管理所有控制器的加载、启动/停止，配置全局控制参数
# ------------------------------
controller_manager:
  ros__parameters:
    # 控制器更新频率（100Hz）：每 10ms 计算一次控制信号，频率越高控制越平滑（需平衡资源占用）
    update_rate: 100  # Hz
    # 使用仿真时间（true=启用）：与 Gazebo 仿真配合必设，读取 Gazebo 发布的 /clock 话题时间，避免时间同步错误
    use_sim_time: true
    # 关节状态广播器：负责读取关节状态，发布 /joint_states 话题（RViz2 可视化关节运动需依赖）
    mybot_joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster  # 官方状态广播器插件
      use_sim_time: true  # 广播器也使用仿真时间，与全局一致
    # 关节力控控制器：底层执行器，接收差速控制器指令，通过力矩驱动车轮关节
    mybot_effort_controller:
      type: effort_controllers/JointGroupEffortController  # 官方关节组力控插件（控制力矩/力）
    # 差速驱动控制器：核心解算模块，接收 cmd_vel 指令，解算为车轮控制信号，同时发布 odom 里程计
    mybot_diff_drive_controller:
      type: diff_drive_controller/DiffDriveController  # 官方差速驱动插件（适配两轮差速机器人）

# ------------------------------
# 2. 关节力控控制器配置（底层驱动模块）
# 作用：直接与车轮关节交互，接收上层差速控制器的力矩指令，驱动关节转动并反馈状态
# ------------------------------
mybot_effort_controller:
  ros__parameters:
    # 要控制的车轮关节名称列表：必须与 URDF/SDF 模型中车轮关节名完全一致！
    joints:
      - wheel_left_joint   # 左车轮关节名
      - wheel_right_joint # 右车轮关节名（注意：原配置中关节名大小写一致，需与模型匹配）
    # 控制接口类型：effort=力矩/力控制（适合仿真或带力矩传感器的真实机器人）
    command_interfaces:
      - effort
    # 状态反馈接口：需要读取的关节状态（用于闭环控制和状态发布）
    state_interfaces:
      - position  # 关节位置（车轮转动角度，单位：弧度）
      - velocity  # 关节速度（车轮转动角速度，单位：rad/s）
      - effort    # 关节受力/力矩（反馈实际驱动扭矩）

# ------------------------------
# 3. 差速驱动控制器配置（核心逻辑模块）
# 作用：1. 解算 cmd_vel 为车轮速度/力矩；2. 计算机器人里程计（odom）；3. 发布 TF 变换
# ------------------------------
mybot_diff_drive_controller:
  ros__parameters:
    # 左右车轮关节名称映射：指定哪几个关节属于左/右轮（与上面 joints 配置一致）
    left_wheel_names: ["wheel_left_joint"]   # 左车轮关节（单轮差速，列表形式支持多轮）
    right_wheel_names: ["wheel_right_joint"] # 右车轮关节

    # 机器人核心物理参数（必须与实际/仿真模型尺寸一致，否则运动解算会出错！）
    wheel_separation: 0.17  # 左右车轮中心间距（单位：米），差速转向解算的关键参数
    wheel_radius: 0.032     # 车轮半径（单位：米），用于速度→车轮转速、里程计位置计算

    # 物理参数修正系数（默认1.0，用于校准实际与理论的偏差）
    wheel_separation_multiplier: 1.0  # 轮距修正系数
    left_wheel_radius_multiplier: 1.0  # 左轮半径修正系数
    right_wheel_radius_multiplier: 1.0 # 右轮半径修正系数

    # 里程计与状态发布参数
    publish_rate: 50.0  # /odom 里程计和关节状态的发布频率（50Hz，平衡实时性和资源）
    odom_frame_id: odom         # 里程计参考坐标系（与 /odom 话题的 frame_id 一致，固定为 "odom"）
    base_frame_id: base_footprint  # 机器人底座坐标系（与 TF 树中的 base_footprint 对应，RViz2 可视化参考）
    # 里程计位置不确定性协方差（仿真场景用，表征定位误差，实际机器人可根据传感器精度调整）
    pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
    # 里程计速度不确定性协方差（同上，仿真场景用）
    twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]

    # 控制模式配置
    open_loop: true  # 开环控制（true=启用）：无需车轮编码器反馈，适合 Gazebo 仿真（无打滑误差）；真实机器人建议设 false（需编码器闭环校正）
    enable_odom_tf: true  # 自动发布 TF 变换（odom → base_footprint）：RViz2 可视化机器人位置必须启用

    # 安全与指令处理配置
    cmd_vel_timeout: 0.5  # cmd_vel 指令超时时间（单位：秒）：0.5秒内未收到新指令，自动停止车轮运动（避免机器人失控）
    # publish_limited_velocity: true  # （注释禁用）发布限制后的速度（用于速度限流场景，需配合限流参数）
    use_stamped_vel: false  # 是否使用带时间戳的 cmd_vel 指令（false=简化模式，直接订阅 /cmd_vel；true=订阅 /cmd_vel_stamped）
    # velocity_rolling_window_size: 10  # （注释禁用）速度滚动窗口大小（用于计算平均速度，优化反馈平滑度）

 差速控制是 “指挥官”：负责规划机器人整体运动，告诉底层 “该跑多快、往哪转”，力控是 “执行者”：负责落实指挥官的命令，告诉电机 “该用多大劲转”，启动顺序：广播器→力控→差速

对比维度	差速控制（diff_drive_controller）	力控（effort_controllers）
核心目标	解决 “机器人整体怎么运动”（路径、速度、定位）	解决 “执行器（车轮）怎么发力”（力矩、电流、驱动）
控制对象	机器人整体（base_footprint 坐标系）	单个 / 多个执行器（车轮关节）
依赖关系	依赖底层力控控制器（或速度控制器）完成执行	不依赖上层控制器，只需要接收指令即可
在 ros2_control 中的角色	上层 “决策型控制器”（无直接硬件接口，需通过底层控制器交互）	底层 “执行型控制器”（直接绑定硬件接口，与 URDF 关节接口匹配）
关键参数	机器人物理尺寸（轮距、轮径）、里程计配置、速度限制	PID 增益（力控精度）、力矩范围（最大 / 最小发力）
输入 / 输出	输入：/cmd_vel（线速度 / 角速度）；输出：车轮目标指令（速度 / 力矩）+ /odom	输入：上层的车轮目标指令（力矩）；输出：车轮实际状态（位置 / 速度 / 力）
适用场景	所有差速轮移动机器人（小车、AGV 等）	需要精准力矩控制的场景（仿真、带力矩传感器的真实电机、机械臂关节）

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

  <!-- ========== Include 部分 ========== -->
  <xacro:include filename="$(find robot_description)/urdf/base.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/mybot.ros2_control.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/camera.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/imu.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/sensor/laser.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/actuator/wheel.urdf.xacro" />
  <xacro:include filename="$(find robot_description)/urdf/actuator/caster.urdf.xacro" />
  <!-- <xacro:include filename="$(find robot_description)/urdf/plugins/gazebo_control_plugin.xacro" /> -->
  <xacro:include filename="$(find robot_description)/urdf/plugins/gazebo_sensor_plugin.xacro" />

  <!-- ========== Base ========== -->
  <xacro:base_xacro
      base_name="base_link"
      xyz="0 0 0"
      rpy="0 0 0"
      radius="0.1"
      length="0.12"
      base_color="White"
      rgba="1 1 1 0.5" />

  <!-- ========== IMU ========== -->
  <xacro:imu_xacro
      imu_name="imu_link"
      xyz="0 0 0"
      rpy="0 0 0"
      size="0.02 0.02 0.02"
      imu_color="Black"
      rgba="0 0 0 0.5"
      joint_name="imu_joint"
      joint_type="fixed"
      parent_name="base_link"
      child_name="imu_link"
      joint_xyz="0 0 0.02"
      joint_rpy="0 0 0" />

  <!-- ========== Camera ========== -->
  <xacro:camera_xacro
      camera_name="camera_link"
      xyz="0 0 0"
      rpy="0 0 0"
      size="0.02 0.1 0.02"
      camera_color="Black"
      rgba="0 0 0 0.5"
      joint_name="camera_joint"
      joint_type="fixed"
      parent_name="base_link"
      child_name="camera_link"
      joint_xyz="0.1 0 0.075"
      joint_rpy="0 0 0"/>

  <!-- ========== Laser  ========== -->
  <xacro:laser_xacro
      laser_name="laser"
      parent_name="base_link"

      joint1_name="laser_stand_joint"
      joint1_type="fixed"
      joint1_xyz="0 0 0.1"
      joint1_rpy="0 0 0"

      joint2_name="laser_mount_joint"
      joint2_type="fixed"
      joint2_xyz="0 0 0.05"
      joint2_rpy="0 0 0"

      cylinder_xyz="0 0 0"
      cylinder_rpy="0 0 0"
      cylinder_radius="0.01"
      cylinder_length="0.1"
      cylinder_color="Black"
      cylinder_rgba="0 0 0 1.0"

      sensor_xyz="0 0 0"
      sensor_rpy="0 0 0"
      sensor_radius="0.02"
      sensor_length="0.02"
      sensor_color="Black"
      sensor_rgba="0 0 0 1.0" />

  <!-- ========== Wheel ========== -->
  <xacro:wheel_xacro
      wheel_name="wheel_left"
      xyz="0 0 0"
      rpy="1.57079 0 0"
      radius="0.032"
      length="0.04"
      wheel_color="Yellow"
      rgba="1 1 0 0.8"
      joint_name="wheel_left_joint"
      joint_type="continuous"
      parent_name="base_link"
      child_name="wheel_left"
      joint_xyz="0 0.1 -0.06"
      joint_rpy="0 0 0" />
    <xacro:wheel_xacro
      wheel_name="wheel_right"
      xyz="0 0 0"
      rpy="1.57079 0 0"
      radius="0.032"
      length="0.04"
      wheel_color="Yellow"
      rgba="1 1 0 0.8"
      joint_name="wheel_right_joint"
      joint_type="continuous"
      parent_name="base_link"
      child_name="wheel_right"
      joint_xyz="0 -0.1 -0.06"
      joint_rpy="0 0 0" />

    <!-- ========== Caster ========== -->
    <xacro:caster_xacro
    caster_name="caster_front"
    xyz="0 0 0"
    rpy="0 0 0"
    radius="0.016"
    caster_color="Yellow"
    rgba="1 1 0 0.8"
    joint_name="caster_front_joint"
    joint_type="fixed"
    parent_name="base_link"
    child_name="caster_front"
    joint_xyz="0.08 0 -0.076"
    joint_rpy="0 0 0" />
    <xacro:caster_xacro
    caster_name="caster_back"
    xyz="0 0 0"
    rpy="0 0 0"
    radius="0.016"
    caster_color="Yellow"
    rgba="1 1 0 0.8"
    joint_name="caster_back_joint"
    joint_type="fixed"
    parent_name="base_link"
    child_name="caster_back"
    joint_xyz="-0.08 0 -0.076"
    joint_rpy="0 0 0" />
    <!-- <xacro:gazebo_control_plugin /> -->
    <xacro:gazebo_sensor_plugin />
    <xacro:mybot_ros2_control />
</robot>

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription,ExecuteProcess,RegisterEventHandler
from launch.substitutions import Command, LaunchConfiguration
from launch.event_handlers import OnProcessExit
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterValue
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    urdf_package_path = get_package_share_directory('robot_description')
    default_xacro_path = os.path.join(urdf_package_path,'urdf','mybot.urdf.xacro')
    default_gazebo_path = os.path.join(urdf_package_path,'world','custom_room.world')
    action_declare_arg_mode_path=DeclareLaunchArgument('model',default_value=str(default_xacro_path))
    command_result = Command(['xacro ',LaunchConfiguration('model')])
    robot_description_value = ParameterValue(command_result,value_type=str)
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}],
    )
     # 通过 IncludeLaunchDescription 包含另外一个 launch 文件
    action_launch_gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']),
        launch_arguments=[('world',default_gazebo_path),('verbose','true')],
    )
    action_spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic','/robot_description','-entity','mybot'],

    )
    # ExecuteProcess 是 ROS 2 Launch 系统中的一个 “命令行执行工具”，核心作用是 在 Launch 启动流程中，直接调用操作系统的命令行指令
    # 加载关节状态广播器（必须第一个激活，负责发布/joint_states话题）
    action_load_joint_state_controller = ExecuteProcess(cmd='ros2 control load_controller mybot_joint_state_broadcaster --set-state active'.split(' '))
    # 加载力控控制器（底层执行器，驱动车轮关节，依赖广播器）
    action_load_effort_controller =ExecuteProcess(cmd='ros2 control load_controller mybot_effort_controller --set-state active'.split(' '))
    # 加载差速驱动控制器（上层决策器，解算cmd_vel，依赖力控控制器）
    action_load_diff_drive_controller =ExecuteProcess(cmd='ros2 control load_controller mybot_diff_drive_controller --set-state active'.split(' '))
    
    return LaunchDescription(
        [action_declare_arg_mode_path,
        robot_state_publisher,
        action_launch_gazebo,
        action_spawn_entity,
         # 事件监听器，当某个事件触发后开始执行等待该事件触发的事件
        RegisterEventHandler(event_handler=OnProcessExit(target_action=action_spawn_entity,on_exit=[action_load_joint_state_controller],)),
        RegisterEventHandler(event_handler=OnProcessExit(target_action=action_load_joint_state_controller,on_exit=[action_load_effort_controller],)),
        RegisterEventHandler(event_handler=OnProcessExit(target_action=action_load_effort_controller,on_exit=[action_load_diff_drive_controller],)),
        ]
    )