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”

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

rviz默认不加载urdf里的joint关系，一般从TF发布获取

先安装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,
    ])

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

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文件

<?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中完成机器人仿真

安装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']
        ),
        launch_arguments=[{'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等进行移动，w/x进行加减速

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

也可以打开rviz查看TF移动

使用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">-1</param>
                    <param name="max">1</param>
                </command_interface>
                <command_interface name="effort">
                    <param name="min">-0.1</param>
                    <param name="max">0.1</param>
                </command_interface>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>
            <joint name="right_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-1</param>
                    <param name="max">1</param>
                </command_interface>
                <command_interface name="effort">
                    <param name="min">-0.1</param>
                    <param name="max">0.1</param>
                </command_interface>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>
        </ros2_control>
        
        <gazebo>
            <plugin filename="/opt/ros/humble/lib/libgazebo_ros2_control.so" name="gazebo_ros2_control">
                <parameters>package://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

在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

再打开一个终端，使用“ros2 control list_hardware_interfaces”命令列出所有硬件接口

我就到此结束了先，因为我肯定是会在硬件上操作的，这篇写的不是很好，大家嘴下留情。

# 人工智能 # xml
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

官网知识库结构化整理指南

avatar DAMO开发者矩阵
cover

RoboMIND 2.0：用于通用具身智能的多模态、双手移动操作数据集（上）

avatar DAMO开发者矩阵

从可观测性的角度研究基于扩展卡尔曼滤波器（EKF）的同时定位与地图构建（SLAM）中的不一致性问题附Matlab代码

在机器人技术与自动驾驶领域，同时定位与地图构建（SLAM）技术是实现机器人自主导航的核心。机器人需要在未知环境中实时确定自身位置，并构建周围环境的地图，以便做出合理的决策和规划路径。基于扩展卡尔曼滤波器（EKF）的 SLAM 是一种经典且广泛应用的方法，然而，该方法存在不一致性问题，即估计的机器人位姿和地图与真实情况的偏差会随时间逐渐累积，严重影响系统性能。从可观测性角度研究这一问题，为深入理解和

avatar DAMO开发者矩阵