ROS2-机器人仿真
本节使用Gazebo进行机器人仿真
1. 为URDF模型添加物理属性
需要为之前创建的URDF模型中的link添加物理属性 碰撞、惯性和 摩擦力，碰撞描述是物体的用于碰撞检测的包围形状，惯性用于描述物体的质量，惯性矩阵。
碰撞检测
collision标签的子标签与visual标签相似，描述link的碰撞检测范围
若两个物体碰撞箱相交，则可认为它们发生了碰撞。
collision标签示例：
<collision>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.12" radius="0.10"/>
</geometry>
<material name="blue">
<color rgba="0.1 0.1 1.0 0.5" />
</material>
</collision>
这是一个圆柱形的碰撞箱
惯性
inertial标签的子标签：
- mass：描述link的质量
- inertia：描述link的旋转惯量（Ixx Ixy Ixz Iyy Iyz Izz）。惯性参数
inertial标签示例：
<inertial>
<mass value="0.2"/>
<inertia ixx="0.0122666" ixy="0" ixz="0" iyy="0.0122666" iyz="0" izz="0.02"/>
</inertial>
摩擦力
摩擦力描述了物体与地面接触时的摩擦特性，主要包括以下参数：
- mu1：静摩擦系数。
- mu2：动摩擦系数。
- kp：弹性系数，数值越大，接触越坚硬。
- kd：阻尼系数，描述物体与地面接触时的阻尼特性，数值越大，接触越有阻尼。
示例:
<gazebo reference="caster_link">
<mu1 value="0.0"/>
<mu2 value="0.0"/>
<kp value="1000000.0" />
<kd value="10.0" />
</gazebo>
caster_link是一个支撑轮，设置其摩擦力为0，模拟一个万向轮的效果，使机器人能够平稳转弯。
完整URDF
<?xml version="1.0"?>
<robot name="fishbot">
<link name="base_footprint"/>
<joint name="base_joint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0.0 0.0 0.076" rpy="0 0 0"/>
</joint>
<!-- 躯干 -->
<link name="base_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.12" radius="0.10"/>
</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="0.12" radius="0.10"/>
</geometry>
<material name="blue">
<color rgba="0.1 0.1 1.0 0.5" />
</material>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.0122666" ixy="0" ixz="0" iyy="0.0122666" iyz="0" izz="0.02"/>
</inertial>
</link>
<!-- 雷达 -->
<link name="laser_link">
<visual>
<geometry>
<cylinder length="0.01" radius="0.01"/>
</geometry>
</visual>
</link>
<joint name="laser_joint" type="fixed">
<parent link="base_link" />
<child link="laser_link" />
<origin xyz="0 0 0.075" />
</joint>
<!-- IMU -->
<link name="imu_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="base_link" />
<child link="imu_link" />
<origin xyz="0 0 0.02" />
</joint>
<!-- 左轮 -->
<link name="left_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.5" />
</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.5" />
</material>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>
</link>
<!-- 右轮 -->
<link name="right_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.5" />
</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.5" />
</material>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>
</link>
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link" />
<child link="left_wheel_link" />
<origin xyz="-0.02 0.10 -0.06" />
<axis xyz="0 1 0" />
</joint>
<joint name="right_wheel_joint" type="continuous">
<parent link="base_link" />
<child link="right_wheel_link" />
<origin xyz="-0.02 -0.10 -0.06" />
<axis xyz="0 1 0" />
</joint>
<!-- 支撑轮 -->
<link name="caster_link">
<visual>
<origin xyz="0 0 0" rpy="1.57079 0 0"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.57079 0 0"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>
</link>
<joint name="caster_joint" type="fixed">
<parent link="base_link" />
<child link="caster_link" />
<origin xyz="0.06 0.0 -0.076" />
<axis xyz="0 1 0" />
</joint>
<!-- 支撑轮摩擦力，这里用0摩擦力模拟 -->
<gazebo reference="caster_link">
<material>Gazebo/Black</material>
<mu1 value="0.0"/>
<mu2 value="0.0"/>
<kp value="1000000.0" />
<kd value="10.0" />
</gazebo>
<gazebo reference="laser_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo>
<!-- 两轮差速插件 -->
<plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.2</wheel_separation>
<wheel_radius>0.032</wheel_radius>
<topic>/cmd_vel</topic>
<odom_topic>/odom</odom_topic>
<frame_id>odom</frame_id>
<child_frame_id>base_footprint</child_frame_id>
</plugin>
<!-- 关节状态发布 -->
<plugin filename="gz-sim-joint-state-publisher-system" name="gz::sim::systems::JointStatePublisher">
<update_rate>30</update_rate>
<joint_name>right_wheel_joint</joint_name>
<joint_name>left_wheel_joint</joint_name>
</plugin>
</gazebo>
<!-- IMU传感器 -->
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<gz_frame_id>imu_link</gz_frame_id>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"></plugin>
<imu>
<angular_velocity>
<x><noise type="gaussian"><mean>0.0</mean><stddev>2e-4</stddev></noise></x>
<y><noise type="gaussian"><mean>0.0</mean><stddev>2e-4</stddev></noise></y>
<z><noise type="gaussian"><mean>0.0</mean><stddev>2e-4</stddev></noise></z>
</angular_velocity>
<linear_acceleration>
<x><noise type="gaussian"><mean>0.0</mean><stddev>1.7e-2</stddev></noise></x>
<y><noise type="gaussian"><mean>0.0</mean><stddev>1.7e-2</stddev></noise></y>
<z><noise type="gaussian"><mean>0.0</mean><stddev>1.7e-2</stddev></noise></z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<!-- 雷达传感器 -->
<gazebo reference="laser_link">
<sensor name="laser_sensor" type="gpu_lidar">
<frame_id>laser_link</frame_id>
<gz_frame_id>laser_link</gz_frame_id>
<topic>scan</topic>
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>10</update_rate>
<lidar>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>-3.14159</min_angle>
<max_angle>3.14159</max_angle>
</horizontal>
</scan>
<range>
<min>0.12</min>
<max>3.5</max>
</range>
</lidar>
<plugin filename="libgz-sim-sensors-system.so" name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
</sensor>
</gazebo>
</robot>
2. 启动仿真
- 启动gzebo仿真环境，创建一个空世界：
gz sim -v 4 -r empty.sdf
-v(verbose) 参数指定日志详细程度（0-4），-r 参数指定要加载的 SDF 文件，这里加载gazebo包自带的empty.sdf文件
- 在新的终端中，使用ros_gz_sim加载机器人URDF：
ros2 run ros_gz_sim create -name fishbot -file /root/ROS2/fishbot_description/urdf/fishbot_base.urdf
-name 参数指定机器人的名称，-file 参数指定要加载的 URDF 文件路径
此时，Gazebo中应该已经加载了机器人模型，并且可以在仿真环境中看到它
- 在新的终端中，使用ros_gz_bridge桥接ROS2和Gazebo之间的通信：
ros2 run ros_gz_bridge parameter_bridge \
/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist \
/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry \
/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V \
/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan \
/imu@sensor_msgs/msg/Imu@gz.msgs.IMU \
/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock
桥接ROS2和Gazebo话题，格式为 Gazebo内部话题@ROS2消息类型@Gazebo消息类型，'@'代表双向流动
在新的终端中，使用 ros2 topic list命令可看到新增的topics:
/clock
/cmd_vel
/tf
/odom
/parameter_events
/rosout
/scan
/imu
- 设置一键启动launch：
在fishbot_description/launch中新建一个gazebo.launch.py文件:
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
def generate_launch_description():
# 1. 设置路径
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
urdf_file = '/root/ROS2/fishbot_description/urdf/fishbot_base.urdf'
# world_path = '/root/ROS2/fishbot_description/world/myworld.sdf'
# 2. 启动 Gazebo Sim
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')
),
# launch_arguments={'gz_args': f'-r {world_path}', 'on_exit_shutdown': 'true'}.items()
launch_arguments={'gz_args': '-r empty.sdf', 'on_exit_shutdown': 'true'}.items()
)
# 3. 创建小车实体
spawn_entity = Node(
package='ros_gz_sim',
executable='create',
arguments=['-name', 'fishbot',
'-file', urdf_file],
output='screen'
)
# 4. 桥接器 (Bridge)
bridge = Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=[
'/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist',
'/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry',
'/imu@sensor_msgs/msg/Imu@gz.msgs.IMU',
'/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan',
'/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock',
'/model/fishbot/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
],
remappings=[
# 将 Gazebo 内部带前缀的里程计 tf 映射到 ROS 2 的标准全局 /tf 上
('/model/fishbot/tf', '/tf'),
# 把 Gazebo 的物理关节状态映射到 /joint_states
('/model/fishbot/joint_state', '/joint_states')
],
output='screen'
)
# 5. 发布静态 TF 树 (机器人状态发布器)
robot_state = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{
'robot_description': open(urdf_file).read()
}]
)
return LaunchDescription([
gazebo,
spawn_entity,
bridge,
robot_state
])
build，source，launch：
colcon build --packages-select fishbot_description
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py
可以看到Gazebo中已经加载了机器人模型，查看TF话题数据：
ros2 topic echo /tf
3. 两轮差速插件
gazebo的插件按照用途大致可以分为两种：
- 用于控制的插件，通过插件可以控制机器人关节运动，可以进行位置、速度、力的控制，比如两轮差速控制器。
- 用于数据采集的插件，比如IMU传感器用于采集机器人的惯性，激光雷达用于采集机器人周围的点云信息
URDF中已经添加了gz-sim-diff-drive-system插件：
<plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.2</wheel_separation>
<wheel_radius>0.032</wheel_radius>
<topic>/cmd_vel</topic>
<odom_topic>/odom</odom_topic>
<frame_id>odom</frame_id>
<child_frame_id>base_footprint</child_frame_id>
</plugin>
这个插件会订阅/cmd_vel话题，接收geometry_msgs/msg/Twist类型的消息，根据消息中的线速度和角速度计算左右轮的速度，并控制机器人运动。同时，它还会发布/odom话题，发布nav_msgs/msg/Odometry类型的消息，包含机器人的位姿和速度信息。
使用键盘控制小车：
- 启动仿真环境并加载小车：
ros2 launch fishbot_description gazebo.launch.py
- 在新的终端中，安装并使用teleop_twist_keyboard功能包控制小车运动：
sudo apt install ros-kilted-teleop-twist-keyboard
ros2 run teleop_twist_keyboard teleop_twist_keyboard
该键盘控制节点默认发布数据至 /cmd_vel话题
按'i'键前进，'k'键后退，'j'键左转，'l'键右转，观察Gazebo中小车的运动情况。
在rqt显示其速度
启动rqt：
rqt
在rqt界面中，选择Plugins -> Visualization -> Plot，添加 /cmd_vel/linear/x和 /cmd_vel/angular/z两个话题，观察小车的目标线速度和角速度随键盘输入的变化情况。
显示当前速度：添加 /odom/twist/twist/linear/x和 /odom/twist/twist/angular/z两个话题。
在rviz2中显示轨迹
打开rviz2：
rviz2
- 修改FixedFrame为odom
- 添加插件，Add->Odometry->OK
- 选择话题，Odometry->Topic->选/odom
- 去除协方差显示，Odometry->Covariance>取消勾选
- 键盘控制节点，点个U，原地转圈圈
可看到rviz2中显示了odom里程计数据，显示了小车的轨迹。
4. IMU传感器插件
惯性测量单元（IMU, Inertial Measurement Unit）是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在导航中有着很重要的应用价值。
URDF中已经添加了IMU传感器插件：
<!-- IMU传感器 -->
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<gz_frame_id>imu_link</gz_frame_id>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"></plugin>
<imu>
<angular_velocity>
<x><noise type="gaussian"><mean>0.0</mean><stddev>2e-4</stddev></noise></x>
<y><noise type="gaussian"><mean>0.0</mean><stddev>2e-4</stddev></noise></y>
<z><noise type="gaussian"><mean>0.0</mean><stddev>2e-4</stddev></noise></z>
</angular_velocity>
<linear_acceleration>
<x><noise type="gaussian"><mean>0.0</mean><stddev>1.7e-2</stddev></noise></x>
<y><noise type="gaussian"><mean>0.0</mean><stddev>1.7e-2</stddev></noise></y>
<z><noise type="gaussian"><mean>0.0</mean><stddev>1.7e-2</stddev></noise></z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
该插件会在Gazebo中模拟一个IMU传感器，发布/imu话题，消息类型为sensor_msgs/msg/Imu，包含了机器人的线加速度和角速度信息，并且添加高斯噪声来模拟真实传感器的测量误差。
使用rqt的Plot插件查看一下/imu话题传输的数据：
5. 激光雷达传感器插件
激光雷达（LIDAR, Light Detection and Ranging）是一种通过发射激光束并测量其反射时间来获取周围环境信息的传感器。
URDF中已经添加了激光雷达传感器插件：
<!-- 雷达传感器 -->
<gazebo reference="laser_link">
<sensor name="laser_sensor" type="gpu_lidar">
<frame_id>laser_link</frame_id>
<gz_frame_id>laser_link</gz_frame_id>
<topic>scan</topic>
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>10</update_rate>
<lidar>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>-3.14159</min_angle>
<max_angle>3.14159</max_angle>
</horizontal>
</scan>
<range>
<min>0.12</min>
<max>3.5</max>
</range>
</lidar>
<plugin filename="libgz-sim-sensors-system.so" name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
</sensor>
</gazebo>
该插件会在Gazebo中模拟一个激光雷达传感器，发布/scan话题，消息类型为sensor_msgs/msg/LaserScan，包含了激光雷达扫描到的环境信息，如距离和角度等。
- 雷达也可以设置更新频率update_rate，这里设置为10
- 雷达可以设置分辨率，设置为1，采样数量360个，最终生成的点云数量就是360
- 雷达也有噪声，模型为gaussian
- 雷达有扫描范围range，这里配置成0.12-3.5
在rviz2中添加LaserScan插件，选择话题/scan，，将Size从0.01改为0.1，并在gazebo仿真环境中放置一些障碍物，可以看到rviz2中显示了激光雷达扫描到的点云信息：
6. World
Gazebo中的世界（World）是一个包含了环境、物理属性和机器人模型的仿真空间。我们可以在世界中添加各种元素，如地面、障碍物、光源等，以创建一个逼真的仿真环境。
在fishbot_description下新建一个world文件夹，在其中创建一个myworld.sdf文件：
<?xml version="1.0" ?>
<sdf version="1.7">
<world name="hexagonal_world">
<light name="sun" type="directional">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="my_ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<cast_shadows>false</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.3 0.3 0.3 1</diffuse> </material>
</visual>
</link>
</model>
<model name="hexagonal_arena">
<static>true</static>
<pose>0 0 0 0 0 0</pose>
<link name="wall_0">
<pose>3.0 0 0.5 0 0 0</pose>
<collision name="c"><geometry><box><size>0.2 3.46 1.0</size></box></geometry></collision>
<visual name="v"><geometry><box><size>0.2 3.46 1.0</size></box></geometry><material><ambient>0.7 0.7 0.7 1</ambient><diffuse>0.7 0.7 0.7 1</diffuse></material></visual>
</link>
<link name="wall_60">
<pose>1.5 2.598 0.5 0 0 1.0472</pose>
<collision name="c"><geometry><box><size>0.2 3.46 1.0</size></box></geometry></collision>
<visual name="v"><geometry><box><size>0.2 3.46 1.0</size></box></geometry><material><ambient>0.7 0.7 0.7 1</ambient><diffuse>0.7 0.7 0.7 1</diffuse></material></visual>
</link>
<link name="wall_120">
<pose>-1.5 2.598 0.5 0 0 2.0944</pose>
<collision name="c"><geometry><box><size>0.2 3.46 1.0</size></box></geometry></collision>
<visual name="v"><geometry><box><size>0.2 3.46 1.0</size></box></geometry><material><ambient>0.7 0.7 0.7 1</ambient><diffuse>0.7 0.7 0.7 1</diffuse></material></visual>
</link>
<link name="wall_180">
<pose>-3.0 0 0.5 0 0 3.1416</pose>
<collision name="c"><geometry><box><size>0.2 3.46 1.0</size></box></geometry></collision>
<visual name="v"><geometry><box><size>0.2 3.46 1.0</size></box></geometry><material><ambient>0.7 0.7 0.7 1</ambient><diffuse>0.7 0.7 0.7 1</diffuse></material></visual>
</link>
<link name="wall_240">
<pose>-1.5 -2.598 0.5 0 0 -2.0944</pose>
<collision name="c"><geometry><box><size>0.2 3.46 1.0</size></box></geometry></collision>
<visual name="v"><geometry><box><size>0.2 3.46 1.0</size></box></geometry><material><ambient>0.7 0.7 0.7 1</ambient><diffuse>0.7 0.7 0.7 1</diffuse></material></visual>
</link>
<link name="wall_300">
<pose>1.5 -2.598 0.5 0 0 -1.0472</pose>
<collision name="c"><geometry><box><size>0.2 3.46 1.0</size></box></geometry></collision>
<visual name="v"><geometry><box><size>0.2 3.46 1.0</size></box></geometry><material><ambient>0.7 0.7 0.7 1</ambient><diffuse>0.7 0.7 0.7 1</diffuse></material></visual>
</link>
<link name="center_box">
<pose>0 0.8 0.25 0 0 0</pose>
<collision name="col"><geometry><box><size>0.5 0.5 0.5</size></box></geometry></collision>
<visual name="vis">
<geometry><box><size>0.5 0.5 0.5</size></box></geometry>
<material><ambient>0 0 1 1</ambient><diffuse>0 0 1 1</diffuse></material>
</visual>
</link>
<link name="center_cyl_1">
<pose>-0.7 -0.5 0.25 0 0 0</pose>
<collision name="col"><geometry><cylinder><radius>0.25</radius><length>0.5</length></cylinder></geometry></collision>
<visual name="vis">
<geometry><cylinder><radius>0.25</radius><length>0.5</length></cylinder></geometry>
<material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse></material>
</visual>
</link>
<link name="center_cyl_2">
<pose>0.7 -0.5 0.25 0 0 0</pose>
<collision name="col"><geometry><cylinder><radius>0.25</radius><length>0.5</length></cylinder></geometry></collision>
<visual name="vis">
<geometry><cylinder><radius>0.25</radius><length>0.5</length></cylinder></geometry>
<material><ambient>0 1 0 1</ambient><diffuse>0 1 0 1</diffuse></material>
</visual>
</link>
</model>
</world>
</sdf>
修改launch文件，加载这个world：
world_path = '/root/ROS2/fishbot_description/world/myworld.sdf' # 设置world文件路径
# 2. 启动 Gazebo Sim
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')
),
launch_arguments={'gz_args': f'-r {world_path}','on_exit_shutdown': 'true'}.items()
)
编译运行，可以看到Gazebo中加载了自定义的世界，并在其中添加了小车：
在rviz2中可看到扫描的点阵图：
