ROS2常用工具

坐标变换工具

TF（Transform）是ROS2中用于处理机器人各部件之间坐标系变换的核心功能，是机器人开发中定位、导航、感知的基础。

机器人有很多部件，每个部件都有自己的局部坐标系。TF 的作用就是实时记录并计算这些坐标系之间的位置（平移）和姿态（旋转）关系，让你能轻松把一个坐标系下的点，转换成另一个坐标系下的点。

核心术语

坐标系（Frame）：每个部件的坐标参考系，比如base_link（底盘坐标系）、laser_link（雷达坐标系）、map（地图坐标系）。

变换（Transform）：两个坐标系之间的关系，包含平移（x/y/z）和旋转（四元数 / 欧拉角）。

TF树：所有坐标系通过父子关系组成的树形结构。

发布机械臂底座到摄像头的静态TF

先安装相关库

sudo apt install ros-$ROS_DISTRO-tf-transformations

在新建一个工作空间，并在下面建一个src文件夹

在src目录下新建一个python_tf功能包

ros2 pkg create --build-type ament_python --dependencies rclpy geometry_msgs tf_ros tf_transformations --license Apache-2.0 python_tf

新建一个py文件编写代码

import rclpy
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster # 静态坐标发布器
from geometry_msgs.msg import TransformStamped # 接口消息
from tf_transformations import quaternion_from_euler # 欧拉角转四元素函数
import math

class StaticTFBroadcaster(Node):
    def __init__(self):
        super().__init__('static_tf_broadcaster')
        self.static_broadcaster = StaticTransformBroadcaster(self) # 创建静态坐标发布器
        self.publish_static_tf()

    # 发布静态TF
    def publish_static_tf(self):
        transform = TransformStamped()      # 消息接口
        transform.header.frame_id = 'base_link'   # 父坐标系
        transform.child_frame_id = 'camera_link'  # 子坐标系
        transform.header.stamp = self.get_clock().now().to_msg()

        transform.transform.translation.x=0.5
        transform.transform.translation.y=0.3
        transform.transform.translation.x=0.6

        q = quaternion_from_euler(math.radians(180),0,0)  # 欧拉角转四元素 q=x,y,z,w
        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.z = q[2]
        transform.transform.rotation.w = q[3]
        # 发布静态坐标关系
        self.static_broadcaster.sendTransform(transform)
        self.get_logger().info(f'发布静态TF:{transform}')

def main():
    rclpy.init()
    node = StaticTFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()

在setup.py中配置可执行文件

构建、配置，然后运行

ros2 run python_tf static_tf_broadcaster

我们可以再开一个终端，查看一下话题

 ros2 topic echo  /tf_static

虽然静态tf只发布一次，但该话题会持续保留一条最新的数据

还可以运行查看两个坐标系的位置关系

ros2 run tf2_ros tf2_echo base_link camera_link

发布摄像头到瓶子的动态TF

新建一个dynamic_tf_broadcaster.py文件

import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster 
from geometry_msgs.msg import TransformStamped
from tf_transformations import quaternion_from_euler
import math

class TFBroadcaster(Node):
    def __init__(self):
        super().__init__('tf_broadcaster')
        self.broadcaster = TransformBroadcaster(self) 
        self.timer=self.create_timer(0.01,self.publish_tf)

    def publish_tf(self):
        transform = TransformStamped()      
        transform.header.frame_id = 'camera_link'   # 父坐标系
        transform.child_frame_id = 'bottle_link'  # 子坐标系
        transform.header.stamp = self.get_clock().now().to_msg()

        transform.transform.translation.x=0.2
        transform.transform.translation.y=0.3
        transform.transform.translation.x=0.5

        q = quaternion_from_euler(0,0,0) 
        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.z = q[2]
        transform.transform.rotation.w = q[3]
        self.broadcaster.sendTransform(transform)
        self.get_logger().info(f'发布TF:{transform}')

def main():
    rclpy.init()
    node = TFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()

再配置setup.py

构建、配置环境，再运行

ros2 run python_tf dynamic_tf_broadcaster

可以再开一个终端查看

ros2 run tf2_ros2 tf2_echo camera_link bottle_link

查询TF关系

订阅话题，收集所有坐标系关系

新建一个tf_listener.py

import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener,Buffer      # 坐标监听器
from tf_transformations import euler_from_quaternion   # 四元素转欧拉角
import math

class TFBroadcaster(Node):
    def __init__(self):
        super().__init__('tf_broadcaster')
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer,self) 
        self.timer=self.create_timer(1.0,self.get_transform)

    def get_transform(self):
        try:
            result = self.buffer.lookup_transform('base_link','bottle_link',rclpy.time.Time(seconds=0.0),rclpy.time.Time(seconds=1.0))
            transform = result.transform
            self.get_logger().info(f'平移：{transform.translation}')
            self.get_logger().info(f'旋转：{transform.rotation}')
            rotation_eular = euler_from_quaternion(transform.rotation.x,transform.rotation.y,transform.rotation.z,transform.rotation.w)
            self.get_logger().info(f'旋转RPY：{rotation_eular}')
        except Exception as e:
            self.get_logger().info(f'报错：{str(e)}')       

def main():
    rclpy.init()
    node = TFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()

配置可执行文件

分别运行第一个（静态发布底座和摄像头）、第二个（动态发布摄像头到瓶子）、和该可执行文件（可获取底座和瓶子的坐标关系）

常用可视化工具

rqt图形化工具

rqt是ROS提供的一套图形化工具框架，本质是基于Qt开发的ROS可视化工具集合，把ROS中分散的功能（如话题查看、参数配置、日志监控等）整合到统一的图形界面中，大幅降低开发和调试成本，由多个 “插件” 组成。

安装TF树插件

打开终端，输入如下命令

sudo apt install ros-humble-rqt-tf-tree -y

更新配置文件

