Gezebo Classic
Gazebo(Gazebo Classic)
這個是老版Gazebo，僅在ROS1和ROS2 Humble上可用。（但是ROS2並不推薦使用老版Gazebo，更加建議使用新版Gazebo）
ROS2老版Gazebo僅在Humble版本上可用，在Jazzy及以後的版本已移除。
（不想學老版Gazebo的，直接跳到下一節的Ignition Gazebo即可）
這倆對咱們的區別不算太大，也就是一個教程多，一個教程少的區別，你直接學老版Gazebo也是一樣用的。
（初學者也可以只學教程多的Gazebo Classic,可以少走一些彎路）
Gezebo官網：
如果你想從老版的Gazebo遷移到新版的Gazebo，那麼請看下方官方教程：
從Gazebo Classic遷移到ROS2 Humble的Ign Gazebo(Gazebo Fortress)：
https://gazebosim.org/docs/fortress/gazebo\_classic\_migration/
從Gazebo Classic遷移到ROS2 Jazzy的Gazebo Sim(Gazebo Harmonic)：
Jazzy之後的版本應該變化不會太大，也可以暫時參照下面這個教程（或者去官網找對應版本的教程）：
https://gazebosim.org/docs/harmonic/gazebo\_classic\_migration/
Gazebo Classic安裝與運行
ROS2只有humble有老版Gazebo
sudo apt install ros-humble-gazebo-ros ros-humble-gazebo-ros-pkgs
安裝完成後，我們就可以通過下面的命令行來啟動gazebo並加載ros2插件。
gazebo --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so
看到下面這個日誌和gazebo界面，沒啥大問題就說明成功了。（一些警告是說Gazebo Classic已經棄用，鼓勵使用新版Ingition Gazebo，忽略這些警告即可）
root@Dell-G15-5511:/home/tungchiahui/UserFolder/MySource/ROS_WS/ROS2_WS/6.ws_simulations$ gazebo --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so
Gazebo multi-robot simulator, version 11.10.2
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org
[Msg] Waiting for master.
Gazebo multi-robot simulator, version 11.10.2
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org
[Wrn] [gazebo_ros_init.cpp:178]
# # ####### ####### ### ##### #######
## # # # # # # # #
# # # # # # # # #
# # # # # # # # #####
# # # # # # # # #
# ## # # # # # # #
# # ####### # ### ##### #######
This version of Gazebo, now called Gazebo classic, reaches end-of-life
in January 2025. Users are highly encouraged to migrate to the new Gazebo
using our migration guides (https://gazebosim.org/docs/latest/gazebo_classic_migration?utm_source=gazebo_ros_pkgs&utm_medium=cli)
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.31.60
[Msg] Loading world file [/usr/share/gazebo-11/worlds/empty.world]
XDG_RUNTIME_DIR (/run/user/1000) is not owned by us (uid 0), but by uid 1000! (This could e.g. happen if you try to connect to a non-root PulseAudio as a root user, over the native protocol. Don't do that.)
ALSA lib pcm_dmix.c:1032:(snd_pcm_dmix_open) unable to open slave
AL lib: (EE) ALCplaybackAlsa_open: Could not open playback device 'default': No such file or directory
[Err] [OpenAL.cc:84] Unable to open audio device[default]
Audio will be disabled.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.31.60
[Wrn] [GuiIface.cc:298] Couldn't locate specified .ini. Creating file at "/root/.gazebo/gui.ini"
[Wrn] [GuiIface.cc:120] QStandardPaths: runtime directory '/run/user/1000' is not owned by UID 0, but a directory permissions 0700 owned by UID 1000 GID 1000
[Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
libcurl: (35) error:0A000126:SSL routines::unexpected eof while reading
[Wrn] [ModelDatabase.cc:212] Unable to connect to model database using [http://models.gazebosim.org//database.config]. Only locally installed models will be available.
[Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
插件及節點服務介紹
使用之前的命令啟動Gazebo並加載gazebo_ros插件，我們使用下面的指令來看插件的節點，以及改節點為我們提供的服務有哪些？
節點列表
ros2 node list
然後我們看看這個節點對外提供的服務有哪些？
ros2 service list
除去和參數相關的幾個服務，我們可以看到另外三個特殊服務：
- /spawn_entity，用於加載模型到gazebo中
- /get_model_list，用於獲取模型列表
- /delete_entity，用於刪除gazbeo中已經加載的模型
我們想要讓gazebo顯示出我們配置好的機器人模型使用/spawn_entity來加載即可。
接著我們可以來請求服務來加載模型，先帶你看一下服務的接口類型。
ros2 service type /spawn_entity
ros2 interface show gazebo_msgs/srv/SpawnEntity
可以看到服務的請求內容包括：
- string name ，需要加載的實體的名稱 (可選的)。
- string xml ，實體的XML描述字符串, URDF或者SDF。
- string robot_namespace ，產生的機器人和所有的ROS接口的命名空間，多機器人仿真的時候很有用。
- geometry_msgs/Pose initial_pose ，機器人的初始化位置
- string reference_frame ，初始姿態是相對於該實體的frame定義的。如果保持"empty"或"world"或“map”，則使用 gazebo的world作為frame。如果指定了不存在的實體，則會返回錯誤
調用服務加載模型
我們這裡教程使用魚香ROS的fishbot模型：https://github.com/fishros/fishbot/blob/navgation2/src/fishbot\_description/urdf/fishbot\_gazebo.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>
<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.5" />
</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.5" />
</material>
</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="laser_joint" type="fixed">
<parent link="base_link" />
<child link="laser_link" />
<origin xyz="0 0 0.075" />
</joint>
<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>
<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>
<sphere radius="0.016"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</collision>
<inertial>
<mass value="0.02"/>
<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>
<gazebo reference="caster_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="caster_link">
<mu1 value="0.0"/>
<mu2 value="0.0"/>
<kp value="1000000.0" />
<kd value="10.0" />
</gazebo>
<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>
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.2</wheel_separation>
<wheel_diameter>0.065</wheel_diameter>
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>false</publish_wheel_tf>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
</plugin>
<plugin name="fishbot_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
<ros>
<remapping>~/out:=joint_states</remapping>
</ros>
<update_rate>30</update_rate>
<joint_name>right_wheel_joint</joint_name>
<joint_name>left_wheel_joint</joint_name>
</plugin>
</gazebo>
<gazebo reference="laser_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<ros>
<namespace>/</namespace>
<remapping>~/out:=imu</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<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>
<gazebo reference="laser_link">
<sensor name="laser_sensor" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>5</update_rate>
<pose>0 0 0.075 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>3.5</max>
<resolution>0.015000</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
<ros>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>laser_link</frame_name>
</plugin>
</sensor>
</gazebo>
</robot>
看到這裡你是不是迫不及待敲起來命令行來加載我們的機器人到gazebo了，彆著急，小魚再推薦一個可視化服務請求工具，其實在第六章中小魚介紹過，在rqt工具集裡有一個叫服務請求工具。
命令行輸入rqt，在插件選項中選擇Services->Service Caller,然後再下拉框選擇/spawn_entity服務，即可看到下面的界面。
接著我們把我們的FishBot的URDF模型複製粘貼，放到xml中（注意要把原來的''刪掉哦！）
然後點右上角的call，可以顯示下圖成功了。
接著就可以看到工廠返回說成功把機器人制作出來送入gazebo了。
此時再看我們的Gazebo,一個小小的，白白的機器人出現了。
shift+鼠標左鍵，或者直接點滾輪中鍵，都可以拖動視角。很多玩過第三人稱遊戲的學弟學妹肯定都不陌生這種操控吧。
在不同位置加載多個機器人
可以再生產一個fishbot（為了後面需要多機器人仿真的小夥伴）。
修改rqt中的參數，增加一個命名空間，然後修改一個位置，讓第二個機器人和第一個相距1m的地方生產，然後點擊Call。
返回成功，此時拖送Gazebo觀察一下，發現多出了一個機器人，距離剛好是在X軸（紅色）1米（一個小格子一米）處。
查詢和刪除機器人
利用rqt工具，我們再對另外兩個服務接口進行請求。
首先先查詢有幾個模型在仿真環境中
查到了三個模型，一個大地，一個fishbot，一個fishbot_0。
我們接著嘗試把fishbot_0刪掉，選擇刪除實體，輸入fishbot_0的名字，拿起小電話通知工廠回收我們的0號fishbot。
調用成功，觀察gazebo發現機器人，人沒了
創建工作空間
本節代碼參考魚香ROS：https://github.com/fishros/fishbot/tree/navgation2
# 创建工作空间
mkdir -p ws_simulations/src
cd ws_simulations/src
創建功能包
ros2 pkg create fishbot_description --build-type ament_cmake
cd fishbot_description
然後配置package.xml
<exec_depend>rviz2</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>ros2launch</exec_depend>
然後修改cmakelists.txt
install(
DIRECTORY launch urdf rviz meshes
DESTINATION share/${PROJECT_NAME}
)
克隆下魚香ROS的倉庫，並複製下里面的文件到咱們的目錄下。這都是上一節的東西，與本節學習無關，直接複製就行。
打開工作空間，在src/fishbot_description/launch中添加一個gazebo.launch.py文件，我們開始編寫launch文件來在gazebo中加載機器人模型。
我們主要需要做兩件事：
- 啟動gazebo，我們可以將命令行寫成一個launch節點
ExecuteProcess(
cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
output='screen')
- 上面我們加載機器人是直接將XML格式的URDF複製過去進行加載的，這樣很不方便，我們可以使用gazebo_ros為我們提供好的一個叫做
spawn_entity.py節點，該節點支持從文件地址直接生產機器人到Gazebo。
該節點需要兩個參數，一個機器人的模型名字和urdf的文件地址，這個簡單，前面我們曾經使用package_share來拼接過urdf路徑。
spawn_entity_cmd = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', robot_name_in_model, '-file', urdf_model_path ], output='screen')
先加載趙虛左老師的模板
我們首先需要ExecuteProcess來輸入終端命令，所以要先把from launch.actions import ExecuteProcess的註釋先打開。
為了方便修改加載的機器人模型和urdf,我們還需要使用share目錄，所以也要把from ament_index_python.packages import get_package_share_directory的註釋打開，並import os。
寫完後的launch文件如下：
from launch import LaunchDescription
from launch_ros.actions import Node
# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
robot_name_in_model = 'fishbot'
package_name = 'fishbot_description'
urdf_name = "fishbot_gazebo.urdf"
pkg_share = get_package_share_directory(f"{package_name}")
urdf_model_path = os.path.join(pkg_share, f'urdf/urdf/{urdf_name}')
# Start Gazebo server
start_gazebo_cmd = ExecuteProcess(
cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
output='screen')
# Launch the robot
spawn_entity_cmd = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', robot_name_in_model, '-file', urdf_model_path ], output='screen')
return LaunchDescription([start_gazebo_cmd,spawn_entity_cmd])
編譯運行
colcon build --packages-select fishbot_description
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py
插件
使用下面的指令可以查看所有的動態鏈接庫：
ls /opt/ros/humble/lib/libgazebo_ros*
/opt/ros/humble/lib/libgazebo_ros2_control.so
/opt/ros/humble/lib/libgazebo_ros_ackermann_drive.so
/opt/ros/humble/lib/libgazebo_ros_bumper.so
/opt/ros/humble/lib/libgazebo_ros_camera.so
/opt/ros/humble/lib/libgazebo_ros_diff_drive.so
/opt/ros/humble/lib/libgazebo_ros_elevator.so
/opt/ros/humble/lib/libgazebo_ros_factory.so
/opt/ros/humble/lib/libgazebo_ros_force.so
/opt/ros/humble/lib/libgazebo_ros_force_system.so
/opt/ros/humble/lib/libgazebo_ros_ft_sensor.so
/opt/ros/humble/lib/libgazebo_ros_gps_sensor.so
/opt/ros/humble/lib/libgazebo_ros_hand_of_god.so
/opt/ros/humble/lib/libgazebo_ros_harness.so
/opt/ros/humble/lib/libgazebo_ros_imu_sensor.so
/opt/ros/humble/lib/libgazebo_ros_init.so
/opt/ros/humble/lib/libgazebo_ros_joint_pose_trajectory.so
/opt/ros/humble/lib/libgazebo_ros_joint_state_publisher.so
/opt/ros/humble/lib/libgazebo_ros_node.so
/opt/ros/humble/lib/libgazebo_ros_p3d.so
/opt/ros/humble/lib/libgazebo_ros_planar_move.so
/opt/ros/humble/lib/libgazebo_ros_projector.so
/opt/ros/humble/lib/libgazebo_ros_properties.so
/opt/ros/humble/lib/libgazebo_ros_ray_sensor.so
/opt/ros/humble/lib/libgazebo_ros_state.so
/opt/ros/humble/lib/libgazebo_ros_template.so
/opt/ros/humble/lib/libgazebo_ros_tricycle_drive.so
/opt/ros/humble/lib/libgazebo_ros_utils.so
/opt/ros/humble/lib/libgazebo_ros_vacuum_gripper.so
/opt/ros/humble/lib/libgazebo_ros_video.so
/opt/ros/humble/lib/libgazebo_ros_wheel_slip.so
運動控制插件+里程計odom
本節課通過配置兩輪差速控制插件，讓我們的機器人動起來
插件介紹：
Gazebo是一個獨立於ROS的軟件，對外提供了豐富的API可以使用，gazebo的插件按照用途大致可以分為兩種：
- 用於控制的插件，通過插件可以控制機器人關節運動，可以進行位置、速度、力的控制，比如我們這節課的兩輪差速控制器。
- 用於數據採集的插件，比如IMU傳感器用於採集機器人的慣性，激光雷達用於採集機器人周圍的點雲信息。
當然上面兩類插件功能也可以寫到一個插件裡，兩輪差速插件（gazebo_ros_diff_drive）就是一個二合一加強版。（差速控制+odom）
兩輪差速插件用於控制機器人輪子關節的位置變化，同時該插件還會獲取輪子的位置以及速度的信息的反饋，根據反饋的位置信息結合運動學模型即可計算出當前機器人的位姿（里程計）。
兩輪差速控制器可以將輪子的目標轉速發送給Gazebo，並從Gazebo獲取到實際的速度和位置。（注意：發送給Gazebo是目標速度，反饋回來的是實際速度。目標!=實際，比如輪子卡住了，無論你發什麼目標速度，實際速度都是0。）
要想快速瞭解一個系統的功能，最直接的就是看系統的對外的輸入和輸出是什麼？什麼都不要說，看下圖：
上圖就是對gazebo_ros_diff_drive的輸入和輸出信息的總結，可以很直觀的看到該插件主要輸入控制指令，主要輸出里程計信息。接著小魚帶你分別認識一下輸入和輸出兩個部分。
這個插件需要配置一系列參數如下圖：
不知道你是否還記得在第七章中，小魚對兩輪差速底盤的運動學正的介紹。如果要完成底盤的正逆解和里程計的推算就必須要知道輪子的直徑和間距。
同時該插件還提供了一些可以控制輸出的選項，因為是仿真，所以還要告訴插件輪子對應的joint名稱等信息，這樣就有了下面這個參數表格：
| 配置項 | 含義 |
|---|---|
| ros | ros相關配置，包含命名空間和話題重映射等 |
| update_rate | 數據更新速率 |
| left_joint | 左輪關節名稱 |
| right_joint | 右輪關節名稱 |
| wheel_separation | 左右輪子的間距 |
| wheel_diameter | 輪子的直徑 |
| max_wheel_torque | 輪子最大的力矩 |
| max_wheel_acceleration | 輪子最大的加速度 |
| publish_odom | 是否發佈里程計 |
| publish_odom_tf | 是否發佈里程計的tf開關 |
| publish_wheel_tf | 是否發佈輪子的tf數據開關 |
| odometry_frame | 里程計的framed ID，最終體現在話題和TF上 |
| robot_base_frame | 機器人的基礎frame的ID |
控制指令：兩輪差速控制器默認通過訂閱話題cmd_vel來獲取目標線速度和角速度。該話題的類型為：geometry_msgs/msg/Twist
該接口裡主要是一些線速度和角速度都包含在x、y、z，代表座標系的三個方向上的對應速度。 （詳細的接口內容請看硬件平臺章節）
兩輪差速控制器收到這個話題數據後將其中的角速度和線速度轉換上兩個輪子的轉動速度發送給Gazebo。
輸出的信息：
里程計信息默認的輸出話題為odom，其消息類型為：nav_msgs/msg/Odometry
其數據主要包含三個部分： （詳細的接口內容請看硬件平臺章節）
- header，表示該消息發佈的時間
- pose，表示當前機器人位置和朝向
- twist，表示當前機器人的線速度和角速度
- 數據中還包含一個covariance，其代表協方差矩陣，後面小魚寫篇文章來介紹下，這裡只需瞭解其含義即可。
里程計TF信息也可以輸出：設為true，訂閱tf話題裡你就可以看到像下面的msg，建議後面配置好後，手動修改下，對比區別
- header:
stamp:
sec: 6157
nanosec: 907000000
frame_id: odom
child_frame_id: base_footprint
transform:
translation:
x: 0.0005557960241049835
y: -0.0007350446303238693
z: 0.01599968753145574
rotation:
x: 4.691143395208505e-07
y: 7.115496626557812e-06
z: -0.018531475772549166
w: 0.9998282774331005
輪子TF信息也可以輸出：設為true，訂閱tf話題裡你就可以看到像下面的msg，建議後面配置好後，手動修改下，對比區別
- header:
stamp:
sec: 6157
nanosec: 941000000
frame_id: base_link
child_frame_id: left_wheel_link
transform:
translation:
x: -0.02
y: 0.1
z: -0.06
rotation:
x: 0.0
y: 0.049519025127821005
z: 0.0
w: 0.9987731805321918
- header:
stamp:
sec: 6157
nanosec: 941000000
frame_id: base_link
child_frame_id: right_wheel_link
transform:
translation:
x: -0.02
y: -0.1
z: -0.06
rotation:
x: 0.0
y: -0.0663387077034509
z: 0.0
w: 0.9977971616817898
咱們之前下載的那個魚香ROS的fishbot的urdf已經包含了差速插件的內容，如下：
因為是給Gazebo的插件，所以在URDF中，我們需要使用<gazebo>進行配置，因為是要給gazebo配置插件，所有要在gazebo標籤下添加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>
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.2</wheel_separation>
<wheel_diameter>0.065</wheel_diameter>
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<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>
編譯一下：
colcon build
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py
然後可以看看下面這倆命令：
ros2 node list
ros2 topic list
可以看到了我們插件訂閱的的/cmd_vel和發佈的/odom了。
然後我們可以通過teleop-twist-keyboard節點發布cmd_vel來控制fishbot。
sudo apt install ros-humble-teleop-twist-keyboard
使用下方節點來控制
ros2 run teleop_twist_keyboard teleop_twist_keyboard
接著嘗試使用來控制機器人運動
U I O
J K L
M < >
點一下，你就能看到fishbot在Gazebo中飛速的移動。接著打開終端，打印一下odom話題和tf話題，移動機器人觀察數據變化。
也可以使用rqt顯示速度數據
rqt
選擇Plugin->Visualization->Plot
在上方Topic輸入/cmd_vel/linear/x，再輸入/cmd_vel/angular/z，然後用鍵盤控制機器人移動。
也可以在rviz2裡看odom
rviz2
點鍵盤控制節點按U，讓機器人轉圈。
雖然機器人的軌跡已經在RVIZ中顯示出來了，但是並沒有機器人的模型，也看不到輪子的轉動，咱們來帶你一起解決這個問題。
前面咱們介紹過，要發佈機器人模型我們所使用的節點是robot_state_publisher,所以我們在gazebo.launch.py中加入這個節點，同時再加上rviz2的啟動節點，最終的gazebo.launch.py內容如下：
from launch import LaunchDescription
from launch_ros.actions import Node
# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
robot_name_in_model = 'fishbot'
package_name = 'fishbot_description'
urdf_name = "fishbot_gazebo.urdf"
pkg_share = get_package_share_directory(f"{package_name}")
urdf_model_path = os.path.join(pkg_share, f'urdf/urdf/{urdf_name}')
# gazebo_world_path = os.path.join(pkg_share, 'world/fishbot.world')
# Start Gazebo server
# start_gazebo_cmd = ExecuteProcess(
# cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so',gazebo_world_path],
# output='screen')
start_gazebo_cmd = ExecuteProcess(
cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
output='screen')
# Launch the robot
spawn_entity_cmd = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', robot_name_in_model, '-file', urdf_model_path ], output='screen')
# Start Robot State publisher
start_robot_state_publisher_cmd = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
arguments=[urdf_model_path]
)
# Launch RViz
start_rviz_cmd = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
# arguments=['-d', default_rviz_config_path]
)
return LaunchDescription([start_gazebo_cmd,spawn_entity_cmd,start_robot_state_publisher_cmd,start_rviz_cmd])
保存編譯啟動
colcon build
ros2 launch fishbot_description gazebo.launch.py
這樣rviz2裡就有模型了。
可以保存下rviz2的配置
然後在launch裡添加上rviz2的路徑配置：
default_rviz_config_path = os.path.join(pkg_share, 'rviz/rviz2.rviz')
# Launch RViz
start_rviz_cmd = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', default_rviz_config_path]
)
如下為全部的launch：
from launch import LaunchDescription
from launch_ros.actions import Node
# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
robot_name_in_model = 'fishbot'
package_name = 'fishbot_description'
urdf_name = "fishbot_gazebo.urdf"
pkg_share = get_package_share_directory(f"{package_name}")
urdf_model_path = os.path.join(pkg_share, f'urdf/urdf/{urdf_name}')
# gazebo_world_path = os.path.join(pkg_share, 'world/fishbot.world')
default_rviz_config_path = os.path.join(pkg_share, 'rviz/rviz2.rviz')
# Start Gazebo server
# start_gazebo_cmd = ExecuteProcess(
# cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so',gazebo_world_path],
# output='screen')
start_gazebo_cmd = ExecuteProcess(
cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
output='screen')
# Launch the robot
spawn_entity_cmd = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', robot_name_in_model, '-file', urdf_model_path ], output='screen')
# Start Robot State publisher
start_robot_state_publisher_cmd = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
arguments=[urdf_model_path]
)
# Launch RViz
start_rviz_cmd = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', default_rviz_config_path]
)
return LaunchDescription([start_gazebo_cmd,spawn_entity_cmd,start_robot_state_publisher_cmd,start_rviz_cmd])
可以自行編譯測試。
慣性計IMU
上節課通過配置兩輪差速控制器我們已經成功的讓fishbot在gazebo中動了起來，本節課我們通過給fishbot的URDF配置IMU傳感器插件，讓IMU模塊工作起來。
慣性測量單元是測量物體三軸姿態角(或角速率)以及加速度的裝置。一般的，一個IMU包含了三個單軸的加速度計和三個單軸的陀螺，加速度計檢測物體在載體座標系統獨立三軸的加速度信號，而陀螺檢測載體相對於導航座標系的角速度信號，測量物體在三維空間中的角速度和加速度，並以此解算出物體的姿態。在導航中有著很重要的應用價值。
上面這段話是小魚從百科中摘抄出來的，你需要知道的一個關鍵點是IMU可以測量以下三組數據：
- 三維加速度計加速度
- 三維陀螺儀角速度
- 三維磁力計（有的也沒有磁力計）
用六軸、九軸算法或其他算法等可以輸出三軸歐拉角（Yaw,Pitch,Roll），歐拉角可以轉化為四元數。
IMU長啥樣？直接線下找控制組要就行，他們經常會玩這個。
便宜的長這樣（MPU6050,MPU9050等）：
MPU6050是六軸的，MPU9050是九軸的。
貴的長這樣（HWT101CT，HWT605等）：
其中HWT101CT是三軸的，HWT605是六軸的。（各有優缺點）
不要錢的長什麼樣？
仿真的不要錢哈哈，接著我們來配置一下仿真的IMU。
IMU對應的消息類型為sensor_msgs/msg/Imu
ROS的imu信息只有三軸加速度，角速度和四元數。（並沒有磁力計和歐拉角，磁力計並非必須要填的，而歐拉角和四元數可以互相轉換，四元數更好被算法運算，所以選擇用四元數）
具體imu接口請看硬件平臺章節。
可以看到除了每個數據對應的三個協方差之外，每一個還都對應一個3*3的協方差矩陣。
有了上節課的經驗，我們可以很輕鬆的添加IMU傳感器，但是還有一個需要注意的地方，為了更真實的模擬IMU傳感器，我們需要給我們的仿真IMU傳感器加點料。
加什麼？加點高斯噪聲，高斯噪聲只需要指定平均值和標準差兩個參數即可，不過因為IMU傳感器的特殊性，我們還需要給模型添加兩個偏差參數，分別是 平均值偏差和标准差偏差。
有關Gazebo仿真和噪聲模型更深入的介紹可以參考魚香ROS發的兩篇推文：
下面是IMU傳感器的URDF配置代碼，大家結合文章對應可以理解一下，IMU對應的插件庫libgazebo_ros_imu_sensor.so：
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<ros>
<namespace>/</namespace>
<remapping>~/out:=imu</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<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>
我們之前下載的fishbot已經包含該內容了，所以不用再添加了，直接運行即可。
ros2 launch fishbot_description gazebo.launch.py
ros2 topic list
ros2 topic info /imu
ros2 topic echo /imu
header:
stamp:
sec: 150
nanosec: 599000000
frame_id: base_footprint
orientation:
x: 3.434713830866392e-07
y: 7.119913105768616e-06
z: -0.00028312437320413914
w: 0.9999999598948884
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
x: -0.00013597855247901325
y: 0.0006306135617081868
z: -0.00015794894627685146
angular_velocity_covariance:
- 4.0e-08
- 0.0
- 0.0
- 0.0
- 4.0e-08
- 0.0
- 0.0
- 0.0
- 4.0e-08
linear_acceleration:
x: 0.08679200038530369
y: 0.07753419258567491
z: 9.687910969061628
linear_acceleration_covariance:
- 0.00028900000000000003
- 0.0
- 0.0
- 0.0
- 0.00028900000000000003
- 0.0
- 0.0
- 0.0
- 0.00028900000000000003
用rqt可視化：
雷達Laser
本節我們來認識一個新的傳感器，該傳感器在自動駕駛、室內導航等應用非常多，比如掃地機器人上就是用的它作為感知環境的重要工具，該傳感器是激光雷達。
激光雷達（Light Detection And Ranging）,縮寫LiDAR，英文也叫laser,翻譯一下叫——激光探測與測距。
激光雷達的原理也很簡單，就像蝙蝠的定位方法一樣，蝙蝠定位大家都知道吧，像下面這樣子的回聲定位。
普通的單線激光雷達一般有一個發射器，一個接收器，發射器發出激光射線到前方的目標上，物品會將激光反射回來，然後激光雷達的接受器可以檢測到反射的激光。
通過計算發送和反饋之間的時間間隔，乘上激光的速度，就可以計算出激光飛行的距離，該計算方法成為TOF（飛行時間法Time of flight，也稱時差法）。
除了TOF之外還有其他方法進行測距，比如三角法，這裡就不拓展了放一篇文章，大家自行閱讀。激光三角測距原理詳述
目前市面上的激光雷達，幾乎都是採用三角測距，比如思嵐的：
需要注意的是雖然只有一個發射器和一個接受器，激光雷達通過電機可以進行旋轉，這樣就可以達到對周圍環境360度測距的目的。
五位數的長這樣：
四位數的長這樣（咱們有一臺）
三位數的長這樣（咱們也有）
兩位數的長這樣
不要錢的長這樣
仿真的，不要錢
因為激光雷達是屬於射線類傳感器，該類傳感在在Gazebo插件中都被封裝成了一個動態庫libgazebo_ros_ray_sensor.so。
接著我們來看看LiDAR的話題消息接口sensor_msgs/msg/LaserScan。
雷達的數據結構有些複雜，但通過註釋和名字相信你可以看的七七八八，看不懂也沒關係，一般情況下我們不會直接的對雷達的數據做操作。
雷達的模型不需要collision，請刪掉，否則會擋激光射出。
有了前面的經驗，我們需要在URDF添加以下內容即可，但我們下載的是魚香ROS添加好的，所以不用改了：
<gazebo reference="laser_link">
<sensor name="laser_sensor" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>10</update_rate>
<pose>0 0 0.075 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>3.5</max>
<resolution>0.015000</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
<ros>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>laser_link</frame_name>
</plugin>
</sensor>
</gazebo>
可以看到:
- 雷達也可以設置更新頻率
update_rate，這裡設置為5 - 雷達可以設置分辨率，設置為1，採樣數量360個，最終生成的點雲數量就是360
- 雷達也有噪聲，模型為
gaussian - 雷達有掃描範圍
range，這裡配置成0.12-3.5，0.015分辨率 - 雷達的
pose就是雷達的joint中位置的設置值
下面這個藍色的就是激光雷達的光線覆蓋範圍：
ros2 topic list
ros2 topic info /scan
ros2 topic echo /scan
接著我們嘗試使用rviz2進行可視化激光雷達數據
添加和修改RVIZ2的如下：（通過LaserScan插件可以看到激光數據）
相信你改完之後依然是看不到任何激光雷達的數據的，反看topic的echo出來的數據，不是0就是inf(無限大)，再看看gazebo你會發現，激光雷達並沒有達到任何一個物體上。
所以我們可以手動的給激光雷達周圍添加一下東西，點擊Gazebo工具欄的正方體，圓球或者圓柱，隨意放置幾個到我們激光雷達的最大掃描半徑內。
超聲波Ultrasonic
這玩意對於ROS2算法意義不是很大，控制組直接在MCU上實現即可，有需求可以學本節。 （可以跳過本節）
在實際的機器人開發過程中，我們可能會利用超聲波傳感器實現實時避障的功能，畢竟超聲波的價格相較於激光雷達要便宜很多（便宜的幾塊錢）。
所以本節我們來說一下如何使用ROS2+Gazebo來仿真超聲波傳感器。
百科來一段：
超聲波傳感器是將超聲波信號轉換成其它能量信號（通常是電信號）的傳感器。超聲波是振動頻率高於20kHz的機械波。它具有頻率高、波長短、繞射現象小，特別是方向性好、能夠成為射線而定向傳播等特點。超聲波對液體、固體的穿透本領很大，尤其是在陽光不透明的固體中。超聲波碰到雜質或分界面會產生顯著反射形成反射回波，碰到活動物體能產生多普勒效應。超聲波傳感器廣泛應用在工業、國防、生物醫學等方面。
接著看看長什麼樣子：
便宜的就長這樣子，一共兩個頭，一個頭用於發送波，一個頭接收波。這個還稍微高級一點，帶一個光敏電阻，可以為超聲波數據做一些補償。
超聲波傳感器原理是什麼呢？
距离=(发送时间-接收时间)*速度/2Copy to clipboardErrorCopied
看了超聲波的原理，你有沒有發現和前面的激光雷達傳感器是一樣的，是的，所以超聲波傳感器插件和激光雷達傳感器插件在Gazebo插件中是同一個：
libgazebo_ros_ray_sensor.so
超聲波總要裝在機器人身上某個位置，所以我們先添加一個關節和Joint，為了省事，link我們就只寫個名字，你如果有需要可以按照前面的章節那樣添加一下。
<link name="ultrasonic_sensor_link" />
<joint name="ultrasonic_sensor_joint" type="fixed">
<parent link="base_link"/>
<child link="ultrasonic_sensor_link"/>
<origin xyz="0.07 0.0 0.076" rpy="0 0 0"/>
</joint>
添加完了關節，我們就可以配置gazebo的插件了，gazebo插件配置如下
<gazebo reference="ultrasonic_sensor_link">
<sensor type="ray" name="ultrasonic_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>5</update_rate>
<ray>
<scan>
<horizontal>
<samples>5</samples>
<resolution>1</resolution>
<min_angle>-0.12</min_angle>
<max_angle>0.12</max_angle>
</horizontal>
<vertical>
<samples>5</samples>
<resolution>1</resolution>
<min_angle>-0.01</min_angle>
<max_angle>0.01</max_angle>
</vertical>
</scan>
<range>
<min>0.02</min>
<max>4</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="ultrasonic_sensor_controller" filename="libgazebo_ros_ray_sensor.so">
<ros>
<remapping>~/out:=ultrasonic_sensor_1</remapping>
</ros>
<output_type>sensor_msgs/Range</output_type>
<radiation_type>ultrasound</radiation_type>
<frame_name>ultrasonic_sensor_link</frame_name>
</plugin>
</sensor>
</gazebo>
添加完成後就可以編譯測試下代碼
colcon build --packages-select fishbot_description
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py
沒有物體的前面可以放個東西
打開終端，輸入下面指令
ros2 topic list
ros2 topic info /ultrasonic_sensor_1
ros2 topic echo /ultrasonic_sensor_1Copy to clipboardErrorCopied
不出意外可以看到下面的數據
header:
stamp:
sec: 4458
nanosec: 1000000
frame_id: ultrasonic_sensor_link
radiation_type: 0
field_of_view: 0.23999999463558197
min_range: 0.019999999552965164
max_range: 4.0
range: 2.6798219680786133
這裡的range就是fishbot到牆之間的距離：2.67982
我們來講一講超聲波傳感器的數據類型sensor_msgs/msg/Range
# ros2 topic info /ultrasonic_sensor_1
Type: sensor_msgs/msg/Range
Publisher count: 1
Subscription count: 0
你可以使用ros2 interface show sensor_msgs/msg/Range看到詳細的解釋，我們翻譯一下
# Single range reading from an active ranger that emits energy and reports
# one range reading that is valid along an arc at the distance measured.
# This message is not appropriate for laser scanners. See the LaserScan
# message if you are working with a laser scanner.
#
# This message also can represent a fixed-distance (binary) ranger. This
# sensor will have min_range===max_range===distance of detection.
# These sensors follow REP 117 and will output -Inf if the object is detected
# and +Inf if the object is outside of the detection range.
std_msgs/Header header # timestamp in the header is the time the ranger
# returned the distance reading
# Radiation type enums
# If you want a value added to this list, send an email to the ros-users list
uint8 ULTRASOUND=0
uint8 INFRARED=1
uint8 radiation_type # 传感器射线类型
# (sound, IR, etc) [enum]
float32 field_of_view # 距离数据对应的弧[rad]的大小，测量物体的范围介于
# -field_of_view/2 到 field_of_view/2 之间。
# 0 角度对应于传感器的 x 轴。
float32 min_range # 最小范围值 [m]
float32 max_range # 最大范围值 [m]
# 固定距离需要 min_range==max_range
float32 range # 范围数据 [m]
# (Note: values < range_min or > range_max should be discarded)
# Fixed distance rangers only output -Inf or +Inf.
# -Inf represents a detection within fixed distance.
# (Detection too close to the sensor to quantify)
# +Inf represents no detection within the fixed distance.
# (Object out of range)
結論，主要關注range就可以了。
在rviz2裡添加超聲波數據
Add ->By topic->Range
搭建世界地圖
本節我們要在Gazebo中建立一個測試的環境，其實也很簡單，利用Gazebo的畫牆工具即可完成。
world即世界，gazebo的world文件就是用於描述世界模型的，也就是環境模型。
Gazebo已經為我們準備了很多常用的物體模型，除了基礎的圓球，圓柱，立方體外的，其實還有飛機、汽車、房子等你現實中無法擁有的。
但是一開始安裝Gazebo的時候並不會幫你下載好這些模型，需要我們手動下載，找一個你要存模型的文件夾，打開終端，複製粘貼下面這句
git clone https://github.com/osrf/gazebo_models
並把存放模型的路徑加到~/.bashrc裡（第二個冒號後面的可以不寫，第二個冒號後面的是多個路徑。）
GAZEBO_MODEL_PATH是老版Gazebo Classic的宏。
IGN_GAZEBO_RESOURCE_PATH是新版Igntion Gazebo的宏。
模型都通用，可以一起配置上。
刷新環境變量
source ~/.bashrc
此時再次打開終端，輸入gazebo，把選項卡切換到Insert
在Insert選項卡下可以看到一個目錄，以及目錄下的模型名稱，隨著下載腳本的不斷下載，這裡的模型會越來越多。
隨手拖幾個，搭建一個漂亮的環境出來~
每個成功的男人都有一輛車，咱們也不例外
上面是Gazebo為我們準備好的開源模型，我們也可以通過Gazebo的工具來自己畫一個環境。
然後也可以用牆壁工具建牆
Gazebo左上角->Edit->Building Editor
接著可以看到這樣一個編輯界面
點擊左邊的Wall,你就可以在上方的白色區域進行建牆了，這個和模擬人生遊戲不一樣，這個是畫二維的牆生成三維的牆，模擬人生是直接在三維裡畫。
建完後還可以用選Add Color或者Add Texture，然後點擊下方牆，給牆添加顏色或者紋理。
首先你要有一個地圖，小魚為你準備了兩個，兩個圖片都是800*600像素的。
打開Gazebo->Gazebo左上角->Edit->Building Editor->左下方選Import
將上面兩個圖片存到本地，在這個界面選圖片，記著選Next
左邊選尺寸對應關係
我們選擇默認的，100像素/米。點擊OK（需要手動將100改變一下才能點擊OK哦），之後就可以用圖片畫牆了。
注意：導入完圖片不會直接出來牆，圖片只是提供了牆的大概位置，需要你手動用牆再將邊描一遍。
建完後點擊File->Exit,在退出的彈框中選Exit。
接著在Gazebo界面中就可以看到牆了，我們再手動添加幾個物體，就可以用於下面的導航使用了。
添加完，接著點擊File->SaveWorld，將文件保存到我們的fishbot_descrption的world下。
沒有world目錄的小夥伴可以先手動創建下
加載world其實也很簡單，可以先啟動Gazebo，再手動的加載文件，也可以在Gazebo啟動時加載：
比如在前面加載ROS2插件基礎上再加載fishbot.world。
gazebo --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so 你的world文件目录/fishbot.world
修改launch文件，將上面的命令行寫到gazebo.launch.py中即可。
gazebo_world_path = os.path.join(pkg_share, 'world/fishbot.world')
# Start Gazebo server
start_gazebo_cmd = ExecuteProcess(
cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
output='screen')
下面是整個launch文件：
from launch import LaunchDescription
from launch_ros.actions import Node
# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
robot_name_in_model = 'fishbot'
package_name = 'fishbot_description'
urdf_name = "fishbot_gazebo.urdf"
world_name = "fishbot.world"
pkg_share = get_package_share_directory(f"{package_name}")
urdf_model_path = os.path.join(pkg_share, f'urdf/urdf/{urdf_name}')
gazebo_world_path = os.path.join(pkg_share, f'world/{world_name}')
default_rviz_config_path = os.path.join(pkg_share, 'rviz/rviz2.rviz')
# Start Gazebo server
start_gazebo_cmd = ExecuteProcess(
cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so',gazebo_world_path],
output='screen')
# Launch the robot
spawn_entity_cmd = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', robot_name_in_model, '-file', urdf_model_path ], output='screen')
# Start Robot State publisher
start_robot_state_publisher_cmd = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
arguments=[urdf_model_path]
)
# Launch RViz
start_rviz_cmd = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', default_rviz_config_path]
)
return LaunchDescription([start_gazebo_cmd,spawn_entity_cmd,start_robot_state_publisher_cmd,start_rviz_cmd])
最後記得修改cmakelists.txt文件，讓編譯後將world文件拷貝到install目錄下
colcon build
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py
框架優化
本節代碼學長也發到倉庫了，有需要的學弟學妹請看:https://github.com/tungchiahui/ROS\_WS/tree/main/ROS2\_WS%2F6.ws\_simulations%2Fsrc%2Ffishbot\_description
比如說支持xacro等優化。
首先先把原來的fishbot_gazebo.urdf裡的內容分成好幾個urdf和xacro再用一個總的fishbot.urdf.xacro去引用。（學過xacro的肯定都會）
優化後的launch如下：
from launch import LaunchDescription
from launch_ros.actions import Node
# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command,LaunchConfiguration
import os
def generate_launch_description():
robot_name_in_model = 'fishbot'
package_name = 'fishbot_description'
# urdf_xacro_name = "fishbot_gazebo.urdf"
world_name = "fishbot.world"
pkg_share = get_package_share_directory(f"{package_name}")
urdf_xacro_model_path = os.path.join(pkg_share, "urdf/xacro","fishbot.urdf.xacro")
gazebo_world_path = os.path.join(pkg_share, f'world/{world_name}')
default_rviz_config_path = os.path.join(pkg_share, 'rviz/rviz2.rviz')
model = DeclareLaunchArgument(name="model", default_value=urdf_xacro_model_path)
robot_description = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
# Start Gazebo server
start_gazebo_cmd = ExecuteProcess(
cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so',gazebo_world_path],
output='screen')
# Launch the robot
spawn_entity_cmd = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', robot_name_in_model, '-topic', '/robot_description' ], output='screen')
# Start Robot State publisher
start_robot_state_publisher_cmd = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{"robot_description": robot_description}]
)
# Launch RViz
start_rviz_cmd = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', default_rviz_config_path]
)
return LaunchDescription([model,start_gazebo_cmd,spawn_entity_cmd,start_robot_state_publisher_cmd,start_rviz_cmd])
colcon build
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py
然後就可以去搞導航啦！也可以接著學新版Ignition Gazebo，但這東西教程巨少，截止2024年只有趙虛左老師講了。 ** 建議學習新的 Gazebo Harmonic （ROS2 Jazzy默認的版本） **