创建工作空间与环境
创建工作空间chap6，创建src目录。
src目录库下创建c++功能包fishbot_description，fishbot_description功能包下创建urdf文件夹，用于存放urdf文件。
fishbot_description功能包下创建launch文件夹，用于存放launch.py文件。在launch文件夹下新建gazebo_sim.launch.py
创建一个config文件夹，用于存放一些配置文件。
在config文件夹下创建一个rviz文件夹，用于存放rviz配置文件。
在fishbot_description创建world文件夹，用于存储地图模型，再创建一个custom_room.world 文件，存储地图模型
在src/fishbot_description/urdf/fishbot 目录下新建plugins目录，用于存放gazebo插件
机器人外观建模部分：
质量与惯性宏定义：
结合惯性公式,可以编写常用图形的惯性矩阵和质量的宏定义,在urdf/fishbot 目录下新建common_inertia.xacro文件,编写代码
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="box_inertia" params="m w h d">
<inertial>
<mass value="${m}" />
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertia" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy="0" ixz="0" iyy="${(m/12) * (3*r*r + h*h)}" iyz="0" izz="${(m/2) * (r*r)}" />
</inertial>
</xacro:macro>
<xacro:macro name="sphere_inertia" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}" />
</inertial>
</xacro:macro>
</robot>
🔄 作用: 依次定义了长方体、圆柱体和球体相关的宏,在宏中,使用标签inertial描述机器人的惯量,其中子标签mass用于描述质量,子标签inertia用于描述惯量。编写完成后,我们就可以在其他Xacro文件导入并使用该宏了
创建机器人传感器部件：
（1）在fishbot 目录下新建sensor子目录,然后在sensor下新建imu.urdf.xacro,编写代码
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
<xacro:macro name="imu_xacro" params="xyz">
<link name="imu_link">
<visual>
<origin xyz="0 0 00" rpy="0 0 0" />
<geometry>
<box size="0.02 0.02 0.02" />
</geometry>
<material name="black">
<color rgba="0 0 0 0.8" />
</material>
</visual>
<collision>
<origin xyz="0 0 00" rpy="0 0 0" />
<geometry>
<box size="0.02 0.02 0.02" />
</geometry>
<material name="black">
<color rgba="0 0 0 0.8" />
</material>
</collision>
<xacro:box_inertia m="0.01" w="0.02" d="0.02" h="0.02"/>
</link>
<joint name="imu_joint" type="fixed">
<parent link="base_link" />
<child link="imu_link" />
<origin xyz="${xyz}" />
</joint>
</xacro:macro>
</robot>
🔄 作用： 定义了一个imu_xacro的宏,在宏内定义了一个部件和关节,关节imu_joint将imu_link和base_link固定连接,调用box_inertia宏描述了该imu传感器的惯性与质量，固定的位置通过参数xyz进行传递。IMU（惯性测量单元）是一种用于测量物体加速度和角速度的传感器。
（2）在sensor目录下新建camera.urdf.xacro,然后编写代码
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
<!-- ============相机模块================ -->
<xacro:macro name="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>
🔄 作用:
深度相机是一种可以获取深度信息的特殊相机,往往会配合彩色相机使用,彩色相机可
以通过图像识别获取物体的像素坐标,结合深度就可以得到识别对象的三维坐标,机器人就
可以针对目标做出相应操作。
该宏代码包含了common_inertia.xacro宏代码，调用了box_inertia宏，来定义相机的质量与惯性。
代码中定义了一个名为camera_xacro的宏, 相机部件固定的位置通过参数xyz进行传递。
该宏中包含了两个连杆与一个关节，一个连杆是相机，一个连杆是虚拟部件（因为深度相机坐标系默认前方是z轴,所以我们先在URDF中添加一个虚拟部件并添加一个关节将其与相机链接，通过参数xyz修改关节的rpy，实现对相机关节的矫正来调整方位）
（3）在sensor目录下新建laser.urdf.xacro,然后编写代码
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
<xacro:macro name="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>
<collision>
<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>
</collision>
<xacro:cylinder_inertia m="0.01" 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>
<gazebo reference="laser_cylinder_link">
<material>Gazebo/Black</material>
</gazebo>
<!-- ============雷达================ -->
<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>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8" />
</material>
</collision>
<xacro:cylinder_inertia m="0.03" 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>
<gazebo reference="laser_link">
<material>Gazebo/Black</material>
</gazebo>
</xacro:macro>
</robot>
🔄 作用：
该宏代码包含了common_inertia.xacro宏代码，调用了box_inertia宏，来定义相机的质量与惯性。
上面的代码用于描述一个雷达传感器组件宏laser_xacro,在宏内定义了一个雷达支撑杆
和一个雷达,并将雷达固定在雷达支撑杆的顶端。
在URDF中,gazebo标签是比较特殊的,这一类标签是写给Gazebo看的,所以它们都是和Gazebo仿真相关的配置。
创建机器人身体部件与虚拟部件：
在chapt6_ws/src/fishbot_description/urdf/目录下新建fishbot目录,然后在该目录下新建
base.urdf.xacro,编写如代码
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
<xacro:macro name="base_xacro" params="length radius">
<link name="base_footprint" />
<link name="base_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0" />
<geometry>
<cylinder length="${length}" radius="${length}" />
</geometry>
<material name="blue">
<color rgba="0.1 0.1 1.0 0.5"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.0" rpy="0 0 0" />
<geometry>
<cylinder length="${length}" radius="${length}" />
</geometry>
<material name="blue">
<color rgba="0.1 0.1 1.0 0.5"/>
</material>
</collision>
<xacro:cylinder_inertia m="1.0" r="${radius}" h="${length}"/>
</link>
<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>
</xacro:macro>
</robot>
🔄 作用：
创建了身体组件，定义了一个名为base_xacro的宏,并将半径和长度设置成参数,方便调整。
这里首先声明了名称为base_footprint的空部件,然后将base_link固定在这个部件的上方,高度则设置为机器人身体高度的一半加上轮子的半径,再稍微减去1mm,让轮子可以贴紧地面。
添加雷达，相机，惯性测量传感器 gazebo插件传感器：
只有里程计传感器还不够,想要完成复杂的交互,还需各种传感器来感知环境,下一步我们来添加常用的传感器插件
在src/fishbot_description/urdf/fishbot/plugins/下新建gazebo_sensor_plugin.xacro,输入代码：
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--
这是一个Gazebo仿真环境中激光雷达(LiDAR)的配置文件
激光雷达通过发射激光束测量距离，常用于机器人导航、避障和建图
就像机器人的眼睛，能"看到"周围环境的距离信息
-->
<gazebo reference="laser_link">
<!--
定义一个激光雷达传感器：
- 这个传感器将附着在URDF文件中定义的"laser_link"连杆上
- laser_link通常代表激光雷达的物理安装位置
-->
<sensor type="ray" name="laser_sensor">
<!--
ROS插件配置：
- name="laserscan"：插件的名称，可以自定义
- filename="libgazebo_ros_ray_sensor.so"：要加载的ROS插件文件
这个文件是Gazebo与ROS通信的桥梁，负责将激光数据发布到ROS话题
-->
<plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
<!-- ROS相关设置 -->
<ros>
<!--
命名空间设置：
- <namespace>/</namespace> 表示使用根命名空间
- 如果设为<namespace>/my_robot</namespace>，则话题会发布在/my_robot下
-->
<namespace>/</namespace>
<!--
话题重映射：
- <remapping>~/out:=scan</remapping>
- 将插件默认输出话题"~out"重映射为标准的"scan"话题
- 在ROS中，激光数据通常发布在"/scan"话题
- 可以通过"rostopic echo /scan"查看数据
-->
<remapping>~/out:=scan</remapping>
</ros>
<!--
指定输出消息类型：
- <output_type>sensor_msgs/LaserScan</output_type>
- 这是ROS中标准的激光扫描消息类型
- 包含角度范围、距离数据等信息
-->
<output_type>sensor_msgs/LaserScan</output_type>
<!--
指定坐标系：
- <frame_name>laser_link</frame_name>
- 表示激光数据是相对于"laser_link"坐标系的
- 在TF变换中非常重要，告诉ROS数据来自哪个坐标系
-->
<frame_name>laser_link</frame_name>
</plugin>
<!-- 传感器基本参数 -->
<!--
传感器激活状态：
- <always_on>true</always_on>
- true：即使没有订阅者，传感器也会持续生成数据
- false：只有当有订阅者时才生成数据
- 激光雷达通常设为true，保证数据连续
-->
<always_on>true</always_on>
<!--
可视化设置：
- <visualize>true</visualize>
- true：在Gazebo仿真界面中显示激光射线（绿色线条）
- false：不显示激光射线（只生成数据，不显示）
- 调试时设为true可以看到激光如何扫描环境
-->
<visualize>true</visualize>
<!--
传感器更新频率：
- <update_rate>5</update_rate>
- 设置为5Hz，即每秒扫描5次
- 真实激光雷达通常10-20Hz，这里设得较低是为了节省计算资源
- 值越大越流畅，但对电脑要求越高
-->
<update_rate>5</update_rate>
<!--
传感器位姿：
- <pose>0 0 0 0 0 0</pose>
- 表示传感器相对于"laser_link"的位姿（无偏移）
- 格式：x y z roll pitch yaw（单位：米和弧度）
- 这里全为0，表示传感器就在laser_link坐标系原点，无旋转
-->
<pose>0 0 0 0 0 0</pose>
<!--
激光雷达核心参数（ray部分）：
- 模拟激光雷达如何发射和接收激光束
-->
<ray>
<!-- 扫描参数设置 -->
<scan>
<horizontal>
<!--
水平方向采样点数：
- <samples>360</samples>
- 表示一圈扫描360个点
- 360个点意味着每1度一个测量点（360度/360点）
- 值越大分辨率越高，但数据量越大
-->
<samples>360</samples>
<!--
水平角分辨率：
- <resolution>1.000000</resolution>
- 表示相邻两个测量点之间的角度间隔
- 1.0表示1弧度？不，这里有点误导
- 实际上，Gazebo中这个值通常设为1.0，真正的分辨率由samples和角度范围决定
-->
<resolution>1.000000</resolution>
<!--
最小扫描角度：
- <min_angle>0.000000</min_angle>
- 扫描起始角度，0弧度=0度
- 通常设为0，从正前方开始扫描
-->
<min_angle>0.000000</min_angle>
<!--
最大扫描角度：
- <max_angle>6.280000</max_angle>
- 扫描结束角度，6.28弧度≈360度（2π）
- 0到6.28弧度表示完整的360度扫描
-->
<max_angle>6.280000</max_angle>
</horizontal>
</scan>
<!-- 测距参数设置 -->
<range>
<!--
最小检测距离：
- <min>0.120000</min>
- 0.12米=12厘米
- 激光雷达无法测量太近的物体，有"盲区"
-->
<min>0.120000</min>
<!--
最大检测距离：
- <max>8.0</max>
- 8.0米
- 超过这个距离的物体将无法检测到
- 根据实际激光雷达性能设置（便宜的可能只有5-6米）
-->
<max>8.0</max>
<!--
距离分辨率：
- <resolution>0.015000</resolution>
- 0.015米=1.5厘米
- 表示能区分的最小距离差
- 值越小精度越高，但真实传感器通常在厘米级
-->
<resolution>0.015000</resolution>
</range>
<!-- 噪声模型设置（模拟真实传感器的误差） -->
<noise>
<!--
噪声类型：
- <type>gaussian</type>
- 使用高斯噪声（正态分布），最常用的噪声模型
-->
<type>gaussian</type>
<!--
噪声均值：
- <mean>0.0</mean>
- 0表示没有系统性偏差
- 如果设为0.01，表示测量值平均比真实值大1厘米
-->
<mean>0.0</mean>
<!--
噪声标准差：
- <stddev>0.01</stddev>
- 0.01米=1厘米
- 表示噪声的波动范围，值越大测量越不准确
- 真实激光雷达通常有1-2厘米的标准差
-->
<stddev>0.01</stddev>
</noise>
</ray>
</sensor>
</gazebo>
<!--
这是一个Gazebo仿真环境中IMU（惯性测量单元）传感器的配置文件
IMU用于测量机器人的角速度（陀螺仪）和线性加速度（加速度计）
通常用于机器人姿态估计、导航等任务
-->
<gazebo reference="imu_link">
<!--
定义一个IMU传感器：
- name="imu_sensor"：传感器的名称，可以自定义
- type="imu"：指定传感器类型为IMU
- 这个传感器将附着在URDF文件中定义的"imu_link"连杆上
-->
<sensor name="imu_sensor" type="imu">
<!--
ROS插件配置：
- name="imu_plugin"：插件的名称
- filename="libgazebo_ros_imu_sensor.so"：要加载的ROS插件文件
这个文件是Gazebo与ROS通信的桥梁，负责将传感器数据发布到ROS话题
-->
<plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
<!-- ROS相关配置 -->
<ros>
<!--
命名空间设置：
- <namespace>/</namespace> 表示使用根命名空间
- 如果设为<namespace>/my_robot</namespace>，则话题会发布在/my_robot下
-->
<namespace>/</namespace>
<!--
话题重映射：
- <remapping>~/out:=imu</remapping>
- 将插件默认输出话题"~out"（当前节点的私有话题）重映射为"/imu"
- 这样在ROS中可以通过"/imu"话题获取IMU数据
-->
<remapping>~/out:=imu</remapping>
</ros>
<!--
初始方向参考设置：
- <initial_orientation_as_reference>false</initial_orientation_as_reference>
- false：使用绝对方向参考（相对于世界坐标系）
- true：将启动时的方向作为参考零点（之后的测量都是相对于这个初始方向）
- 通常设为false，除非你有特殊需求
-->
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<!--
传感器更新频率：
- <update_rate>100</update_rate>
- 设置为100Hz，即每秒发布100次数据
- 根据实际IMU硬件性能设置，常见值有100Hz、200Hz等
-->
<update_rate>100</update_rate>
<!--
传感器激活状态：
- <always_on>true</always_on>
- true：即使没有订阅者，传感器也会持续生成数据
- false：只有当有订阅者时才生成数据
- 通常设为true以保证数据连续性
-->
<always_on>true</always_on>
<!--
IMU噪声模型配置：
- 真实的IMU传感器都有噪声和偏差，这里模拟这些特性
- 包含角速度（陀螺仪）和线性加速度（加速度计）两部分
-->
<imu>
<!-- 角速度（陀螺仪）噪声设置 -->
<angular_velocity>
<!-- X轴角速度噪声 -->
<x>
<noise type="gaussian">
<!--
高斯噪声参数：
- mean：噪声均值，0表示无系统性偏差
- stddev：标准差，值越大噪声越大（2e-4 = 0.0002 rad/s）
- bias_mean：偏置均值，模拟传感器固定偏差（0.0000075 rad/s）
- bias_stddev：偏置标准差，模拟偏置的随机变化（0.0000008 rad/s）
-->
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<!-- Y轴角速度噪声（参数与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轴角速度噪声（参数与X轴相同） -->
<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轴加速度噪声 -->
<x>
<noise type="gaussian">
<!--
注意：加速度计的噪声参数通常比陀螺仪大
- stddev：1.7e-2 = 0.017 m/s²
- bias_mean：0.1 m/s²（较大的固定偏差）
- bias_stddev：0.001 m/s²
-->
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<!-- Y轴加速度噪声（参数与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轴加速度噪声（参数与X轴相同） -->
<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>
<!--
这是一个Gazebo仿真环境中深度相机的配置文件
深度相机可以同时获取彩色图像和深度信息（每个像素的距离）
常用于机器人视觉、SLAM、物体识别等任务
注意：虽然type="depth"，但这里实际配置的是RGBD相机（彩色+深度）
-->
<gazebo reference="camera_link">
<!--
定义一个深度相机传感器：
- type="depth"：指定传感器类型为深度相机
- name="camera_sensor"：传感器的名称，可以自定义
- 这个传感器将附着在URDF文件中定义的"camera_link"连杆上
- 注意：在Gazebo中，type="depth"实际上会创建一个RGBD相机
-->
<sensor type="depth" name="camera_sensor">
<!--
ROS插件配置：
- name="depth_camera"：插件的名称，可以自定义
- filename="libgazebo_ros_camera.so"：要加载的ROS插件文件
这个文件是Gazebo与ROS通信的桥梁，负责将相机数据发布到ROS话题
注意：虽然文件名是"camera"而不是"depth_camera"，但它同时支持深度相机
深度信息会发布在/camera/depth/image_raw话题，彩色图像在/camera/image_raw
-->
<plugin name="depth_camera" filename="libgazebo_ros_camera.so">
<!--
相机光学坐标系：
- <frame_name>camera_optical_link</frame_name>
- 指定相机的光学坐标系名称
- 光学坐标系是计算机视觉中使用的标准坐标系：
* +X 向右
* +Y 向下
* +Z 向前（指向场景）
- 这与机器人常用的坐标系（+Z向上）不同，需要转换
-->
<frame_name>camera_optical_link</frame_name>
</plugin>
<!--
传感器激活状态：
- <always_on>true</always_on>
- true：即使没有订阅者，相机也会持续生成数据
- false：只有当有订阅者时才生成数据
- 深度相机数据量大，有时会设为false节省资源，但通常设为true
-->
<always_on>true</always_on>
<!--
传感器更新频率：
- <update_rate>10</update_rate>
- 设置为10Hz，即每秒发布10帧图像
- 深度相机数据量大，频率通常比IMU低（IMU可能100Hz，相机10-30Hz）
- 根据计算资源和应用需求调整
-->
<update_rate>10</update_rate>
<!--
相机参数配置：
- name="camera"：相机的内部名称，一般不需要修改
-->
<camera name="camera">
<!--
水平视场角：
- <horizontal_fov>1.5009831567</horizontal_fov>
- 单位是弧度(radians)，1.5弧度约等于86度
- 视场角越大，看到的范围越广，但远处物体越小
- 常见值：60-90度（约1.0-1.57弧度）
- 计算公式：弧度 = 角度 × π/180
-->
<horizontal_fov>1.5009831567</horizontal_fov>
<!--
图像参数配置：
- 定义输出图像的尺寸和格式
-->
<image>
<!--
图像宽度：
- <width>800</width>
- 设置图像宽度为800像素
- 分辨率越高，细节越多，但数据量越大
-->
<width>800</width>
<!--
图像高度：
- <height>600</height>
- 设置图像高度为600像素
- 常见分辨率：640x480, 800x600, 1280x720等
-->
<height>600</height>
<!--
图像格式：
- <format>R8G8B8</format>
- 表示每个像素由8位红色、8位绿色、8位蓝色组成（24位真彩色）
- 其他常见格式：B8G8R8（OpenCV常用顺序）、L8（灰度图）等
-->
<format>R8G8B8</format>
</image>
<!--
镜头畸变参数：
- 真实相机镜头会有畸变，这里可以模拟或校正
- 这里全部设为0，表示无畸变（理想相机）
-->
<distortion>
<!--
径向畸变系数：
- k1, k2, k3：模拟镜头的桶形或枕形畸变
- 0表示无畸变
- 真实相机可能有非零值（如k1=0.1, k2=-0.01等）
-->
<k1>0.0</k1>
<k2>0.0</k2>
<k3>0.0</k3>
<!--
切向畸变系数：
- p1, p2：模拟镜头与图像平面不平行导致的畸变
- 0表示无畸变
-->
<p1>0.0</p1>
<p2>0.0</p2>
<!--
畸变中心：
- <center>0.5 0.5</center>
- 表示畸变中心在图像中心（归一化坐标）
- 0.5 0.5 = (width/2, height/2)
- 如果设为0.4 0.6，则畸变中心在图像左下方
-->
<center>0.5 0.5</center>
</distortion>
</camera>
</sensor>
</gazebo>
</xacro:macro>
</robot>
🔧 雷达插件工作原理
你可以把这个插件想象成一个非常精密的激光手电筒，它的工作流程是这样的：
-
发射激光：插件模拟激光雷达，从
laser_link的位置向周围环境（在你的配置中是水平360度）发射出无数条看不见的激光束。 -
测量距离：Gazebo的物理引擎会计算每一条激光束是否打到了障碍物，以及从发射到返回用了多少时间，从而精确计算出机器人到障碍物的距离。
-
处理数据：将测量到的距离信息，结合你设置的噪声模型，处理成更真实的数据。
-
发布信息：最后，插件将所有数据打包成ROS标准格式的
LaserScan消息，并通过/scan这个话题发布出去。这样，其他程序（比如导航或SLAM建图算法）就能订阅这个话题，知道机器人周围的环境是什么样的了。
🔧IMU 插件工作原理
IMU就是我们手机里的陀螺仪+加速度计的组合，能感知旋转和加速度
你可以把IMU想象成机器人的“耳朵”，它的工作流程非常直接：
-
感知运动：插件实时监测
imu_link这个连杆在Gazebo仿真世界中的运动变化。 -
测量数据：
-
陀螺仪（
angular_velocity）：测量机器人绕X、Y、Z三个轴旋转的角速度（单位通常是弧度/秒）。比如，机器人左转时，Z轴的角速度值就会变化。 -
加速度计（
linear_acceleration）：测量机器人沿X、Y、Z三个轴方向的线性加速度（单位通常是米/秒²）。需要注意的是，它永远会测到重力加速度（约9.8 m/s²），当机器人加速前进时，X轴的加速度值会叠加变化。
-
-
添加真实感：根据你设置的噪声参数，在完美的理论数据上叠加各种误差，使数据更接近真实IMU的输出。
-
发布信息：最后，插件将所有数据打包成ROS标准格式的
Imu消息，并通过/imu这个话题发布出去。这样，其他程序（比如用于稳定控制的算法或姿态解算器）就能订阅这个话题，知道机器人自身的运动状态了。
🔧 相机插件如何工作
这双“智能眼睛”的工作原理可以概括为以下几步：
-
感知环境：插件模拟深度相机，在Gazebo仿真环境中，从
camera_link的位置和朝向“观察”世界。 -
生成图像：
-
RGB图像：生成普通的彩色图片，就像手机拍照一样，发布到类似
/camera/image_raw的话题。 -
深度图像：这是关键！对于视野内的每个物体，计算其与相机的距离，生成一张“距离图”（深度图）。这张图上每个像素的值不是颜色，而是距离（单位通常是米），并发布到类似
/camera/depth/image_raw的话题。 -
点云数据：有时插件还会将深度信息转换成三维点云（PointCloud2），发布到类似
/camera/depth/points的话题，直观显示物体的三维轮廓。
-
-
发布信息：插件将上述数据打包成ROS标准消息，通过相应的话题发布出去。这样，其他程序（如视觉SLAM、物体识别、导航避障算法）就能订阅这些话题，让机器人“看懂”周围环境了。
创建机器人执行（运动）部件：
只有传感器没有执行器,机器人不会动可不行,接下来我们给机器人添加执行器。执行器由驱动轮和万向轮组成。
（1）在actuator中新建caster.urdf.xacro文件,然后编写代码
1<?xml version="1.0"?>
2<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3 <!-- 包含惯性计算宏文件 -->
4 <!-- 这个文件提供了计算球体惯性矩阵的宏，用于简化物理计算 -->
5 <xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
6
7 <!-- 定义一个名为caster_xacro的Xacro宏，用于创建万向轮（Caster Wheel） -->
8 <!-- 参数说明： -->
9 <!-- caster_name: 万向轮的名称（例如：front_caster） -->
10 <!-- xyz: 万向轮在机器人底盘（base_link）上的位置坐标(x y z) -->
11 <xacro:macro name="caster_xacro" params="caster_name xyz">
12
13 <!-- 1. 创建万向轮的物理链接（轮子本身） -->
14 <link name="${caster_name}_caster_link">
15 <!-- 1.1 视觉模型（用于仿真中的显示） -->
16 <visual>
17 <!-- 位置和朝向（原点在轮子中心） -->
18 <origin xyz="0 0 0" rpy="0 0 0" />
19 <!-- 几何形状：球体 -->
20 <geometry>
21 <sphere radius="0.016" /> <!-- 球体半径：1.6厘米 -->
22 </geometry>
23 <!-- 材质：黑色，透明度80% -->
24 <material name="black">
25 <color rgba="0.0 0.0 0.0 0.8"/>
26 </material>
27 </visual>
28
29 <!-- 1.2 碰撞模型（用于物理仿真中的碰撞检测） -->
30 <collision>
31 <origin xyz="0 0 0" rpy="0 0 0" />
32 <geometry>
33 <sphere radius="0.016" />
34 </geometry>
35 <material name="black">
36 <color rgba="0.0 0.0 0.0 0.8"/>
37 </material>
38 </collision>
39
40 <!-- 1.3 惯性计算（用于物理仿真中的质量分布） -->
41 <!-- 调用包含的宏：计算球体的惯性矩阵 -->
42 <xacro:sphere_inertia m="0.01" r="0.016" />
43 <!-- 参数说明： -->
44 <!-- m="0.01": 质量 0.01kg (10克) -->
45 <!-- r="0.016": 半径 0.016m (1.6厘米) -->
46 </link>
47
48 <!-- 2. 为万向轮添加Gazebo物理属性 -->
49 <gazebo reference="${caster_name}_caster_link">
50 <!-- 摩擦系数：设置为0.0，表示无摩擦 -->
51 <!-- 注意：万向轮通常需要低摩擦以实现自由旋转 -->
52 <mu1 value="0.0" />
53 <mu2 value="0.0" />
54
55 <!-- 弹簧常数：非常大值(1e9)，表示刚性约束 -->
56 <!-- 确保轮子不会穿透地面 -->
57 <kp value="1000000000.0" />
58
59 <!-- 阻尼系数：1.0，用于控制旋转阻尼 -->
60 <kd value="1.0" />
61 </gazebo>
62
63 <!-- 3. 创建固定关节，将万向轮连接到机器人底盘 -->
64 <joint name="${caster_name}_caster_joint" type="fixed">
65 <!-- 父链接：机器人底盘 -->
66 <parent link="base_link" />
67 <!-- 子链接：万向轮 -->
68 <child link="${caster_name}_caster_link" />
69 <!-- 万向轮在底盘上的位置 -->
70 <origin xyz="${xyz}" />
71 <!-- 关节轴：固定关节，没有旋转自由度 -->
72 <axis xyz="0 0 0" />
73 </joint>
74
75 </xacro:macro>
76</robot>
🔄 作用： 万向轮的几何形状采用的是圆球形状,半径为0.016,关节部分采用的是固定的类型
（2）在fishbot目录下新建actuator子目录,然后在actuator下新建wheel.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
<xacro:macro name="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>
<collision>
<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>
</collision>
<xacro:cylinder_inertia m="0.1" h="0.04" r="0.032"/>
</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>
🔄 作用：
完成驱动轮的定义,声明了名称为wheel_xacro的宏,参数是轮子名称和固定位置。
因为默认轮子是躺平状态,所以这里调整了轮子部件中rpy的值,将r部分调整成了1.57079rad,即90°,将轮子竖起来。
除此之外,在关节定义部分,关节类型采用连续关节continuous,该类型关节可以绕着某个轴进行无限制的旋转。在关节标签中添加了axis子标签,用于表示旋转轴和方向,<axis xyz="010"/>则表示绕y轴正方向旋转。
建模于此结束，接下来准备让机器人动起来
创建机器人控制系统： 使用ros2_control接入Gazebo，控制机器人运动
其实想要控制机器人运动，有两种方式，一种是直接使用插件，一种是使用ros2control系统。第二种方法才是主流，所以本文将采取这种方法。
gazebo_ros2_control对硬件资源的描述使用的也是XML格式,所以我们可以将配置写到URDF中,在src/fishbot_description/urdf/fishbot/中新建 fishbot.ros2_control.xacro,在该文件中编写代码
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--
定义一个Xacro宏：
- name="fishbot_ros2_control"：宏的名称
- 宏就像一个代码模板，可以在其他URDF文件中被调用
-->
<xacro:macro name="fishbot_ros2_control">
<!--
ROS 2控制系统配置：
- name="FishBotGazeboSystem"：给这个控制系统起个名字
- type="system"：表示这是一个完整的系统控制系统，type也可以选取为传感器控制系统，或者执行器控制系统
- ROS 2 Control是ROS 2的机器人控制框架，负责硬件与软件之间的通信
-->
<ros2_control name="FishBotGazeboSystem" type="system">
<!--
硬件接口配置：
- 指定Gazebo仿真的硬件插件
- plugin>gazebo_ros2_control/GazeboSystem</plugin>
这是连接Gazebo仿真器和ROS 2 Control的桥梁
-->
<hardware>
<plugin>
gazebo_ros2_control/GazeboSystem
</plugin>
</hardware>
<!--
左轮关节配置：
- name="left_wheel_joint"：对应URDF中定义的左轮关节名称
- 这里定义了如何控制左轮以及获取其状态
-->
<joint name="left_wheel_joint">
<!--
速度命令接口：
- name="velocity"：表示可以通过发送速度命令来控制轮子
- min/max：速度范围限制
* -1：最大后退速度
* 1：最大前进速度
* 单位可能是m/s或rad/s，取决于具体实现
-->
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<!--
力矩命令接口：
- name="effort"：表示可以通过发送力矩命令来控制轮子
- min/max：力矩范围限制
* -0.1：最大反向力矩
* 0.1：最大正向力矩
* 单位可能是牛·米(Nm)
-->
<command_interface name="effort">
<param name="min">-0.1</param>
<param name="max">0.1</param>
</command_interface>
<!--
状态接口（获取关节信息）：
- position：可以获取轮子的当前位置（角度）
- velocity：可以获取轮子的当前速度
- effort：可以获取轮子的当前力矩
- 这些接口没有参数，只是声明可以获取这些状态
-->
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<!--
右轮关节配置：
- 与左轮配置完全相同
- name="right_wheel_joint"：对应URDF中定义的右轮关节名称
- 差速驱动机器人需要独立控制左右两个轮子
-->
<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仿真插件配置：
- 这部分专门用于Gazebo仿真器
- 连接ROS 2 Control与Gazebo仿真环境
-->
<gazebo>
<!--
加载Gazebo ROS 2控制插件：
- filename="libgazebo_ros2_control.so"：插件的共享库文件
- name="gazebo_ros2_control"：给插件起个名字
-->
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<!--
控制器参数文件：
- <parameters>$(find fishbot_description)/config/fishbot_ros2_controller.yaml</parameters>
- 指定ROS 2控制器的配置文件路径
- $(find fishbot_description)：ROS查找包路径的命令
- 这个YAML文件包含更详细的控制器参数，如PID增益、发布频率等
-->
<parameters>$(find fishbot_description)/config/fishbot_ros2_controller.yaml</parameters>
<!--
ROS话题重映射：
- 将控制器内部话题重映射到标准话题名称
- 这样其他ROS节点可以使用标准话题名称与机器人交互
-->
<ros>
<!--
速度命令重映射：
- /fishbot_diff_drive_controller/cmd_vel_unstamped:=/cmd_vel
- 将控制器的内部速度命令话题重映射到标准的/cmd_vel话题
- 这样你只需发布到/cmd_vel话题就能控制机器人移动
- 例如：ros2 topic pub /cmd_vel geometry_msgs/Twist "{linear: {x: 0.2}, angular: {z: 0.5}}"
-->
<remapping>/fishbot_diff_drive_controller/cmd_vel_unstamped:=/cmd_vel</remapping>
<!--
里程计重映射：
- /fishbot_diff_drive_controller/odom:=/odom
- 将控制器生成的里程计数据发布到标准的/odom话题
- /odom是ROS中标准的里程计话题名称
- 导航、SLAM等算法会订阅这个话题获取机器人位置
-->
<remapping>/fishbot_diff_drive_controller/odom:=/odom</remapping>
</ros>
</plugin>
</gazebo>
</xacro:macro>
</robot>
1. 这是什么？
- 这是一个机器人控制系统配置文件，专门用于FishBot（一种差速驱动机器人）
- 它连接了Gazebo仿真器和ROS 2控制系统，让你能在Gazebo仿真器中使用ROS 2控制系统控制FishBot。定义了控制系统与FishBot的轮子交互（通信）的具体方式
2. 核心概念：ROS 2 Control
- ROS 2 Control 是ROS 2的机器人控制框架
- 就像机器人的"神经系统"，负责：
- 接收cmd的控制命令（如"前进0.5m/s"）
- 翻译命令，发送命令到硬件（或gazebo中的虚拟硬件）
- 获取传感器反馈（如轮子转速）
3. 两个关键部分
A. ros2_control部分（定义接口）
-
命令接口：你发送给机器人的指令
velocity：速度控制（-1到1，负值表示后退）effort：力矩控制（-0.1到0.1，控制电机力量）
-
状态接口：机器人反馈给你的信息
position：轮子转了多少角度velocity：轮子当前转速effort：轮子当前受力情况
想象一下汽车：
- 命令接口 = 你踩油门/刹车的力度
- 状态接口 = 速度表、转速表显示的信息
B. Gazebo插件部分（连接仿真）
-
参数文件：
fishbot_ros2_controller.yaml- 包含更详细的控制参数（如PID控制器的增益）
-
话题重映射：
-
/cmd_vel：标准的"命令速度"话题- 你发送Twist（扭转）消息到这里控制机器人
- 格式：
{linear: {x: 0.2}, angular: {z: 0.5}}（前进+左转）
-
/odom：标准的"里程计"话题- 包含机器人在环境中的位置和朝向
- 导航系统依赖这个数据
-
c. gazebo_ros2_control/GazeboSystem
想象你有一个机器人仿真世界(Gazebo) 和一个机器人控制中心(ROS 2 Control)，它们说不同的"语言"：
- Gazebo 说："我要模拟物理世界，知道每个关节的位置、速度和受力"
- ROS 2 Control 说："我想要控制机器人，需要发送命令并获取反馈"
gazebo_ros2_control/GazeboSystem 就是这两个系统之间的**专业翻译+通信中介**，让它们能够完美协作。
4. 实际工作流程
-
你发送命令：
ros2 topic pub /cmd_vel geometry_msgs/Twist "{linear: {x: 0.3}, angular: {z: 0.0}}" #将geometry_msgs/Twist格式消息linear: {x: 0.3}, angular: {z: 0.5}发送到cmd_vel话题频道（让机器人以0.3m/s前进，同时以0.5m/s速度左转）
-
ROS 2 Control接收命令：
- 通过重映射，命令到达
/cmd_vel话题 - 控制器计算左右轮应该以什么速度转动才能实现0.3m/s前进，同时以0.5m/s速度左转。并将命令发送给gazebo
- 通过重映射，命令到达
-
Gazebo仿真执行：
- 插件将速度命令发送给Gazebo中的虚拟电机
- Gazebo模拟轮子转动，推动机器人在虚拟环境中移动
-
反馈循环：
- Gazebo计算轮子的实际位置和速度
- 通过状态接口反馈给ROS 2 Control
- 生成里程计数据发布到
/odom话题
Q：如果我想让机器人跑得更快怎么办？
A：可以修改max值（如改为2），但也要相应调整控制器配置文件中的参数，否则可能导致不稳定。
在功能包的config目录下新建fishbot_ros2_controller.yaml,然后输入代码：
# 这是ROS 2控制器管理器的配置文件
# 控制器管理器就像机器人的"大脑中枢"，负责协调所有控制器
# 整个文件分为三大部分：全局设置、力矩控制器、差速驱动控制器
# ------------------------------
# 第一部分：控制器管理器全局配置
# ------------------------------
controller_manager:
ros__parameters:
# 控制循环更新频率：100 Hz（每秒执行100次控制计算）
# 值越大控制越精确，但对计算资源要求越高
update_rate: 100 # Hz
# 使用仿真时间标志，确保控制与仿真同步
# true：使用Gazebo提供的仿真时间（不是电脑系统时间）
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 # 左轮关节名称（必须与URDF中一致）
- right_wheel_joint # 右轮关节名称（必须与URDF中一致）
# 命令接口类型 - 我们要发送什么类型的控制命令，这里选择"effort"（力矩控制），即直接控制电机输出力矩
command_interfaces:
- effort # 也可以是"velocity"（速度控制）或"position"（位置控制）
# 状态接口类型 - 我们要获取什么类型的反馈，这里获取位置、速度和力矩三方面信息
state_interfaces:
- position # 关节当前位置（角度）
- velocity # 关节当前速度（角速度）
- effort # 关节当前受力（力矩）
# ------------------------------
# 第三部分：差速驱动控制器配置
# ------------------------------
fishbot_diff_drive_controller:
ros__parameters:
# 左轮关节名称列表，差速驱动机器人通常有1个或多个左轮，这里只有一个
left_wheel_names: ["left_wheel_joint"]
# 右轮关节名称列表，同样，这里只有一个右轮
right_wheel_names: ["right_wheel_joint"]
# 两轮之间的距离（米），从FishBot的URDF模型测量得到，这个值影响转弯半径，非常重要！
wheel_separation: 0.17 # 17厘米
# 轮子半径（米），从FishBot实际测量或URDF中获取，这个值影响速度计算，非常重要！
wheel_radius: 0.032 # 3.2厘米
# 轮间距乘数（通常为1.0，用于微调）
wheel_separation_multiplier: 1.0
# 左右轮半径乘数（通常为1.0，用于微调）
left_wheel_radius_multiplier: 1.0
right_wheel_radius_multiplier: 1.0
# 里程计发布频率：50 Hz，每秒发布50次机器人位置信息
publish_rate: 50.0
# 里程计坐标系名称 odom 是ROS中标准的里程计坐标系
odom_frame_id: odom
# 机器人基座坐标系名称 ， /base_footprint 通常指机器人底部中心点
base_frame_id: base_footprint
# 位置估计的不确定性（协方差对角线），值越小表示估计越准确， [x, y, z, roll, pitch, yaw]
pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
# 速度估计的不确定性（协方差对角线），[vx, vy, vz, wx, wy, wz]
twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]
# 开环控制模式
# true：不使用反馈校正（简单但可能有累积误差）
# false：使用反馈校正（更精确但需要更多传感器）
open_loop: true
# 是否发布odom到base_footprint的TF变换
# true：发布，这样其他节点可以看到机器人位置
enable_odom_tf: true
# 速度命令超时时间（秒），如果超过0.5秒没收到新命令，机器人会停止， 防止命令丢失导致机器人一直运动
cmd_vel_timeout: 0.5
# 是否使用带时间戳的速度命令
# false：使用当前时间（更简单）
# true：使用消息中的时间戳（更精确，但需要同步）
use_stamped_vel: false
# 注意：以下参数被注释掉了，表示不使用
# publish_limited_velocity: true # 发布限制后的速度
# velocity_rolling_window_size: 10 # 速度计算的滑动窗口大小
三个关键控制器
A. 关节状态广播器 (JointStateBroadcaster)
- 作用：从Gazebo仿真环境中获取关节状态（位置、速度等）
- 类比：就像汽车的转速表和里程表，实时显示车轮状态
- 重要性：（1）将这些状态转换为ROS 2标准的
sensor_msgs/msg/JointState格式发布到/joint_states话题，方便导航系统获取到这些数据知道机器人在哪。（2）与robot_state_publisher协作：robot_state_publisher订阅/joint_states话题，结合URDF模型计算机器人的坐标变换，发布到TF系统供其他节点使用。
fishbot_joint_state_broadcaster如何“看到”关节状态？第一步：Gazebo 仿真引擎驱动关节运动
- 当你在 Gazebo 中运行机器人时，Gazebo 物理引擎会根据力、速度等计算每个关节的 位置（position）、速度（velocity）、力矩（effort）
- 这些数据存在于 Gazebo 内部
第二步：
gazebo_ros2_control/GazeboSystem插件读取 Gazebo 状态
- 在你的 URDF/Xacro 中，有如下配置：
<hardware> <plugin>gazebo_ros2_control/GazeboSystem</plugin> </hardware>- 启动仿真时，Gazebo 会加载
libgazebo_ros2_control.so插件- 该插件会：
- 订阅 Gazebo 的关节状态（通过 Gazebo 的 C++ API）
- 将这些状态写入 ROS 2 Control 的
state_interface✅ 关键点：
GazeboSystem是唯一与 Gazebo 直接交互的部分。第三步：ROS 2 Control 框架维护一个“硬件抽象层”
- 在
ros2_control配置中，你为每个关节声明了状态接口：<joint name="left_wheel_joint"> <state_interface name="position"/> <state_interface name="velocity"/> <state_interface name="effort"/> </joint>GazeboSystem在每次仿真步（simulation step）中：
- 从 Gazebo 读取实际的
position,velocity,effort- 更新到对应的
state_interface中第四步：
joint_state_broadcaster从 ROS 2 Control 读取状态
joint_state_broadcaster是一个 只读控制器（read-only controller）- 它不发送命令，只读取已注册的
state_interface- 启动后，它会：
- 遍历所有在
ros2_control中声明的关节- 从
state_interface中提取position,velocity,effort- 打包成
sensor_msgs/msg/JointState消息- 发布到
/joint_states话题🔄 所以：
Gazebo →GazeboSystem→state_interface→joint_state_broadcaster→/joint_states
B. 力矩控制器 (EffortController)
- 作用：直接控制电机输出多大的力量
- 类比：就像直接控制汽车油门的深度（而不是控制速度）
- 使用场景：高级控制或需要精确力控制时使用
C. 差速驱动控制器 (DiffDriveController) - 最常用
- 作用：将"前进0.3m/s，左转0.5rad/s"转换为左右轮速度
- 类比：就像汽车的"驾驶系统"，你控制方向盘和油门，它自动分配左右轮动力
- 为什么重要：你通常只用这个控制器，发送
/cmd_vel命令即可
组合机器人各部件与控制功能：
将各个部件组合起来，在urdf/fishbot/下新建fishbot.urdf.xacro,然后编写如代码
作用： 通过xacro:include标签来包含其他的Xacro文件,通过该标签的filename属性可以指定要包含的Xacro文件的文件名称,需要注意的是,通过$(find fishbot_description)可以查找功能包的安装目录。关于传感器位置值的设定,文件中的位置可作为参考,你也可以根据显示效果进行调整。
创建launch文件，启动节点，运行仿真：
在launch文件夹创建gazebo_sim.launch.py文件如下，运行，在gazebo中观察与运行机器人
import launch # 导入ROS2的启动管理模块，用于创建和管理启动描述
import launch_ros # 导入ROS2的ROS特定启动管理模块
from ament_index_python.packages import get_package_share_directory # 用于查找ROS包的共享目录路径
from launch.launch_description_sources import PythonLaunchDescriptionSource # 用于指定包含的启动文件是Python格式
def generate_launch_description(): # 这是ROS2启动文件的必需函数，ROS2会自动调用这个函数来生成启动描述
# 1. 设置机器人相关参数变量
robot_name_in_model = "fishbot" # 机器人在Gazebo中的名称（就像给机器人起个名字）
urdf_tutorial_path = get_package_share_directory('fishbot_description') # 获取fishbot_description包的共享目录路径
default_model_path = urdf_tutorial_path + '/urdf/fishbot/fishbot.urdf.xacro' # 机器人URDF模型的默认路径（xacro是URDF的扩展格式）
default_world_path = urdf_tutorial_path + '/world/custom_room.world' # Gazebo世界文件的默认路径（定义了Gazebo中的环境）
# 2. 声明启动参数（允许用户在启动时修改模型路径）
#等价于ros2 launch fishbot_description fishbot.launch.py model:=/path/to/your/model.urdf.xacro
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
name='model', # 参数名称，用户可以通过ros2 launch命令指定这个参数
default_value=str(default_model_path), # 默认值，使用上面定义的默认路径
description='URDF的绝对路径' # 参数说明，告诉用户这个参数是做什么的
)
# 3. 处理URDF文件（将xacro文件转换为URDF格式）
# ROS2的robot_state_publisher节点只能处理标准的URDF格式，而不能直接处理Xacro文件。所以我们需要在启动过程中将Xacro文件转换为URDF。
#substitutions意思是替代，将xacro文件替换成urdf文件
#LaunchConfiguration('model')作用是：获取之前通过DeclareLaunchArgument声明的参数model的值。，返回用户指定的路径/path/to/custom_model.urdf.xacro
#等价于 xacro /urdf/fishbot/fishbot.urdf.xacro
robot_description = launch_ros.parameter_descriptions.ParameterValue(
launch.substitutions.Command( # 用于执行命令的替代方式
['xacro ', launch.substitutions.LaunchConfiguration('model')] # 构建xacro命令，'model'是上面声明的参数
),
value_type=str # 转换后的URDF内容被作为字符串传递给robot_state_publisher节点，robot_state_publisher节点使用这个URDF来发布TF变换和关节状态
)
# 4. 创建机器人状态发布节点（发布机器人的TF变换和关节状态）
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher', # 节点所属的包
executable='robot_state_publisher', # 节点的可执行文件
parameters=[{'robot_description': robot_description}] # 传递参数，将处理后的URDF模型作为robot_description参数
)
# 5. 启动Gazebo仿真环境
# 等价于ros2 launch gazebo_ros gazebo.launch.py world:=/path/to/world.world verbose:=true
launch_gazebo = launch.actions.IncludeLaunchDescription(
PythonLaunchDescriptionSource( # 指定包含的启动文件是Python格式
[get_package_share_directory('gazebo_ros'), '/launch', '/gazebo.launch.py'] # 构建gazebo.launch.py的路径
),
# 传递参数给gazebo.launch.py
launch_arguments=[('world', default_world_path), ('verbose', 'true')] # 设置Gazebo的世界文件和详细输出
)
# 6. 在Gazebo中加载机器人
#等价于ros2 run gazebo_ros spawn_entity.py -topic /robot_description -entity fishbot
spawn_entity_node = launch_ros.actions.Node(
package='gazebo_ros', # 包名
executable='spawn_entity.py', # 可执行文件
arguments=['-topic', '/robot_description', # 从哪个topic获取机器人描述
'-entity', robot_name_in_model] # 机器人在Gazebo中的名称
)
# 7. 加载并激活关节状态广播控制器（用于发布关节状态）
#等价于ros2 control load_controller --set-state active fishbot_joint_state_broadcaster
load_joint_state_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'fishbot_joint_state_broadcaster'], # 执行ros2控制命令
output='screen' # 将输出打印到屏幕
)
# 8. 加载并激活努力控制器（用于控制机器人关节力矩）
load_fishbot_effort_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'fishbot_effort_controller'],
output='screen' #verbose参数：设置为true，启用详细输出
)
# 9. 加载并激活差速驱动控制器（用于控制机器人的运动）
# 等价于ros2 control load_controller --set-state active fishbot_effort_controller
load_fishbot_diff_drive_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'fishbot_diff_drive_controller'], #加载后立即激活控制器
output='screen'
)
# 10. 返回启动描述（定义启动顺序和事件）
#这段代码定义了整个启动流程的执行顺序，确保机器人在Gazebo中正确加载后，再按顺序加载必要的控制器。
return launch.LaunchDescription([
action_declare_arg_mode_path, # 声明模型路径参数（如：/path/to/model.urdf.xacro）
robot_state_publisher_node, # 发布机器人模型和TF变换的节点（必须最先启动）
launch_gazebo, # 启动Gazebo仿真环境（需要机器人模型才能加载机器人）
spawn_entity_node, # 在Gazebo中加载机器人实体（需要Gazebo已启动）
# 事件处理器1：当机器人实体加载完成（spawn_entity_node结束）后
# 执行加载关节状态控制器
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=spawn_entity_node, # 监听的目标动作：机器人实体加载
on_exit=[load_joint_state_controller], # 当目标动作结束后执行的动作
)
),
# 事件处理器2：当关节状态控制器加载完成（load_joint_state_controller结束）后
# 执行加载差速驱动控制器
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=load_joint_state_controller, # 监听的目标动作：关节状态控制器加载
on_exit=[load_fishbot_diff_drive_controller], # 当目标动作结束后执行的动作
)
),
# 事件处理器3：当差速驱动控制器加载完成（load_fishbot_diff_drive_controller结束）后
# 执行加载努力控制器
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=load_fishbot_diff_drive_controller, # 监听的目标动作：差速驱动控制器加载
on_exit=[load_fishbot_effort_controller], # 当目标动作结束后执行的动作
)
)
])
launch.actions.ExecuteProcess：这是ROS2中用于执行外部命令的类，cmd与output是他的属性，
package、executable和arguments确实是launch_ros.actions.Node类的属性。
在ROS2中，robot_state_publisher节点会发布/robot_description话题，包含机器人的URDF模型。spawn_entity.py需要从这个话题获取模型信息，然后在Gazebo中创建机器人实体。
问题：
# 4. 创建机器人状态发布节点（发布机器人的TF变换和关节状态）
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher', # 节点所属的包
executable='robot_state_publisher', # 节点的可执行文件
parameters=[{'robot_description': robot_description}] # 传递参数，将处理后的URDF模型作为robot_description参数
)是如何接收到urdf对应的字符串文件的？
答案：
1. 启动文件系统的详细工作流程
启动文件系统不是直接传递"robot_description"这个变量，而是：
看到 parameters=[{'robot_description': robot_description}]
知道 robot_description 是一个 ParameterValue 对象
执行 ParameterValue 对象中存储的命令：
xacro /home/user/robot_description/urdf/fishbot/fishbot.urdf.xacro
获取命令的输出（URDF字符串）
设置 robot_description 参数的值为这个URDF字符串
2. 机器人组装机（robot_state_publisher）的工作
启动时，从ROS参数服务器请求 robot_description 参数
收到参数的值（URDF字符串）
使用这个URDF字符串来发布TF变换和关节状态
5. 实际工作流程
步骤1：机器人启动
机器人模型（URDF）和TF变换已经发布（通过robot_state_publisher）
Gazebo仿真环境已经启动（通过launch_gazebo）
步骤2：加载并激活控制器
ros2 control load_controller --set-state active fishbot_effort_controller
控制器管理器（controller_manager）收到指令
加载名为fishbot_effort_controller的控制器
立即激活该控制器
步骤3：控制器开始工作
控制器开始接收关节力矩指令
机器人关节开始根据指令移动
ROS2控制器如何接收指令及开始工作
控制器接收指令的来源
在ROS2中，控制器（如努力控制器）接收指令的来源是ROS2话题（Topic）。具体来说：
努力控制器（Effort Controller） 通常订阅一个特定的话题，例如：
/fishbot_effort_controller/command（控制器名称+/command）
或者更通用的 /command（取决于控制器的配置）
话题类型：std_msgs/msg/Float64MultiArray，其中包含每个关节的目标力矩值
控制器开始工作的完整流程
1. 控制器加载与激活
load_fishbot_effort_controller = launch.actions.ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'fishbot_effort_controller'],
output='screen'
)
这个命令会加载并激活fishbot_effort_controller
2. 控制器开始监听话题
当控制器被激活后，它会：
向ROS2系统注册自己
开始监听其配置的ROS话题（例如/fishbot_effort_controller/command）
准备接收力矩指令
3. 指令发送（关键步骤）
控制器开始工作的关键在于有节点向其订阅的话题发布指令。例如：
# 发送关节力矩指令到控制器
ros2 topic pub /fishbot_effort_controller/command std_msgs/msg/Float64MultiArray "{data: [1.0, 1.0, -1.0, -1.0]}"
这个命令会向控制器发送力矩指令，其中[1.0, 1.0, -1.0, -1.0]表示每个关节的目标力矩值。
通俗类比
想象一下你有一个遥控机器人手臂：
控制器 = 机器人手臂的"控制板"
它需要被"打开"（激活）
打开后，它会"等待"你的遥控信号
话题 = 遥控信号的传输通道
例如：/fishbot_effort_controller/command
就像无线电频率，控制板只监听这个频率
指令 = 你的遥控信号
例如：[1.0, 1.0, -1.0, -1.0] 表示"让关节1和2向前转，关节3和4向后转"
就像你按遥控器上的按钮
工作流程：
你先"打开"控制板（加载并激活控制器）
然后你"按下按钮"（发布力矩指令）
控制板"接收信号"并"移动关节"
核心语法结构
python
编辑
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=spawn_entity_node,
on_exit=[load_joint_state_controller],
)
)
逐层解析
1. RegisterEventHandler - 这是ROS2启动文件中用于注册事件处理器的类。它的作用是告诉启动系统："当特定事件发生时，请执行指定的动作"。
参数：event_handler=... - 指定要注册的事件处理器
返回：注册了事件处理器的启动描述
2. OnProcessExit - 事件处理器类型，这是ROS2中的一种事件处理器，表示"当某个进程退出时触发"。
参数：
target_action=...：指定要监听的目标动作
on_exit=...：指定当目标动作退出时要执行的动作
spawn_entity.py的执行顺序非常重要：
首先需要robot_state_publisher发布/robot_description话题
然后spawn_entity.py才能从这个话题获取模型信息
最后在Gazebo中加载机器人
在启动文件中，通过事件处理（RegisterEventHandler）确保了正确的执行顺序：
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=spawn_entity_node,
on_exit=[load_joint_state_controller],
)
)
这确保了在机器人加载完成后（spawn_entity_node执行完毕）才加载关节状态控制器。
在launch文件夹创建display_robot.launch.py文件如下，在rviz中查看一下机器人建模
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(
['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
])
配置“package.xml”和“CMakeLists.txt”
略
编译，配置环境变量
运行代码进行仿真：
ros2 launch fishbot_description gazebo_sim.launch.py
ros2 topic pub /cmd_vel geometry_msgs/Twist "{linear: {x: 0.3}, angular: {z: 0.0}}" #差速控制
ros2 topic pub /fishbot_effort_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.0001, 0.0001]}" #力控，记得把差速控诉取消激活先
启动gazebo后，打开新终端，在rviz中打开机器中模型进行观察：
ros2 launch fishbot_description display_robot.launch.py model:=/home/crj/ros2bookcode/chapt6/chapt6_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro #把模型路径更换为你自己的
在rviz中显示雷达点云与相机点云：
整体架构回顾
在 ROS 2 + Gazebo 的仿真系统中，关键组a
- URDF/Xacro 文件：定义机器人模型（包括关节名称、类型等）
gazebo_ros2_control插件：Gazebo ↔ ROS 2 Control 的桥梁ros2_control系统配置（如你之前提到的第二段代码）：声明哪些关节、接口可用- Controller Manager（控制器管理器）：加载并运行控制器（如
joint_state_broadcaster） fishbot_joint_state_broadcaster控制器：读取关节状态并发布到/joint_states
扫一扫
1130
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