rm -rf ~/.config/ros/org/rqt_gui.ini

打开rqt

rqt

然后就能看到TF Tree插件

右侧就能显示当前节点的TF结构了（当前无运行，你运行之前的可执行文件就有显示了）

rviz可视化工具

rvis可以实现坐标变换的可视化

打开rviz

rviz2

点击左下角的“Add”添加可视化视图

点击“TF”，点击“OK”，就添加了坐标变换视图

ros2 bag数据记录工具

先打开一个海龟模拟器

ros2 run turtlesim turtlesim_node

再打开一个按键控制

ros2 run turtlesim turtle_teleop_key

再打开一个终端，启动记录数据工具

ros2 bag record /turtle1/cmd_vel
                # 话题名

将光标放在按键控制终端，使用“上下左右”箭头移动小海龟

接下来将三个终端都关闭，打开一个新的海归模拟器

在第三个终端输入“ls ros*”，查看所有ros文件，可以看到有两个rosbag2文件夹（我好像打开了两次），里面分别有.yaml和.db3文件

可以通过如下命令，详细查看记录里面的内容

cat rosbag2_2026_02_03-18_01_13/metadata.yaml

通过话题数据包发布话题，控制小海龟移动

ros2 bag play rosbag2_2026_02_03-18_01_13/

可以看到，和之前录入的移动数据一致

机器人建模与仿真

使用URDF创建机器人

URDF（统一机器人描述格式）使用XML（可扩展标记语言）来描述机器人的几何结构、传感器和传感器等信息，是ROS实现机器人仿真、运动规划、可视化的基础。

机器人在 URDF 中被描述为树形结构，核心层级：根连竿 (link) → 关节 (joint) → 子连杆 (link) → 关节 (joint) → ...

连杆 (link)：机器人的刚体部件（如底座、手臂、夹爪、轮子），描述部件的几何形状、质量、惯性、坐标系、可视化 / 碰撞属性。

关节 (joint)：连接两个连杆的运动副，描述关节的类型（转动 / 移动 / 固定）、运动范围、限位、原点位置、父/子连杆关系。

先安装一个URDF插件

创建工作空间，创建src文件夹，创建功能包（src目录下）

ros2 pkg create bot_description --build-type ament_cmake --license Apache-2.0

创建一个urdf文件夹，存放机器人的模型文件

新建一个first_robot.urdf文件

<?xml version="1.0"?>
<robot name="first_robot">
    <!-- 机器人的身体部分 -->
    <link name="base_link">
        <!-- 部件的外观描述 -->
        <visual>
            <!-- 沿着自己几何中心的偏移和旋转量 -->
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <!-- 几何形状 -->
            <geometry>
                <!-- 圆柱体 -->
                <cylinder radius="0.10" length="0.12"/>
            </geometry>
            <!-- 材质、颜色 -->
            <material>
                <color rgba="1.0 1.0 1.0 0.5"/>
            </material>
        </visual>
    </link>

    <!-- 机器人的IMU部件，惯性测量传感器 -->
    <link name="imu_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <!-- 正方体 -->
                <box size="0.02 0.02 0.02"/>
            </geometry>
            <material name="black">
                <color rgba="0.0 0.0 0.0 0.5"/>
            </material>
        </visual>
    </link>

    <!-- 机器人的关节 -->
    <joint name="imu_joint" type="fixed">
        <parent link="base_link"/>
        <child link="imu_link"/>
        <!-- 固定位置 -->
        <origin xyz="0.0 0.0 0.03" rpy="0.0 0.0 0.0"/>
    </joint>
</robot>

进入urdf文件夹，使用urdf_to_graphviz first_robot.urdf命令，进行结构可视化

然后就生成了.gv和.pdf两个文件

打开pdf文件就能直观查看结构

在Rviz中显示机器人

打开Rviz

Rviz2

点击左下角的“Add”，选择“RobotModel”，点击“OK”

将“描述来源”改为“File”

点击“...”，选择文件

选择“.urdf”文件，点击“open”

可以看到右侧有个小圆点，双指可放大视图（为什么是正方形？我没找到原因，伙伴们帮忙指点迷津吧）

可以看到urdf解析没问题，然后有两个报错，没有base_link/imu_link到map的TF（因为我们没有发布map坐标系）

将Fixed Frame中的map改为base_link，这样就可以得到base_link到base_link的tf了（即(0,0,0)）

robotModel默认不加载urdf里的joint关系，一般由TF发布获取（但我们不写代码发布，因为我们在urdf中已经描述了。我们通过另外两个组件加载urdf文件，去自动发布）

先安装joint-state-publisher节点和robot-state-publisher

sudo apt install ros-humble-joint-state-publisher
sudo apt install ros-humble-robot-state-publisher

在功能包下创建一个launch文件夹方便我们同时运行，创建一个.launch.py文件

打开CMakeLists.txt文件，将launch和urdf文件夹拷贝到install的share目录下

回到工作空间，colcon build构建一下

再继续编写launch文件

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    # 获取默认的urdf路径
    urdf_package_path = get_package_share_directory('bot_description')
    default_urdf_path = os.path.join(urdf_package_path, 'urdf', 'first_robot.urdf')
    # 声明参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model',default_value=default_urdf_path,description='加载的模型文件路径'
    )
    # 通过文件路径，获取内容，并转换成参数值对象
    substitutions_commend_result=launch.substitutions.Command(['cat ',launch.substitutions.LaunchConfiguration('model')])
    robot_description_value=launch_ros.parameter_descriptions.ParameterValue(substitutions_commend_result,value_type=str)
    action_robot_state_publisher=launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}]
    )
    action_joint_state_publisher=launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        parameters=[{'joint_description':robot_description_value}]
    )
    action_rviz_node=launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
    )
    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_joint_state_publisher,
        action_rviz_node,
    ])

再构建配置一下环境source，再启动

