ROS2入门：机器人仿真
一、仿真介绍
常用仿真工具
- Gazebo / Ignition：三维物理仿真环境
- RViz2：数据可视化、传感器模拟
- rqt 工具套件：调试、可视化话题和节点关系
ROS2 仿真工作流概述
- 机器人建模（URDF/Xacro）
- 加载模型到 Gazebo
- 控制器配置（ros2_control）
- 可视化与调试（RViz、rqt）
- 算法验证（SLAM、导航、路径规划等）
二、机器人建模（URDF）
1. URDF是什么
URDF 是 Unified Robot Description Format 的缩写，即统一机器人描述格式。它是一种基于 XML 的文件格式，用来描述机器人各个部分的物理结构和运动关系。在 ROS 和 ROS2 中，URDF 是机器人仿真、可视化和控制的基础文件。
通过 URDF 文件，你可以告诉 ROS：
- 机器人由哪些刚体（Link）组成；
- 各个刚体之间通过什么样的关节（Joint）连接；
- 每个部件的几何形状、尺寸、质量和惯性参数；
- 视觉模型和碰撞检测模型的区别；
- 各部件的坐标系定义。
URDF 文件本质上是机器人“数字孪生”的骨架，它既能在 RViz 中显示机器人的外观，也能加载到 Gazebo 等物理仿真器中，实现动态仿真和控制。
这个例子描述了一个带底盘和轮子的简单移动机器人：
xml
<?xml version="1.0"?>
<robot name="simple_bot">
<!-- 底盘 -->
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.3 0.1"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size="0.5 0.3 0.1"/>
</geometry>
</collision>
<inertial>
<mass value="2.0"/>
<inertia ixx="0.02" ixy="0.0" ixz="0.0"
iyy="0.02" iyz="0.0" izz="0.02"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</inertial>
</link>
<!-- 左轮 -->
<link name="left_wheel">
<visual>
<geometry>
<cylinder length="0.05" radius="0.05"/>
</geometry>
<material name="black"/>
</visual>
</link>
<!-- 右轮 -->
<link name="right_wheel">
<visual>
<geometry>
<cylinder length="0.05" radius="0.05"/>
</geometry>
<material name="black"/>
</visual>
</link>
<!-- 左轮关节 -->
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0 0.15 -0.05" rpy="0 1.5708 0"/>
<axis xyz="0 0 1"/>
</joint>
<!-- 右轮关节 -->
<joint name="right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="0 -0.15 -0.05" rpy="0 1.5708 0"/>
<axis xyz="0 0 1"/>
</joint>
</robot>说明：
<robot>：定义机器人名称。<link>：定义刚体，包括可视化（visual）、碰撞（collision）、惯性（inertial）。<joint>：定义关节连接关系，type="continuous" 表示轮子可以无限旋转。<origin>：定义相对于父坐标系的位置和姿态（xyz + rpy）。<axis>：定义旋转轴方向。
2. URDF实践-创建一个简单的机器人
新建工作区和功能包：
mddir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create fishbot_description --build-type ament_cmake --license TIM新建 URDF 文件:
在包 fishbot_description 下创建 urdf 文件夹，并新建first_robot.urdf：
xml
<?xml version="1.0"?>
<robot name="first_robot">
<!-- 身体部分 -->
<link name="base_link">
<!-- 部件的外观描述 -->
<visual>
<!-- 旋转平移 -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<!-- 形状 -->
<geometry>
<!-- 圆柱体 -->
<cylinder length="0.12" radius="0.1"/>
</geometry>
<!-- 材质 颜色 -->
<material name="white">
<color rgba="1 1 1 0.5"/>
</material>
</visual>
</link>
<!-- IMU -->
<link name="imu_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
<material name="blue">
<color rgba="0 0 1 0.5"/>
</material>
</visual>
</link>
<!-- 关节（组合身体和IMU） -->
<joint name="imu_joint" type="fixed">
<!-- 关节连接的部件 -->
<parent link="base_link"/>
<child link="imu_link"/>
<origin rpy="0 0 0.03" xyz="0 0 0"/>
</joint>
</robot>这个 URDF 描述了一个简单的机器人模型，包括一个圆柱形底盘（base_link）和一个固定在底盘上方的 IMU 传感器（imu_link），IMU 通过固定关节与底盘连接。
2. 使用 RViz 的 RobotModel 组件可视化 URDF
启动 RViz2
rviz2添加 RobotModel:
- 在左侧 Displays 面板点击 Add
- 选择 RobotModel → OK
- 在 RobotModel 属性中找到 Description File（或 Robot Description），选择你的 URDF 文件路径
可以正常显示，但是部件之间的关系不太对，因为RobotModel 不识别URDF文件里的位置关系
3. 修正部件位置关系
虽然 RobotModel 可以直接加载 URDF 显示机器人模型，但它无法自动识别关节的动态位置或固定偏移，这会导致部件之间的相对位置不正确。为了解决这个问题，可以借助以下工具：
（1）Joint State Publisher
- 功能：提供一个 GUI 界面或者话题接口，用于发布机器人各关节的状态（角度或位置）。
- 对于固定关节，也可以设置初始偏移，使 RobotModel 能正确显示部件之间的位置关系。
（2）Robot State Publisher
- 功能：订阅 /joint_states 话题，将关节状态转换为 TF 树，RobotModel 可以根据 TF 树正确渲染模型
- 对于固定关节（如 IMU 固定在底盘上），Robot State Publisher 会解析 URDF 中的
<joint type="fixed">，发布静态 TF，使部件位置正确
安装命令如下：
sudo apt install ros-$ROS_DISTRO-joint-state-publisher
sudo apt install ros-$ROS_DISTRO-robot-state-publisher启动命令如下：
ros2 run joint_state_publisher joint_state_publisher
ros2 run robot_state_publisher robot_state_publisher --ros-args -p robot_description:=<urdf_file_path>我们可以在包下面建立一个launch文件，通过launch一键启动
在 launch 文件夹下新建 display_robot.launch.py：
python
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# 获取默认路径
urdf_tutorial_path = get_package_share_directory('fishbot_description')
default_model_path = urdf_tutorial_path + '/urdf/first_robot.urdf'
default_rviz_config_path = urdf_tutorial_path + '/config/rviz/display_model.rviz'
# 为 Launch 声明参数
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
name='model', default_value=str(default_model_path),
description='URDF 的绝对路径')
# 获取文件内容生成新的参数
robot_description = launch_ros.parameter_descriptions.ParameterValue(
launch.substitutions.Command(
['cat ', launch.substitutions.LaunchConfiguration('model')]),
value_type=str)
# 状态发布节点
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}]
)
# 关节状态发布节点
joint_state_publisher_node = launch_ros.actions.Node(
package='joint_state_publisher',
executable='joint_state_publisher',
)
# RViz 节点
rviz_node = launch_ros.actions.Node(
package='rviz2',
executable='rviz2',
arguments=['-d', default_rviz_config_path]
)
return launch.LaunchDescription([
action_declare_arg_mode_path,
joint_state_publisher_node,
robot_state_publisher_node,
rviz_node
])在CMackeLists.txt里添加
# 安装 launch 文件
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
)编译并启动
bash
colcon build
source install/setup.bash
ros2 launch fishbot_description display_robot.launch.py显示正常： 
4. 使用Xacro简化URDF
Xacro（XML Macros）是一种宏扩展语言，可以生成标准 URDF 文件。它可以通过参数化和宏定义来简化复杂机器人的 URDF 文件，使得机器人模型更易维护和复用。 优点
- 避免重复代码，例如多个轮子或传感器的重复定义；
- 可以使用参数控制尺寸、位置、颜色等；
- 支持条件判断和循环，方便生成复杂结构；
下面我们使用Xacro重构一下 本节第二部分的first_robot.urdf，我们新建一个first_robot.xacro文件
xml
<?xml version="1.0"?>
<robot xmlns:xacro = "http://www.ros.org/wiki/xacro" name="first_robot">
<xacro:macro name="base_link" params="length radius">
<!-- 身体部分 -->
<link name="base_link">
<!-- 部件的外观描述 -->
<visual>
<!-- 旋转平移 -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<!-- 形状 -->
<geometry>
<!-- 圆柱体 -->
<cylinder length="${length}" radius="${radius}"/>
</geometry>
<!-- 材质 颜色 -->
<material name="white">
<color rgba="1 1 1 0.5"/>
</material>
</visual>
</link>
<xacro:macro />
<xacro:macro name="imu_link" params="imu_name xyz">
<!-- IMU -->
<link name="${imu_name}_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
<material name="blue">
<color rgba="0 0 1 0.5"/>
</material>
</visual>
</link>
<!-- 关节（组合身体和IMU） -->
<joint name="imu_joint" type="fixed">
<!-- 关节连接的部件 -->
<parent link="base_link"/>
<child link="${imu_name}_link"/>
<origin rpy="0 0 0" xyz="${xyz}"/>
</joint>
<xacro:macro />
<xacro:base_link length="0.12" radius="0.1"/>
<xacro:imu_link imu_name="imu-up" xyz="0.0 0 0.05"/>
<xacro:imu_link imu_name="imu-down" xyz="0.0 0 -0.05"/>
</robot>xacro文件无法直接使用，我们需要使用工具将xacro文件转化为urdf文件：
安装工具：
bash
sudo apt install ros-$ROS_DISTRO-xacro通过命令行生成 URDF，使用 xacro 命令将 .xacro 文件转换为标准 URDF 文件：
xacro first_robot.xacro如下： 
我们可以利用这个命令来修改launch文件
python
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# 获取默认路径
urdf_tutorial_path = get_package_share_directory('fishbot_description')
default_model_path = urdf_tutorial_path + '/urdf/first_robot.xacro'
default_rviz_config_path = urdf_tutorial_path + '/config/rviz/display_model.rviz'
# 为 Launch 声明参数
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
name='model', default_value=str(default_model_path),
description='URDF 的绝对路径')
# 获取文件内容生成新的参数
robot_description = launch_ros.parameter_descriptions.ParameterValue(
launch.substitutions.Command(
['xacro ', launch.substitutions.LaunchConfiguration('model')]),
value_type=str)
# 状态发布节点
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}]
)
# 关节状态发布节点
joint_state_publisher_node = launch_ros.actions.Node(
package='joint_state_publisher',
executable='joint_state_publisher',
)
# RViz 节点
rviz_node = launch_ros.actions.Node(
package='rviz2',
executable='rviz2',
arguments=['-d', default_rviz_config_path]
)
return launch.LaunchDescription([
action_declare_arg_mode_path,
joint_state_publisher_node,
robot_state_publisher_node,
rviz_node
])编译并启动
bash
colcon build
source install/setup.bash
ros2 launch fishbot_description display_robot.launch.py可以看到这里已经显示新的机器人结构了：
5. 创建一个完整的机器人
下面我们基于上面的知识，建立一个新的完整的机器人模型。这里我们使用多个xacro文件复用的形式：
5.1 本体及传感器
新建文件：base.urdf.xacro ，作为机器人的主题部分：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 定义底盘宏 -->
<xacro:macro name="base_xacro" params="length radius">
<!-- 身体部分 -->
<link name="base_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="${length}" radius="${radius}"/>
</geometry>
<material name="white">
<color rgba="1 1 1 0.5"/>
</material>
</visual>
</link>
</xacro:macro>
</robot>新建文件：sensors/imu.urdf.xacro ，作为通用imu单元：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 定义IMU宏 -->
<xacro:macro name="imu_xacro" params=" xyz">
<link name="imu_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
<material name="blue">
<color rgba="0 0 1 0.5"/>
</material>
</visual>
</link>
<!-- 固定关节 -->
<joint name="imu_joint" type="fixed">
<parent link="base_link"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="${xyz}"/>
</joint>
</xacro:macro>
</robot>新建文件：sensors/camera.urdf.xacro ，作为相机单元：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 定义camera宏 -->
<xacro:macro name="camera_xacro" params=" xyz">
<link name="camera_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.02 0.10 0.02"/>
</geometry>
<material name="blue">
<color rgba="0 0 1 0.5"/>
</material>
</visual>
</link>
<!-- 固定关节 -->
<joint name="camera_joint" type="fixed">
<parent link="base_link"/>
<child link="camera_link"/>
<origin rpy="0 0 0" xyz="${xyz}"/>
</joint>
</xacro:macro>
</robot>新建文件：sensors/laser.urdf.xacro ，作为激光雷达单元：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="laser_xacro" params="xyz">
<!-- ============雷达支撑杆================ -->
<link name="laser_cylinder_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.10" radius="0.01" />
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8" />
</material>
</visual>
</link>
<joint name="laser_cylinder_joint" type="fixed">
<parent link="base_link" />
<child link="laser_cylinder_link" />
<origin xyz="${xyz}" />
</joint>
<!-- ============雷达================ -->
<link name="laser_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.02" radius="0.02" />
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8" />
</material>
</visual>
</link>
<joint name="laser_joint" type="fixed">
<parent link="laser_cylinder_link" />
<child link="laser_link" />
<origin xyz="0 0 0.05" />
</joint>
</xacro:macro>
</robot>接下来我们新建一个fishbot.urdf.xacro文件来组装这些原件：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fishbot">
<!-- 包含其他xacro文件 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/base.urdf.xacro" />
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensors/imu.urdf.xacro" />
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensors/camera.urdf.xacro" />
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensors/laser.urdf.xacro" />
<xacro:base_xacro length = "0.12" radius = "0.10" />
<xacro:imu_xacro xyz = "0.0 0.0 0.02" />
<xacro:camera_xacro xyz = "0.10 0.0 0.075" />
<xacro:laser_xacro xyz = "0.0 0.0 0.10" />
</robot>注意这里的文件路径需要一致化（和自己的实际文件一致）
编译并启动(注意选择自己的模型文件的位置)：
bash
colcon build
source install/setup.bash
ros2 launch fishbot_description display_robot.launch.py model:=/root/project/ros2_learning/part6/ros2_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro这就是我们的机器人主体结构了：
5.2 执行器器件
下面我们完善机器人的执行器件，执行器件是很复杂的部分，最简单的莫过于运动行走机构-轮子。下面我们为机器人添加轮子，使其可以运动：
创建文件urdf/fishbot/actuator/wheel.urdf.xacro,这是轮子：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="wheel_xacro" params="wheel_name xyz">
<link name="${wheel_name}_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="1.57079 0 0" />
<geometry>
<cylinder length="0.04" radius="0.032" />
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8"/>
</material>
</visual>
</link>
<gazebo reference="${wheel_name}_wheel_link">
<mu1 value="20.0" />
<mu2 value="20.0" />
<kp value="1000000000.0" />
<kd value="1.0" />
</gazebo>
<joint name="${wheel_name}_wheel_joint" type="continuous">
<parent link="base_link" />
<child link="${wheel_name}_wheel_link" />
<origin xyz="${xyz}" />
<axis xyz="0 1 0" />
</joint>
</xacro:macro>
</robot>创建文件urdf/fishbot/actuator/caster.urdf.xacro,这是辅助万向轮：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="caster_xacro" params="caster_name xyz">
<link name="${caster_name}_caster_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="0.016" />
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8"/>
</material>
</visual>
</link>
<joint name="${caster_name}_caster_joint" type="fixed">
<parent link="base_link" />
<child link="${caster_name}_caster_link" />
<origin xyz="${xyz}" />
<axis xyz="0 0 0" />
</joint>
</xacro:macro>
</robot>同样在机器人主体中加入这些部件：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fishbot">
<!-- 包含其他xacro文件 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/base.urdf.xacro" />
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensors/imu.urdf.xacro" />
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensors/camera.urdf.xacro" />
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensors/laser.urdf.xacro" />
<!-- 执行器 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/wheel.urdf.xacro" />
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/caster.urdf.xacro" />
<!-- 创建机器人 -->
<xacro:base_xacro length = "0.12" radius = "0.10" />
<xacro:imu_xacro xyz = "0.0 0.0 0.02" />
<xacro:camera_xacro xyz = "0.10 0.0 0.075" />
<xacro:laser_xacro xyz = "0.0 0.0 0.10" />
<xacro:wheel_xacro wheel_name = "left_wheel" xyz = "0.0 0.1 -0.06" />
<xacro:wheel_xacro wheel_name = "right_wheel" xyz = "0.0 -0.1 -0.06" />
<xacro:caster_xacro caster_name = "front_caster" xyz = "0.08 0.0 -0.06" />
<xacro:caster_xacro caster_name = "back_caster" xyz = "-0.08 0.0 -0.06" />
</robot>效果如图：
5.3 虚拟部件
有时候为了方便调试和规划，需要给机器人添加一些辅助坐标系或虚拟连接，这里我们为了让机器人贴合地面，就可以利用虚拟部件（虚拟部件没有内容，但是和其他器件有几何关系）。
我们在base.urdf.xacro里添加虚拟部件：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="base_xacro" params="length radius">
<link name="base_footprint" />
<joint name="base_joint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0.0 0.0 ${length/2.0+0.032-0.001}" rpy="0 0 0" />
</joint>
<link name="base_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0" />
<geometry>
<cylinder length="${length}" radius="${length}" />
</geometry>
<material name="blue">
<color rgba="0.1 0.1 1.0 0.5"/>
</material>
</visual>
</link>
</xacro:macro>
</robot>添加了虚拟部件 base_footprint，选择参考系为base_footprint就可以达到让机器人站到地面的效果：
5.4 碰撞属性
机器人在仿真中要考虑碰撞检测，需要为每个 link 添加 <collision> 标签，和 <visual> 类似，但可以简化几何模型以提高性能。
例如在 wheel.urdf.xacro 中加上：
xml
<collision>
<origin xyz="0 0 0" rpy="1.57079 0 0" />
<geometry>
<cylinder length="0.04" radius="0.032" />
</geometry>
</collision>在base.urdf.xacro中同样加：
xml
<collision>
<origin xyz="0 0 0" />
<geometry>
<cylinder length="${length}" radius="${radius}" />
</geometry>
</collision>注意：<collision> 里的内容可以和 <visual> 一致，这样碰撞体积就和实际看到的体积是相同的。但碰撞几何体可以简化为长方体、圆柱体等基本形状，提高计算效率。
给每一个部件添加碰撞属性后，在软件中勾选碰撞属性显示和之前一致说明都加上碰撞属性了，如下图：
5.5 质量与惯性
为了让机器人在仿真中具备真实的物理特性（例如惯性、重力、动力学计算），需要给每个 link 添加<inertial>标签，用于定义质量和惯性张量。
<inertial> 标签结构：
xml
<inertial>
<mass value="质量(kg)" />
<origin xyz="质心位置" rpy="0 0 0" />
<inertia
ixx="xx" ixy="xy" ixz="xz"
iyy="yy" iyz="yz" izz="zz" />
</inertial>- mass：刚体质量，单位 kg。
- origin：质心位置，相对于 link 坐标系。
- inertia：惯性张量的 6 个分量，描述物体的转动惯量。
由于这个3X3矩阵是对称的，说要最后我们使用六个分量来表示物体的转动惯量。
新建文件urdf/fishbot/common_inertia.xacro 来描述惯性：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="box_inertia" params="m w h d">
<inertial>
<mass value="${m}" />
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertia" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy="0" ixz="0" iyy="${(m/12) * (3*r*r + h*h)}" iyz="0" izz="${(m/2) * (r*r)}" />
</inertial>
</xacro:macro>
<xacro:macro name="sphere_inertia" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}" />
</inertial>
</xacro:macro>
</robot>比如我们给机器人主体添加质量和惯性base.urdf.xacro：
引入模块：
xml
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />添加质量和惯性：
xml
<xacro:cylinder_inertia m="1.0" r="${radius}" l="${length}" />最终base.urdf.xacro如下：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
<xacro:macro name="base_xacro" params="length radius">
<link name="base_footprint" />
<joint name="base_joint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0.0 0.0 ${length/2.0+0.032-0.001}" rpy="0 0 0" />
</joint>
<link name="base_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0" />
<geometry>
<cylinder length="${length}" radius="${radius}" />
</geometry>
<material name="blue">
<color rgba="1 1 1.0 0.5"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.0" rpy="0 0 0" />
<geometry>
<cylinder length="${length}" radius="${radius}" />
</geometry>
<material name="blue">
<color rgba="1 1 1.0 0.5"/>
</material>
</collision>
<xacro:cylinder_inertia m="1.0" r="${radius}" h="${length}" />
</link>
</xacro:macro>
</robot>编译，打开如软件，关闭外观显示，打开质量显示：
显示这个红球，说明设置对了。
按照这样的设置，给其他部件添加惯性和重量，最终效果如下：
至此我们就完成了一个小机器人的建模，接下来我们就可以将机器人导入仿真软件进行仿真
三、机器人仿真（Gazebo）
Gazebo 是一个开源的机器人仿真平台，可以提供逼真的物理环境，包括重力、摩擦和碰撞力，同时支持多种传感器仿真如摄像头、激光雷达和 IMU，并能直接加载 ROS 的 URDF/Xacro 机器人模型或 SDF 场景文件，方便在虚拟环境中验证机械结构和控制算法，同时通过gazebo_ros插件与 ROS2 无缝集成，实现机器人状态、传感器数据的实时发布和控制，为开发和调试机器人提供直观、高效的仿真环境。
1.Gazebo安装
bash
sudo apt update
sudo apt install gazebo下载模型(模型比较大，尽量科学上网)：
mkdir -p ~/.gazebo/models
# 手动从 GitHub 克隆官方模型仓库
git clone https://github.com/osrf/gazebo_models ~/.gazebo/models打开gazebo：
gazebo这里也显示出很多我们刚才下载好的模型，可供我们调用：
2.在Gazebo中加载机器人模型
Gazebo 本身使用的是 SDF（Simulation Description Format） 格式，而我们机器人建模使用的是 URDF（Unified Robot Description Format）。不过不用手动转换，ROS2 提供了 gazebo_ros 插件和 robot_state_publisher 节点，可以在启动时自动把 URDF 转成 SDF 并加载到 Gazebo 中。
安装 ROS2 与 Gazebo 的集成插件：
bash
sudo apt install ros-${ROS_DISTRO}-gazebo-ros-pkgs我们新建一个launch文件，在 fishbot_description/launch/ 下创建 gazebo_sim.launch.py：
python
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.parameter_descriptions import ParameterValue
from launch.actions import TimerAction
def generate_launch_description():
# 获取默认路径
urdf_tutorial_path = get_package_share_directory('fishbot_description')
default_model_path = urdf_tutorial_path + '/urdf/fishbot/fishbot.urdf.xacro'
default_gazebo_world_path = urdf_tutorial_path + '/worlds/room.world'
# 声明 Launch 参数
declare_model_path_cmd = launch.actions.DeclareLaunchArgument(
name='model',
default_value=str(default_model_path),
description='URDF 的绝对路径'
)
# robot_description 参数
robot_description = ParameterValue(
Command(['xacro ', LaunchConfiguration('model')]),
value_type=str
)
# 状态发布节点
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}]
)
# 启动 Gazebo 仿真环境
launch_gazebo = launch.actions.IncludeLaunchDescription(
PythonLaunchDescriptionSource([
get_package_share_directory('gazebo_ros'), '/launch', '/gazebo.launch.py'
]),
launch_arguments=[
('world', default_gazebo_world_path),
('verbose', 'true')
]
)
spawn_entity_node = launch_ros.actions.Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-topic', '/robot_description',
'-entity', 'fishbot',
]
)
delayed_spawn = TimerAction(
period=10.0, # 延迟 5 秒启动
actions=[spawn_entity_node]
)
return launch.LaunchDescription([
declare_model_path_cmd,
robot_state_publisher_node,
launch_gazebo,
delayed_spawn
])这里的'/worlds/room.world'是保存的Gazebo世界文件，随便新建一个世界，搭建点场景保存即可，路径和自己保存的一致即可
还要在CMakeLists.txt里添加：
# 安装 launch 和 urdf文件
install(DIRECTORY launch urdf worlds
DESTINATION share/${PROJECT_NAME}/
)把场景文件包含进去。
编译并启动
bash
colcon build
source install/setup.bash
ros2 launch fishbot_description gazebo_sim.launch.py就可以看到我们之前建模的机器人：
但是由于标签转换过程中会丢失数据，这里机器人的颜色都没有了，下面我们学习在机器人模型里添加Gazebo模型来扩展URDF标签，完善机器人。
2.使用Gazebo标签扩展URDF
URDF 默认只描述机器人的结构、关节和惯性等信息。为了让 Gazebo 更真实地模拟机器人，需要用<gazebo>标签：
比如去修改机器人的颜色，把雷达部分改为黑色：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
<xacro:macro name="laser_xacro" params="xyz">
<gazebo reference="laser_cylinder_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="laser_link">
<material>Gazebo/Black</material>
</gazebo>
<!-- ============雷达支撑杆================ -->
<link name="laser_cylinder_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.10" radius="0.01" />
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.10" radius="0.01" />
</geometry>
</collision>
<xacro:cylinder_inertia m="0.05" r="0.01" h="0.10" />
</link>
<joint name="laser_cylinder_joint" type="fixed">
<parent link="base_link" />
<child link="laser_cylinder_link" />
<origin xyz="${xyz}" />
</joint>
<!-- ============雷达================ -->
<link name="laser_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.02" radius="0.02" />
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.02" radius="0.02" />
</geometry>
</collision>
<xacro:cylinder_inertia m="0.10" r="0.02" h="0.02" />
</link>
<joint name="laser_joint" type="fixed">
<parent link="laser_cylinder_link" />
<child link="laser_link" />
<origin xyz="0 0 0.05" />
</joint>
</xacro:macro>
</robot>这里我们额外添加了：
xml
<gazebo reference="laser_cylinder_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="laser_link">
<material>Gazebo/Black</material>
</gazebo>重新编译运行：
这里的颜色就调整过来了。
还有比如说，给轮子增大摩擦力：
xml
<gazebo reference="${wheel_name}_wheel_link">
<mu1 value="20.0" />
<mu2 value="20.0" />
<kp value="1000000000.0" />
<kd value="1.0" />
</gazebo>- mu1：沿轮子法线方向的摩擦系数（通常与轮子旋转方向垂直）。
- mu2：沿轮子切线方向的摩擦系数（通常与轮子旋转方向平行）。
- kp：弹簧刚度（spring constant），用于模拟接触点的刚性，值越大接触更“硬”。
- kd：阻尼系数（damping constant），用于模拟接触点的阻尼，值越大碰撞后震动衰减越快。
- 里 kp=1e9 表示轮子与地面接触几乎是刚性接触；kd=1.0 给轮子一个轻微阻尼，避免抖动。
3.使用两轮差速插件控制机器人
为了让机器人可控，需要给轮子添加 Gazebo 差速驱动插件，通过使用Gazebo插件，可以
- 模拟轮子的物理驱动：插件会根据你发送的速度命令计算左右轮子的转速，并在仿真中推动机器人移动，而不必手动设置每个关节的旋转。
- 自动发布里程计信息：插件会生成 /odom 里程计消息，提供机器人位置和姿态信息，可用于导航或控制算法。
- 接收 ROS 速度命令：通过订阅 /cmd_vel（geometry_msgs/Twist 类型）消息，插件可以驱动机器人前进、后退和旋转，实现远程控制或自动导航。
- 参数可调：可以在插件中设置轮子半径、轮距、摩擦力、阻尼等参数，使仿真运动更接近真实情况。
建立urdf/fishbot/plugins/gazebo_control_plugin.xacro文件，用于实现机器人控制：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="gazebo_control_plugin">
<gazebo>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
<ros>
<namespace>/</namespace>
<remapping>cmd_vel:=cmd_vel</remapping>
<remapping>odom:=odom</remapping>
</ros>
<update_rate>30</update_rate>
<!-- wheels -->
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<!-- kinematics -->
<wheel_separation>0.2</wheel_separation>
<wheel_diameter>0.064</wheel_diameter>
<!-- limits -->
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<!-- output -->
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
</plugin>
</gazebo>
</xacro:macro>
</robot>总的来说实现以下几个功能，详细的解释可以让AI逐行解释：
- 加载差速驱动插件：libgazebo_ros_diff_drive.so 控制左右轮运动。
- ROS 话题交互：通过 /cmd_vel 接收速度指令，发布 /odom 里程计信息和 TF。
- 轮子参数配置：指定左右轮关节、轮距、轮径，限制轮子最大力矩和加速度，保证运动符合物理规律。
- 更新频率和 TF：设置插件更新频率、发布里程计和轮子关节的 TF 变换，便于可视化和导航。
我们在机器人主体中引入，就可以使用了：
xml
<!-- 引入插件 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_control_plugin.xacro" />
<!-- Gazebo差速驱动插件 -->
<xacro:gazebo_control_plugin />编译运行后，就出现了控制话题：
可以使用键盘控制节点进行控制：ros2 run teleop_twist_keyboard teleop_twist_keyboard
也可以使用rqt工具查看当前的目录树（可以看到还是比较复杂的）：
4.激光雷达传感器仿真
建立urdf/fishbot/plugins/gazebo_sensor_plugin.xacro文件，用于实现传感器仿真：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="gazebo_sensor_plugin">
<gazebo reference="laser_link">
<sensor name="laserscan" type="ray">
<plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
<ros>
<namespace>/</namespace>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>laser_link</frame_name>
</plugin>
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>5</update_rate>
<pose>0 0 0 0 0 0</pose>
<!-- 激光传感器配置 -->
<ray>
<!-- 设置扫描范围 -->
<scan>
<horizontal>
<samples>360</samples>
<resolution>1.000000</resolution>
<min_angle>0.000000</min_angle>
<max_angle>6.280000</max_angle>
</horizontal>
</scan>
<!-- 设置扫描距离 -->
<range>
<min>0.120000</min>
<max>8.0</max>
<resolution>0.015000</resolution>
</range>
<!-- 设置噪声 -->
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
</sensor>
</gazebo>
</xacro:macro>
</robot>在主体中引入：
xml
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro" />
<xacro:gazebo_sensor_plugin />效果如下：
5.惯性传感器IMU仿真
添加下面的内容到urdf/fishbot/plugins/gazebo_control_plugin.xacro文件，并在主体中注册：
xml
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
<ros>
<namespace>/</namespace>
<remapping>~/out:=imu</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<update_rate>100</update_rate>
<always_on>true</always_on>
<!-- 六轴噪声设置 -->
<imu>
<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>
<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>可以查看IMU发布的话题：
ros2 topic echo /imu --once
6.深度相机仿真
首先调整相机的建模，添加虚拟关系，修正相机坐标系：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
<xacro:macro name="camera_xacro" params="xyz">
<!-- ============相机模块================ -->
<link name="camera_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0" />
<geometry>
<box size="0.02 0.10 0.02" />
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.0" rpy="0 0 0" />
<geometry>
<box size="0.02 0.10 0.02" />
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8"/>
</material>
</collision>
<xacro:box_inertia m="0.01" w="0.02" h="0.10" d="0.02" />
</link>
<link name="camera_optical_link"></link>
<joint name="camera_optical_joint" type="fixed">
<parent link="camera_link" />
<child link="camera_optical_link" />
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
</joint>
<joint name="camera_joint" type="fixed">
<parent link="base_link" />
<child link="camera_link" />
<origin xyz="${xyz}" />
</joint>
</xacro:macro>
</robot>添加下面的内容到urdf/fishbot/plugins/gazebo_control_plugin.xacro文件，并在主体中注册：
xml
<gazebo reference="camera_link">
<sensor type="depth" name="camera_sensor">
<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>
<camera name="camera">
<horizontal_fov>1.5009831567</horizontal_fov>
<image>
<width>800</width>
<height>600</height>
<format>R8G8B8</format>
</image>
<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>这里分布使用三种工具预览深度相机的效果：
四、机器人控制（ros2_control）
ros2_control 是 ROS 2 生态系统中标准的机器人硬件抽象与控制框架。它为机器人开发者提供了统一、可扩展的接口，连接高级机器人算法（如路径规划、导航、感知等）与底层硬件设备。ros2_control 通过插件化的控制器与硬件接口、标准化的生命周期管理、实时控制循环和细粒度资源管理，极大提升了机器人系统的模块化、可维护性和实时性，是现代机器人控制系统的基础设施。
ros2_control 的整体架构分为以下几个关键层次：
- 控制器层（Controllers） ：实现具体的控制策略（如位置、速度、力矩控制），作为插件动态加载。
- 控制器管理器（Controller Manager） ：负责控制器的生命周期管理、资源分配、控制循环调度。
- 硬件接口层（Hardware Interface） ：定义统一的硬件抽象接口，负责与底层物理设备（关节、执行器、传感器等）的通信。
- 传动接口层（Transmission Interface） ：建模和管理虚拟关节与实际执行器之间的机械传动关系。
- 消息与服务层：通过 ROS 2 消息和服务实现控制器和硬件的远程管理与监控。
mermaid
graph TD
A[高级机器人算法<br>如 MoveIt, Nav2] --> B[Controller Plugins<br>控制器插件]
B --> C[Controller Manager<br>控制器管理器]
C --> D[Hardware Interface<br>硬件接口]
D --> E[具体硬件驱动/设备]
C --> F[Transmission Interface<br>传动接口]
F --> D
C --> G[Lifecycle Management<br>生命周期管理]
G --> C
C --> H[ROS 2 Services/Messages<br>服务/消息]
H --> C1 .ros2_control安装
本体安装：
sudo apt install ros-${ROS_DISTRO}-ros2-control控制器安装：
sudo apt install ros-${ROS_DISTRO}-ros2-controllers2 .使用Gazebo接入ros2_control
Gazebo接入ros2_control,其实就是让Gazebo按照rcs2_control指定的接口提供数据。在ROS2中利用相应的Gazebo插件，可以方便的实现Gazebo和ros2_control的对接。
安装Gazebo的ros2_control插件：
sudo apt install ros-${ROS_DISTRO}-gazebo-ros2-control然后我们需要在机器人模型中调用该插件：
新建urdf/fishbot/fishbot.ros_control.xacro文件：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="fishbot_ros2_control">
<ros2_control name="FishBotGazeboSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="left_wheel_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>
<joint name="right_wheel_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>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find fishbot_description)/config/fishbot_ros2_controller.yaml</parameters>
</plugin>
</gazebo>
</xacro:macro>
</robot>新建ros2_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml文件，用于插件参数配置：
controller_manager:
ros__parameters:
update_rate: 100 # Hz
use_sim_time: true在主文件引入并使用(注意去除原有的Gazebo控制插件)
xml
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/fishbot.ros_control.xacro" />
<xacro:fishbot_ros2_control />编译并启动
bash
colcon build
source install/setup.bash
ros2 launch fishbot_description gazebo_sim.launch.py我们可以使用这个命令列出所有硬件接口：
ros2 control list_hardware_interfaces如下：
3 .使用关节状态发布控制器
在ros2_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml文件中添加下面的配置，以启动关节状态发布器：
controller_manager:
ros__parameters:
update_rate: 100 # Hz
use_sim_time: true
fishbot_joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster并且使用命令激活：
ros2 control load_controller fishbot_joint_state_broadcaster --set-state active可以在启动脚本中添加，以自动激活：
python
action_load_joint_state_publisher = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', 'fishbot_joint_state_broadcaster', '--set-state', 'active'],
output='screen'
)
return launch.LaunchDescription([
declare_model_path_cmd,
robot_state_publisher_node,
launch_gazebo,
delayed_spawn,
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=spawn_entity_node,
on_exit=[action_load_joint_state_publisher]
)
)
])4 .使用力控制器控制轮子
在配置文件添加力控制器：
controller_manager:
ros__parameters:
update_rate: 100 # Hz
use_sim_time: true
fishbot_joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
use_sim_time: true
fishbot_effort_controller:
type: effort_controllers/JointGroupEffortController
fishbot_effort_controller:
ros__parameters:
joints:
- left_wheel_joint
- right_wheel_joint
command_interfaces:
- effort
state_interfaces:
- position
- velocity
- effort在启动文件添加激活动作：
python
action_load_effort_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', 'fishbot_effort_controller', '--set-state', 'active'],
output='screen'
)
return launch.LaunchDescription([
declare_model_path_cmd,
robot_state_publisher_node,
launch_gazebo,
delayed_spawn,
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=spawn_entity_node,
on_exit=[action_load_joint_state_publisher]
)
),
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=action_load_joint_state_publisher,
on_exit=[action_load_effort_controller]
)
)
])运行后使用ros2 topic list -t | grep effort 查看相关话题:
使用命令发布话题：
ros2 topic pub /fishbot_effort_controller/commands std_msg/msg/Float64MultiArray "{data:[0.001,0.001]}"
可以观察到车缓慢向前移动了。
5.使用两轮差速控制器控制机器人
在配置文件添加两轮差速控制器：
controller_manager:
ros__parameters:
update_rate: 100 # Hz
use_sim_time: true
fishbot_joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
use_sim_time: true
fishbot_effort_controller:
type: effort_controllers/JointGroupEffortController
fishbot_diff_drive_controller:
type: diff_drive_controller/DiffDriveController
fishbot_effort_controller:
ros__parameters:
joints:
- left_wheel_joint
- right_wheel_joint
command_interfaces:
- effort
state_interfaces:
- position
- velocity
- effort
fishbot_diff_drive_controller:
ros__parameters:
left_wheel_names: ["left_wheel_joint"]
right_wheel_names: ["right_wheel_joint"]
wheel_separation: 0.17
#wheels_per_side: 1 # actually 2, but both are controlled by 1 signal
wheel_radius: 0.032
wheel_separation_multiplier: 1.0
left_wheel_radius_multiplier: 1.0
right_wheel_radius_multiplier: 1.0
publish_rate: 50.0
odom_frame_id: odom
base_frame_id: base_footprint
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
enable_odom_tf: true
cmd_vel_timeout: 0.5
#publish_limited_velocity: true
use_stamped_vel: false
#velocity_rolling_window_size: 10在启动文件添加激活动作：
python
action_load_diff_drive_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', 'fishbot_diff_drive_controller', '--set-state', 'active'],
output='screen'
)
return launch.LaunchDescription([
declare_model_path_cmd,
robot_state_publisher_node,
launch_gazebo,
delayed_spawn,
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=spawn_entity_node,
on_exit=[action_load_joint_state_publisher]
)
),
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=action_load_joint_state_publisher,
on_exit=[action_load_diff_drive_controller]
)
)
])运行后使用ros2 topic list -t 查看话题列表:
修改fishbot.ros_control.xacro重映射话题（给话题改名字）：
xml
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find fishbot_description)/config/fishbot_ros2_controller.yaml</parameters>
<ros>
<remapping>/fishbot_diff_drive_controller/cmd_vel_unstamped:=/cmd_vel</remapping>
<remapping>/fishbot_diff_drive_controller/odom:=/odom</remapping>
</ros>
</plugin>这样就可以使用键盘控制节点去控制机器人了：
bash
ros2 run teleop_twist_keyboard teleop_twist_keyboardOK，本节到此结束