ros2 launch bot_description display_robot.launch.py

将Fixed Frame可以改为“base_link”（TF中已经有了说明）

点击“Add”，选择“RobotModel”，点击“OK”

Topic description可以选择刚刚创建的robot description

选完之后放大，可以看到右边有模型了。TF也没有报错了

可以再点击“Add”，添加“TF”模块

将“show names”打开，“Marker Scale”调小，可以发现有圆柱坐标，只是图像没颜色了（关闭时的配置都保留，不然下一次又要Add）

使用Xacro简化URDF

Xacro是基于XML的宏语言，使用它可以将部件等定义为宏，在需要时调用即可。

打开src目录下的urdf文件夹，新建一个first_robot.xacro文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="first_robot">
    <!-- 定义底座宏：参数为length和radius] -->
    <xacro:macro name="base_link" params="length radius">
        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material>
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </visual>
        </link>
    </xacro:macro>
    <!-- 定义IMU链接宏：参数为imu_name和xyz -->
    <xacro:macro name="imu_link" params="imu_name xyz">
        <link name="${imu_name}_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.02 0.02 0.02"/> 
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.5"/>
                </material>
            </visual>
        </link>

        <joint name="${imu_name}_joint" type="fixed">
            <parent link="base_link"/>
            <child link="${imu_name}_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
    </xacro:macro> 

    <!-- 调用宏 -->
    <xacro:base_link length="0.12" radius="0.1"/> 
    <xacro:imu_link imu_name="imu_up" xyz="0.0 0.0 0.03"/> 
    <xacro:imu_link imu_name="imu_down" xyz="0.0 0.0 -0.03"/>

</robot>

xacro不能直接用，还需要转换为urdf

先下载工具

sudo apt install ros-humble-xacro

然后再使用“xacro+文件路径”解析文件，转换为urdf格式

 xacro ros2/src/bot_description/urdf/first_robot.xacro

生成的就是urdf格式的

再将display_robot.launch.py里的原来的“cat”命令改为“xacro”

再构建配置，运行

ros2 launch bot_description display_robot.launch.py model:=/home/lin07/ros2/src/bot_description/urdf/first_robot.xacro
                                                       # xacro文件路径

圆柱体还是显示不了

创建机器人及传感器部件

先创建机器人的身体部分，在urdf文件夹下，新建一个base.urdf.xacro文件（是.xacro文件，中间的.urdf只是为了见名知意）

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="base_xacro" params="length radius">
        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </visual>
        </link>
    </xacro:macro>
</robot>

再创建传感器部分，在urdf文件夹下新建一个sensor文件夹，在文件夹下新建一个imu.urdf.xacro文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="imu_xacro" params="xyz">
        <link name="imu_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.02 0.02 0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.5"/>
                </material>
            </visual>
        </link>
        <joint name="imu_joint" type="fixed">
            <parent link="base_link"/>
            <child link="imu_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
    </xacro:macro>
</robot>

再创建一个camera.urdf.xacro文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="camera_xacro" params="xyz">
        <link name="camera_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.02 0.02 0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.5"/>
                </material>
            </visual>
        </link>
        <joint name="camera_joint" type="fixed">
            <parent link="base_link"/>
            <child link="camera_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
    </xacro:macro>
</robot>

再创建一个laser.urdf.xacro文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="laser_xacro" params="xyz">
        <!-- 雷达支撑杆 -->
        <link name="laser_cylinder_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.01" length="0.10"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 1.5"/>
                </material>
            </visual>
        </link>
        <!-- 雷达 -->
        <link name="laser_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.02" length="0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 1.0"/>
                </material>
            </visual>
        </link>
        <joint name="laser_joint" type="fixed">
            <parent link="laser_cylinder_link"/>
            <child link="laser_link"/>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        </joint>
        <joint name="laser_cylinder_joint" type="fixed">
            <parent link="base_link"/>
            <child link="laser_cylinder_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
    </xacro:macro>
</robot>

在urdf文件夹下创建一个bot.urdf.xacro文件，进行组装

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

<xacro:include filename="$(find bot_description)/urdf/base.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/sensor/imu.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/sensor/camera.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/sensor/laser.urdf.xacro"/>
<!-- 调用宏 -->
<xacro:base_xacro length="0.12" radius="0.10"/>
<xacro:imu_xacro xyz="0.0 0.0 0.02"/>
<xacro:camera_xacro xyz="0.10 0.0 0.075"/>
<xacro:laser_xacro xyz="0.0 0.0 0.10"/>
</robot>

构建配置，运行

ros2 launch bot_description display_robot.launch.py model:=/home/lin07/ros2/src/bot_description/urdf/bot.urdf.xacro

哇有了有了（家人们，我第一次打开没有形状只有坐标系，重新运行了一次就有了）（我回去试过了，就是之前不行现在行，我也不知道怎么回事了）

完善机器人执行器部件

在urdf下新建一个actuator文件夹，在actuator目录下新建一个wheel.urdf.xacro文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="wheel_xacro" params="wheel_name xyz">
        <link name="${wheel_name}_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="1.57079 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.032" length="0.04"/>
                </geometry>
                <material name="yellow">
                    <color rgba="1.0 1.0 0.0 0.8"/>
                </material>
            </visual>
        </link>
        <joint name="${wheel_name}_joint" type="continuous">
            <parent link="base_link"/>
            <child link="${wheel_name}_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
            <axis xyz="0 1 0"/>
        </joint>
    </xacro:macro>
</robot>

在新建一个万向轮文件caster.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="caster_xacro" params="caster_name xyz">
        <link name="${caster_name}_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <sphere radius="0.016"/>
                </geometry>
                <material name="yellow">
                    <color rgba="1.0 1.0 0.0 0.8"/>
                </material>
            </visual>
        </link>
        <joint name="${caster_name}_joint" type="fixed">
            <parent link="base_link"/>
            <child link="${caster_name}_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
    </xacro:macro>
</robot>

再更新bot.urdf.xacro文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="bot">
<!-- 基础部分 -->
<xacro:include filename="$(find bot_description)/urdf/base.urdf.xacro"/>
<!-- 传感器部分 -->
<xacro:include filename="$(find bot_description)/urdf/sensor/imu.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/sensor/camera.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/sensor/laser.urdf.xacro"/>
<!-- 执行器部分 -->
<xacro:include filename="$(find bot_description)/urdf/actuator/wheel.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/actuator/caster.urdf.xacro"/>
<!-- 调用宏 -->
<xacro:base_xacro length="0.12" radius="0.10"/>
<xacro:imu_xacro xyz="0.0 0.0 0.02"/>
<xacro:camera_xacro xyz="0.10 0.0 0.075"/>
<xacro:laser_xacro xyz="0.0 0.0 0.10"/>

<xacro:wheel_xacro wheel_name="left_wheel" xyz="0 0.10 -0.06"/>
<xacro:wheel_xacro wheel_name="right_wheel" xyz="0 -0.10 -0.06"/>
<xacro:caster_xacro caster_name="front_caster" xyz="0.08 0.0 -0.076"/>
<xacro:caster_xacro caster_name="back_caster" xyz="-0.08 0.0 -0.076"/>
</robot>

添加虚拟部件，使轮子贴合地面

打开base.urdf.xacro文件进行修改

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="base_xacro" params="length radius">
        <!-- 虚拟部件，base投影到地面的点 -->
        <link name="base_footprint"/>
        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </visual>
        </link>
        <joint name="joint_name" type="fixed">
            <origin xyz="0.0 0.0 ${length/2.0+0.032-0.001}" rpy="0.0 0.0 0.0"/>
            <parent link="base_footprint"/>
            <child link="base_link"/>
        </joint>
    </xacro:macro>
</robot>

构建配置运行

将fixed frame选为“base_footprint”（原点）

可以看到轮子紧贴地面了

添加物理属性让机器人更真实

添加碰撞属性

修改base.urdf.xacro文件，给base_link添加碰撞属性

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="base_xacro" params="length radius">
        <!-- 虚拟部件，base投影到地面的点 -->
        <link name="base_footprint"/>
        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </visual>
            <collision>
                <!-- 将visual里面的内容拷贝一份，有时也可用规则图形近似不规则图形 -->
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </collision>
        </link>
        <joint name="joint_name" type="fixed">
            <origin xyz="0.0 0.0 ${length/2.0+0.032-0.001}" rpy="0.0 0.0 0.0"/>
            <parent link="base_footprint"/>
            <child link="base_link"/>
        </joint>
    </xacro:macro>
</robot>

用同样的方法分别给laser等三个传感器、两个执行器添加碰撞属性（下面仅展示laser）

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="laser_xacro" params="xyz">
        <!-- 雷达支撑杆 -->
        <link name="laser_cylinder_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.01" length="0.10"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.5"/>
                </material>
            </visual>
            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.01" length="0.10"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.5"/>
                </material>
            </collision>
        </link>
        <!-- 雷达 -->
        <link name="laser_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.02" length="0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 1.0"/>
                </material>
            </visual>
            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.02" length="0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 1.0"/>
                </material>
            </collision>
        </link>
        <joint name="laser_joint" type="fixed">
            <parent link="laser_cylinder_link"/>
            <child link="laser_link"/>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        </joint>
        <joint name="laser_cylinder_joint" type="fixed">
            <parent link="base_link"/>
            <child link="laser_cylinder_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
        <gazebo reference="laser_cylinder_link">
            <material>Gazebo/Black</material>
        </gazebo>
        <gazebo reference="laser_link">
            <material>Gazebo/Black</material>
        </gazebo>
    </xacro:macro>
</robot>

再重新构建配置，运行

取消勾选visual Enabled，点击“Collision Enabled”，可以看出碰撞可视化与机器人可视化一致

添加质量和惯性

质量可以用千克表示，而旋转惯性需要使用一个3*3的矩阵表述

在urdf目录下新建一个common_inertial.xacro文件（不用记，直接让豆包写就好了）

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.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.0" ixz="0.0" iyy="${m / 12 * (3*r*r + h*h)}" iyz="0.0" izz="${m / 2 * r*r}"/>
        </inertial>
    </xacro:macro>
</robot>

在base.urdf.xacro文件中引入刚刚创建的惯性宏（如下就添加好了惯性矩阵）

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find bot_description)/urdf/common_inertial.xacro"/>
    <xacro:macro name="base_xacro" params="length radius">
        <!-- 虚拟部件，base投影到地面的点 -->
        <link name="base_footprint"/>
        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </visual>
            <collision>
                <!-- 将visual里面的内容拷贝一份，有时也可用规则图形近似不规则图形 -->
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </collision>
            <xacro:cylinder_inertia m="1.0" r="${radius}" h="${length}"/>
        </link>
        <joint name="joint_name" type="fixed">
            <origin xyz="0.0 0.0 ${length/2.0+0.032-0.001}" rpy="0.0 0.0 0.0"/>
            <parent link="base_footprint"/>
            <child link="base_link"/>
        </joint>
    </xacro:macro>
</robot>

构建配置，运行

关闭visual Enabled，打开Mass Properties里的Mass，这样就可以在rviz中的质量视角，再点下imertial就能可视化惯性

其它的部件同理添加质量和惯性属性，不再一一演示

在Gazebo中完成机器人仿真

在RViz2中可以看机器人长啥样，在Gazebo中可以让机器人在虚拟世界里动起来

安装Gazebo

sudo apt install gazebo

下载模型

mkdir -p ~/.gazebo
cd ~/.gazebo
git clone https://gitee.com/ohhuo/gazebo_models.git models
rm -rf ~/.gazebo/models/.git

在终端中输入gazebo就能打开gazebo了

可以点击左上角的图形，添加图形（shift+左键可以旋转视角）

右键可以Delete删除

也可以手动画墙

点击左上角的“Edit”，选择“Building Editor”，点击“Wall”

然后在右侧白色方格作画，就能画出指定形状的墙

保存构建

画好之后，点击左上角的“File”，退出文件

选择第一个，“保存并退出”

新建一个world文件夹存储刚刚的世界文件

命名为room

点击Insert，还可以选择插入各种各样的物品

点击左上角的移动图标，拖拉三个方向箭头，可分别向三个方向移动，将房间移动到靠近原点的位置（加载机器人时，机器人将出现在中间位置）

编辑完之后，点击左上角“File”，选择“Save World As”

以.world后缀保存文件

可以发现，sdf文件是urdf文件的扩展

在gazebo中加载机器人模型

需要将机器人建模使用的urdf格式转换成sdf格式

安装功能包

sudo apt install ros-humble-gazebo-ros-pkgs

在launch目录下新建一个gazebo_sim.launch.py文件

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    urdf_package_path = get_package_share_directory('bot_description')
    default_xacro_path = os.path.join(urdf_package_path, 'urdf', 'bot.urdf.xacro')
    default_gazebo_world_path=os.path.join(urdf_package_path,'world','custom_room.world')
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model',default_value=default_xacro_path,description='加载的模型文件路径'
    )
    substitutions_commend_result=launch.substitutions.Command(['xacro ',launch.substitutions.LaunchConfiguration('model')])
    robot_description_value=launch_ros.parameter_descriptions.ParameterValue(substitutions_commend_result,value_type=str)
    action_robot_state_publisher=launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}]
    )
    action_launch_gazebo=launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            [get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']
        ),
        [('world', default_gazebo_world_path), ('verbose', 'true')]
    )
    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_launch_gazebo,
    ])

在CMakeLists.txt中添加拷贝

构建配置，运行

ros2 launch bot_description gazebo_sim.launch.py

能成功打开

现在我们将机器人加载到世界中

更新gazebo_sim.launch.py文件

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    urdf_package_path = get_package_share_directory('bot_description')
    default_xacro_path = os.path.join(urdf_package_path, 'urdf', 'bot.urdf.xacro')
    default_gazebo_world_path=os.path.join(urdf_package_path,'world','custom_room.world')
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model',default_value=default_xacro_path,description='加载的模型文件路径'
    )
    substitutions_commend_result=launch.substitutions.Command(['xacro ',launch.substitutions.LaunchConfiguration('model')])
    robot_description_value=launch_ros.parameter_descriptions.ParameterValue(substitutions_commend_result,value_type=str)
    action_robot_state_publisher=launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}]
    )
    action_launch_gazebo=launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            [get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']
        ),
        launch_arguments=[('world', default_gazebo_world_path),('verbose', 'true')]
    )
    action_spawn_entity=launch_ros.actions.Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=['-topic','/robot_description','-entity','bot']
    )
    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_launch_gazebo,
        action_spawn_entity
    ])

构建配置，运行

 ros2 launch bot_description gazebo_sim.launch.py 

能正常显示机器人

使用gazebo标签扩展urdf

可改变机器人在gazebo中的颜色等各属性（摩擦力）

打开urdf/sensor下的laser.urdf.xacro，加上gazebo标签

构建配置，运行

可以看到雷达和雷达支撑杆的颜色成功变为了黑色

使用两轮差速插件控制机器人

在plugin标签中使用两轮差速插件

在urdf目录下新建一个plugins文件夹，在里面创建gazebo_control_plugin.xacro文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 声明宏-->
    <xacro:macro name="gazebo_control_plugin">
        <gazebo>
            <!-- 插件标签：插件名 插件文件名字 -->
            <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
                <ros>
                    <namespace>/</namespace>
                    <remapping>cmd_vel:=cmd_vel</remapping>
                    <remapping>odom:=odom</remapping>
                </ros>
                <update_rate>30</update_rate>
                <left_joint>left_wheel_joint</left_joint>
                <right_joint>right_wheel_joint</right_joint>
                <wheel_separation>0.2</wheel_separation>
                <wheel_radius>0.064</wheel_radius>
                <max_vel>20</max_vel>
                <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>
    </xacro:macro>
</robot>

然后在bot.urdf.xacro文件中引入插件并调用

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="bot">
<!-- 基础部分 -->
<xacro:include filename="$(find bot_description)/urdf/base.urdf.xacro"/>
<!-- 传感器部分 -->
<xacro:include filename="$(find bot_description)/urdf/sensor/imu.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/sensor/camera.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/sensor/laser.urdf.xacro"/>
<!-- 执行器部分 -->
<xacro:include filename="$(find bot_description)/urdf/actuator/wheel.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/actuator/caster.urdf.xacro"/>
<!-- 插件 -->
<xacro:include filename="$(find bot_description)/urdf/plugins/gazebo_control_plugin.xacro"/>
<!-- 调用宏 -->
<xacro:base_xacro length="0.12" radius="0.10"/>
<xacro:imu_xacro xyz="0.0 0.0 0.02"/>
<xacro:camera_xacro xyz="0.10 0.0 0.075"/>
<xacro:laser_xacro xyz="0.0 0.0 0.10"/>

<xacro:wheel_xacro wheel_name="left_wheel" xyz="0 0.10 -0.06"/>
<xacro:wheel_xacro wheel_name="right_wheel" xyz="0 -0.10 -0.06"/>
<xacro:caster_xacro caster_name="front_caster" xyz="0.08 0.0 -0.076"/>
<xacro:caster_xacro caster_name="back_caster" xyz="-0.08 0.0 -0.076"/>

<xacro:gazebo_control_plugin/>
</robot>

大家可以再检测以下轮子的代码

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="wheel_xacro" params="wheel_name xyz">
        <link name="${wheel_name}_link">
            <!-- 可视化部分（RViz/Gazebo显示用） -->
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="1.57079 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.032" length="0.04"/>
                </geometry>
                <material name="yellow">
                    <color rgba="1.0 1.0 0.0 0.8"/>
                </material>
            </visual>

            <!-- 碰撞部分（Gazebo物理碰撞用） -->
            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="1.57079 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.032" length="0.04"/>
                </geometry>
            </collision>

            <!-- 新增：惯性属性（Gazebo必需，没有则轮子不显示） -->
            <inertial>
                <mass value="0.1"/> <!-- 轮子质量0.1kg，可根据实际调整 -->
                <!-- 圆柱型轮子的惯性矩阵（适配半径0.032、长度0.04的尺寸） -->
                <inertia ixx="0.00002" ixy="0.0" ixz="0.0"
                         iyy="0.00001" iyz="0.0"
                         izz="0.00002"/>
            </inertial>
        </link>

        <!-- 关节定义（原代码无问题，保留） -->
        <joint name="${wheel_name}_joint" type="continuous">
            <parent link="base_link"/>
            <child link="${wheel_name}_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
            <axis xyz="0 1 0"/>
            <!-- 新增：关节物理参数，防止Gazebo中关节异常 -->
            <limit effort="50" velocity="50"/>
            <dynamics damping="0.01" friction="0.01"/>
        </joint>

        <!-- 新增：Gazebo专属配置，优化轮子显示和物理表现 -->
        <gazebo reference="${wheel_name}_link">
            <!-- 指定Gazebo内置材质，兼容更好 -->
            <material>Gazebo/Yellow</material>
            <!-- 禁用自动惯性计算，使用我们定义的inertial -->
            <auto_inertial>false</auto_inertial>
        </gazebo>
    </xacro:macro>
</robot>

再构建配置，运行

ros2 launch bot_description gazebo_sim.launch.py

为了方便显示，我把之前的方块删除了

在右侧可以看到有一个plugins

再打开一个终端，使用键盘控制节点控制机器人移动

ros2 run teleop_twist_keyboard teleop_twist_keyboard 

光标在此终端，我们就可以使用ijkl等进行移动

可能是我没设置摩擦力，前后移动比较缓慢，JK控制左右比较明细

也可以打开rviz查看TF移动

激光雷达传感器仿真

在plugins目录下新建gazebo_sensor_plugin.xacro文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 声明宏-->
    <xacro:macro name="gazebo_sensor_plugin">
        <!-- 参考关节（把哪个关节视为雷达） -->
        <gazebo reference="laser_link">
            <sensor name="laserscan" type="ray">
                <plugin name="laser_plugin" filename="libgazebo_ros_ray_sensor.so">
                    <ros>
                        <namespace>/</namespace>
                        <remapping>~/out:=scan</remapping>
                    </ros>
                    <output_type>sensor_msgs/LaserScan</output_type>
                    <frame_id>laser_link</frame_id>
                </plugin>
                <always_on>true</always_on>
                <visualize>true</visualize>
                <update_rate>10</update_rate>
                <pose>0 0 0 0 0 0</pose>
                <ray>
                    <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>8.0</max>
                        <resolution>0.015</resolution>
                    </range>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.01</stddev>
                    </noise>
                </ray>
            </sensor>
        </gazebo>
    </xacro:macro>
</robot>

然后在bot.urdf.xacro中引入插件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="bot">
<!-- 基础部分 -->
<xacro:include filename="$(find bot_description)/urdf/base.urdf.xacro"/>
<!-- 传感器部分 -->
<xacro:include filename="$(find bot_description)/urdf/sensor/imu.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/sensor/camera.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/sensor/laser.urdf.xacro"/>
<!-- 执行器部分 -->
<xacro:include filename="$(find bot_description)/urdf/actuator/wheel.urdf.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/actuator/caster.urdf.xacro"/>
<!-- 插件 -->
<xacro:include filename="$(find bot_description)/urdf/plugins/gazebo_control_plugin.xacro"/>
<xacro:include filename="$(find bot_description)/urdf/plugins/gazebo_sensor_plugin.xacro"/>
<!-- 调用宏 -->
<xacro:base_xacro length="0.12" radius="0.10"/>
<xacro:imu_xacro xyz="0.0 0.0 0.02"/>
<xacro:camera_xacro xyz="0.10 0.0 0.075"/>
<xacro:laser_xacro xyz="0.0 0.0 0.10"/>

<xacro:wheel_xacro wheel_name="left_wheel" xyz="0 0.10 -0.06"/>
<xacro:wheel_xacro wheel_name="right_wheel" xyz="0 -0.10 -0.06"/>
<xacro:caster_xacro caster_name="front_caster" xyz="0.08 0.0 -0.076"/>
<xacro:caster_xacro caster_name="back_caster" xyz="-0.08 0.0 -0.076"/>

<xacro:gazebo_control_plugin/>
<xacro:gazebo_sensor_plugin/>
</robot>

然后构建，source，运行命令

ros2 launch bot_description gazebo_sim.launch.py

下图蓝色的就是射出的雷达

scan话题就是我们发布的激光数据，如果没有显示，看看报错，可能是没装插件包

sudo apt install ros-humble-gazebo-ros-pkgs

再开一个终端，source配置，输入rviz2，来对激光数据进行可视化

点击“Add”，在“By topic”里选择“/scan”，点击“OK”

固定位置选择“odom”

点云大小可以自行调整

这样就算完成了对激光雷达的仿真

惯性测量传感器仿真

imu是一个集成了多个惯性传感器的设备，可以得到设备的姿态变化信息

在之前的gazebo_sensor_plugin.xacro文件中加入imu相关的gazebo标签

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 声明宏-->
    <xacro:macro name="gazebo_sensor_plugin">
        <!-- 参考关节（把哪个关节视为雷达） -->
        <gazebo reference="laser_link">
            <sensor name="laserscan" type="ray">
                <plugin name="laser_plugin" filename="libgazebo_ros_ray_sensor.so">
                    <ros>
                        <namespace>/</namespace>
                        <remapping>~/out:=scan</remapping>
                    </ros>
                    <output_type>sensor_msgs/LaserScan</output_type>
                    <frame_id>laser_link</frame_id>
                </plugin>
                <always_on>true</always_on>
                <visualize>true</visualize>
                <update_rate>10</update_rate>
                <pose>0 0 0 0 0 0</pose>
                <ray>
                    <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>8.0</max>
                        <resolution>0.015</resolution>
                    </range>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.01</stddev>
                    </noise>
                </ray>
            </sensor>
        </gazebo>

        <gazebo reference="imu_link">
            <sensor name="imu_sensor" type="imu">
                <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
                    <ros>
                        <namespace>/</namespace>
                        <remapping>~/out:=imu</remapping>
                    </ros>
                    <initial_orientation_as_reference>false</initial_orientation_as_reference>
                </plugin>
                <update_rate>100</update_rate>
                <always_on>true</always_on>
                <imu>
                    <angular_velocity>
                        <x>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>2e-4</stddev>
                                <bias_mean>0.0000075</bias_mean>
                                <bias_stddev>0.0</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.0</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.0</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>
    </xacro:macro>
</robot>

再构造、配置，运行仿真命令

ros2 launch bot_description gazebo_sim.launch.py 

再打开一个终端，查看话题（第三个就是）

深度相机传感器仿真

深度相机使用时应该将前方设为z轴，在urdf中要添加一个虚拟的部件来调整方位

打开sensor文件夹中的camera.urdf.xacro文件，进行如下修改

打开gazebo_sensor_plugin.xacro文件，添加相机的gazebo标签

        <gazebo reference="camera_link">
            <sensor name="camera" type="depth">
                <plugin name="depth_camera" filename="libgazebo_ros_camera.so">
                    <frame_name>camera_optical_link</frame_name>
                    <camera_name>camera</camera_name>
                </plugin>
                <always_on>true</always_on>
                <update_rate>10</update_rate>
                <camera name="camera">
                    <horizontal_fov>1.50098</horizontal_fov>
                    <image>
                        <width>800</width>
                        <height>600</height>
                        <format>R8G8B8</format>
                    </image>
                    <depth_camera>
                        <width>800</width>
                        <height>600</height>
                        <format>DEPTH_32BIT</format>
                    </depth_camera>
                    <distortion>
                        <k1>0.0</k1>
                        <k2>0.0</k2>
                        <p1>0.0</p1>
                        <p2>0.0</p2>
                        <center>0.5 0.5</center>
                    </distortion>
                    <clip>
                        <near>0.1</near>
                        <far>10.0</far>
                    </clip>
                </camera>
            </sensor>
        </gazebo>

再构建、配置，运行

ros2 launch bot_description gazebo_sim.launch.py

再开一个终端，看一下和camera相关的话题

打开rviz2，更换固定坐标系

点击“Add”，添加“By topic”中的点云

可以看到前方的墙

我把小车移动开了一点，现在真的展示摄像头视角的墙了

我们也可以打开rqt查看彩色图和深度图

打开两个“Plugins”中的Image view

分别选择image_raw和depth/image_raw查看

使用ros2_control驱动机器人

gazebo中的仿真传感器与其它防止模拟世界、真实世界中的接口不一致，所以我们又引入ros2_control控制框架，统一硬件与控制器的交互。

ROS 2 Control是ROS2中用于机器人硬件控制的核心框架，核心作用是解析上层速度指令→转换成电机能识别的控制信号→同时反馈硬件状态。

由硬件接口、控制器、控制器管理器3部分组成

安装ros2 control

sudo apt install ros-humble-ros2-control

安装控制器

sudo apt install ros-humble-ros2-controllers

使用gazebo接入ros2_control

安装gazebo ros2 control插件

sudo apt install ros-humble-gazebo-ros2-control

在urdf目录下新建一个bot.ros2_control.xacro文件

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

    <xacro:macro name="bot_ros2_control">
        <ros2_control name="BotGazeboSystem" type="system">
            <hardware>
                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
            </hardware>

            <joint name="left_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>

            <joint name="right_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
        </ros2_control>

        <gazebo>
            <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
                <parameters>$(find bot_description)/config/bot_ros2_controller.yaml</parameters>
            </plugin>
        </gazebo>
    </xacro:macro>

</robot>

在bot_description目录下新建一个config文件夹，在里面新建一个bot_ros2_controller.yaml文件

controller_manager:
  ros__parameters:
    update_rate: 100
    use_sim_time: true

在CMakeList.txt中添加该config文件夹的构建

在bot.urdf.xacro文件中调用宏

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="bot">
    <xacro:include filename="$(find bot_description)/urdf/base.urdf.xacro"/>
    <!-- 传感器部分 -->
    <xacro:include filename="$(find bot_description)/urdf/sensor/imu.urdf.xacro"/>
    <xacro:include filename="$(find bot_description)/urdf/sensor/camera.urdf.xacro"/>
    <xacro:include filename="$(find bot_description)/urdf/sensor/laser.urdf.xacro"/>
    
    <!-- 执行器部分 -->
    <xacro:include filename="$(find bot_description)/urdf/actuator/wheel.urdf.xacro"/>
    <xacro:include filename="$(find bot_description)/urdf/actuator/caster.urdf.xacro"/>
    
    <!-- 插件部分：删除冲突的 gazebo_control_plugin include -->
    <!-- <xacro:include filename="$(find bot_description)/urdf/plugins/gazebo_control_plugin.xacro"/> -->
    <xacro:include filename="$(find bot_description)/urdf/bot.ros2_control.xacro"/>

    <!-- 宏调用：顺序不变 -->
    <xacro:base_xacro length="0.12" radius="0.10"/>
    <xacro:imu_xacro xyz="0.0 0.0 0.02"/>
    <xacro:camera_xacro xyz="0.10 0.0 0.075"/>
    <xacro:laser_xacro xyz="0.0 0.0 0.10"/>

    <xacro:wheel_xacro wheel_name="left_wheel" xyz="0 0.10 -0.06"/>
    <xacro:wheel_xacro wheel_name="right_wheel" xyz="0 -0.10 -0.06"/>
    <xacro:caster_xacro caster_name="front_caster" xyz="0.08 0.0 -0.076"/>
    <xacro:caster_xacro caster_name="back_caster" xyz="-0.08 0.0 -0.076"/>

    <!-- 调用 ros2_control 宏 -->
    <xacro:bot_ros2_control/>
</robot>

再构建配置，运行

ros2 launch bot_description gazebo_sim.launch.py 

可以使用命令查看一下已经启动的控制器

使用关节状态发布控制器

打开rviz2，可以看到机器人模型的轮子位置不对，没有TF变换（因为之前使用的gazebo_control_plugin里面有wheel_tf的发布）

而在ros2_control中我们可以使用关节状态发布器发布话题

更新.yaml文件，添加控制器

更改gazebo_sim.launch.py文件，自动加载和激活控制器

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
import os

def generate_launch_description():
    urdf_package_path = get_package_share_directory('bot_description')
    default_xacro_path = os.path.join(urdf_package_path, 'urdf', 'bot.urdf.xacro')
    default_gazebo_world_path = os.path.join(urdf_package_path, 'world', 'custom_room.world')

    declare_model_arg = launch.actions.DeclareLaunchArgument(
        name='model',
        default_value=default_xacro_path,
        description='加载的模型文件路径'
    )

    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(['xacro ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str
    )

    robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )

    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
        ),
        launch_arguments={
            'world': default_gazebo_world_path,
            'verbose': 'true'
        }.items()
    )

    spawn_entity = launch_ros.actions.Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=['-topic', '/robot_description', '-entity', 'bot'],
        output='screen'
    )

    action_load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', 'bot_joint_state_broadcaster', '--set-state', 'active'],
        output='screen'
    )

    return launch.LaunchDescription([
        declare_model_arg,
        robot_state_publisher,
        gazebo,
        spawn_entity,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=spawn_entity,
                on_exit=[action_load_joint_state_controller],
            )
        )
    ])

构建配置运行，看终端1和3，可以发现成功发布

rviz中的轮子也正常了

使用力控制器控制轮子

在.yaml文件中添加控制器

在launch中加载激活

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
import os

def generate_launch_description():
    urdf_package_path = get_package_share_directory('bot_description')
    default_xacro_path = os.path.join(urdf_package_path, 'urdf', 'bot.urdf.xacro')
    default_gazebo_world_path = os.path.join(urdf_package_path, 'world', 'custom_room.world')

    declare_model_arg = launch.actions.DeclareLaunchArgument(
        name='model',
        default_value=default_xacro_path,
        description='加载的模型文件路径'
    )

    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(['xacro ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str
    )

    robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )

    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
        ),
        launch_arguments={
            'world': default_gazebo_world_path,
            'verbose': 'true'
        }.items()
    )

    spawn_entity = launch_ros.actions.Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=['-topic', '/robot_description', '-entity', 'bot'],
        output='screen'
    )

    action_load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', 'bot_joint_state_broadcaster', '--set-state', 'active'],
        output='screen'
    )

    action_load_effort_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', 'bot_effort_controller', '--set-state', 'active'],
        output='screen'
    )

    return launch.LaunchDescription([
        declare_model_arg,
        robot_state_publisher,
        gazebo,
        spawn_entity,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=spawn_entity,
                on_exit=[action_load_joint_state_controller],
            )
        ),
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_load_joint_state_controller,
                on_exit=[action_load_effort_controller],
            )
        )
    ])

构建配置运行，可以使用如下命令查看与力相关的话题

给左右轮发布一个微小的力

可以看到小车在缓缓移动了

使用两轮差速控制器控制机器人

状态发布控制器和力控制器只是对状态接口的简单第哦啊有，两轮差速控制器不仅是数据转发，还涉及到了运动学计算，从而得到里程计信息和两个轮子的目标速度

在.yaml文件中添加控制器

controller_manager:
  ros__parameters:
    update_rate: 100  
    use_sim_time: true

    bot_joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
      use_sim_time: true
    bot_effort_controller:
      type: effort_controllers/JointGroupEffortController
    bot_diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

bot_effort_controller:
  ros__parameters:
    joints:
      - left_wheel_joint
      - right_wheel_joint
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort


bot_diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]

    wheel_separation: 0.20
    wheel_radius: 0.032

    wheel_separation_multiplier: 1.0
    left_wheel_radius_multiplier: 1.0
    right_wheel_radius_multiplier: 1.0

    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_footprint
    pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
    twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]

    open_loop: true
    enable_odom_tf: true

    cmd_vel_timeout: 0.5
    #publish_limited_velocity: true
    use_stamped_vel: false
    #velocity_rolling_window_size: 10

在gazebo_sim.launch.py中加载激活（删掉力控制器，两者都是控制轮子的）

在重映射，修改控制器话题的名字（第33、34行）

构建配置运行，可以看到发布的话题里有odom和cmd_vel

键盘控制节点默认发布的话题就是cmd_vel，我们可以通过键盘控制节点控制小车移动

ros2 run teleop_twist_keyboard teleop_twist_keyboard

我试过了很多方法都不能正常控制运动，我也不知道问题在哪了，可能是环境已经不适应了吧。家人们如果找到解决方法了欢迎分享哦