在上一节Gazebo 仿真中， 我们通过ros2_control 让模型动了起来，这属于一种真实的物理仿真。但在这节中，我们不打算使用Gazebo仿真。而是跳过Gazebo使用纯数学模拟 方式，让在RVIZ 中动起来。目的是熟悉robot_state_publisher。

1. RVIZ中的模型靠什么动起来？

RViz 本身是“被动”的，它自己不会让任何东西动起来。 它的运动完全依赖于外部输入的数据。

可以把 RViz 想象成一个 极其复杂的“视频播放器”，而整个 ROS 系统就是 “实时制片团队”。RViz 只是忠实地将收到的数据渲染到屏幕上。

RViz 的动画效果需要两个不断更新的数据流，下图清晰地展示了其依赖关系和数据流动路径：

从上图可以看出，RViz 依赖于两个核心的、持续更新的数据源：

1.1 坐标变换数据 (/tf / /tf_static)

这是让机器人模型“整体动起来”的绝对前提。

• 是什么：一个实时更新的坐标变换树，定义了机器人每个部件（连杆）在三维空间中的位置和姿态关系。
• 谁提供：主要由 robot_state_publisher 节点 发布。该节点将URDF模型和关节状态作为输入，输出所有连杆的变换。
• RViz 怎么做：RViz 内部有一个 TF 监听器，它订阅 /tf 话题，并持续维护着一个最新的坐标变换树。当它收到一个新的变换时，它会立即用这个新位置去更新3D视图里对应的机器人部件。

没有持续更新的 TF 数据，RViz 中的机器人模型就是一堆“僵硬的、不会动的积木”。

1.2 数据解析

• 通过订阅 /robot_description 话题或从参数服务器读取 robot_description 参数，加载并显示 URDF 模型。
• 订阅/pointcloud 显示来自深度相机、激光雷达、3D 扫描仪的点云数据。
• 通过sensor_msgs/LaserScan 显示2D 激光雷达的数据
• 显示来自摄像头的视频流。
• 显示机器人与导航信息等

---

2. 关于robot_state_publisher

robot_state_publisher 的根本任务是：
1. 读取 URDF 模型（静态机器人结构）
2. 订阅 /joint_states 话题（实时关节角度）
3. 计算并发布两类信息：
- /tf 话题： 动态 TF 变换（每帧更新，随关节变化）
- /tf_static 话题： 静态 TF 变换（固定链接，如 base → sensor）
4. 同时发布 /robot_description 参数（完整 URDF 字符串，供其他节点使用）。

对于不同的机器人，robot_state_publisher 节点是完全一样的、可复用的二进制程序。

2.1 运行robot_state_publisher节点

命令行运行如下：

robot_state_publisher /path/to/urdf/file.urdf.xacro

2.2 谁定义了ROS2 中的坐标系？

URDF 中的 <link> 标签，就是机器人系统中每个坐标系的定义。
每一个 <link> 代表一个刚体坐标系 , name 表示坐标系的名字。

2.2.1 <link> ≠ 实体，= 坐标系！

<link> 不是几何体的定义，而是坐标系的定义。
几何体（geometry）是由 <visual> 和 <collision> 元素定义的，而不是 <link> 本身。
<link> 的name并没有什么特殊限制，你写什么，它就认什么。
常见坐标系命名规范（REP-105）—— 不是强制，但强烈推荐：

坐标系	推荐名称	主要用途
机器人底盘中心	base_link	机器人本体的主坐标系，所有传感器、部件都以它为参考。
地面投影坐标系	base_footprint	机器人在地面上的投影坐标系，去除 Z 轴高度，用于 2D 导航与路径规划。
里程计坐标系	odom	由里程计计算的坐标系，连续、平滑，但会随时间产生漂移。
全局地图坐标系	map	固定的全局参考坐标系，无漂移，用于定位与全局导航。
激光雷达坐标系	laser / lidar_link	激光雷达传感器自身的坐标系，用于描述雷达数据相对于机器人本体的位置与方向。

2.2.2 <joint> 是“坐标系之间的关系说明书”

<joint> 描述了坐标系之间的关系。 origin 元素给出坐标系之间位姿变换（平移 + 旋转）。
让我们给出一个最简单的双连杆机器人：

<link name="base_link"/>   <!-- 定义底盘坐标系 -->
<link name="arm_link"/>    <!-- 定义ARM坐标系 -->

<joint name="shoulder_joint" type="revolute">
  <parent link="base_link"/> <!-- 父坐标系是base_link，根坐标系 -->
  <child link="arm_link"/>  <!-- 子坐标系是arm_link -->
  <!-- arm_link坐标系原点 位于 base_link 上方（z轴） 0.5m，
   相对于父坐标系，没有旋转-->
  <origin xyz="0 0 0.5" rpy="0 0 0"/>
  <!-- 关节的旋转轴（或移动轴）是沿 Z 轴正方向。-->
  <axis xyz="0 0 1"/>
</joint>

在 URDF（Unified Robot Description Format） 中：

概念	对应元素	实质含义
<link>	<link name="...">	坐标系（刚体的参考系），不是几何“实体”本身
<visual> / <collision>	<link> 内的子元素	几何体（实体外观 / 碰撞体），只是相对于该 link 坐标系定义的形状
<joint>	<joint name="...">	连接两个坐标系（link）之间的约束关系，定义它们的相对运动与位置
<origin xyz="" rpy=""/>	<joint> 或 <visual> / <collision> 内	坐标系之间的位姿变换（平移 + 旋转）

所以，总结起来就是：

• 坐标系 → URDF 里 定义
• 变换关系 → robot_state_publisher 实时发布
• robot_state_publisher 必须运行，否则：
  • 看不到完整的 TF 树
  • 看不到 /robot_description 参数
  • RViz 里机器人模型不显示或“漂移”

---

3. 关于static_transform_publisher

robot_state_publisher 会解析URDF 中的所有坐标系，并将其发布。对于URDF 没有描述到的坐标系关系， 我们通常使用static_transform_publisher来发布。它会将坐标关系发布到/tf_static话题。

其命令行执行如下：

# 格式 1：x y z yaw pitch roll parent child
ros2 run tf2_ros static_transform_publisher 0.1 0 0.2 0 0 0 base_link laser

或者在launch 文件中启动如：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # 激光雷达固定在底盘前方 0.1m，上方 0.2m
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='laser_tf',
            arguments=[
                '0.1', '0', '0.2',      # x, y, z
                '0', '0', '0', '1',     # qx, qy, qz, qw（零旋转）
                'base_footprint', 'laser'
            ],
            output='screen'
        ),
    ])

---

4. 通过robot_state_publisher 让Rviz 中的模型动起来

本教程将展示如何为一个行走机器人进行建模，将其状态以 tf2 消息的形式发布，并在 Rviz 中查看仿真效果。

• 首先，我们将创建描述机器人装配结构的 URDF 模型。
• 接着，编写一个节点来模拟机器人的运动，并发布 JointState 和变换信息。
• 随后，我们将使用 robot_state_publisher 将完整的机器人状态发布到 /tf2。

1. 创建一个项目

# 设置ROS2 环境变量
source /opt/ros/humble/setup.bash

mkdir -p ros2_ws/src

cd ros2_ws/src
ros2 pkg create --build-type ament_python --license MIT urdf_tutorial_r2d2 --dependencies rclpy
cd urdf_tutorial_r2d2

2. 创建URDF 文件

cd urdf_tutorial_r2d2
mkdir -p urdf

创建URDF 文件r2d2.urdf.xml，内容如下：

<robot name="r2d2">

  <link name="axis">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder radius="0.01" length=".5" />
      </geometry>
      <material name="gray">
        <color rgba=".2 .2 .2 1" />
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder radius="0.01" length=".5" />
      </geometry>
      <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
    </collision>
  </link>

  <link name="leg1">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
      <origin/>
    </inertial>

    <visual>
      <origin xyz="0 0 -.3" />
      <geometry>
        <box size=".20 .10 .8" />
      </geometry>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 -.3" />
      <geometry>
        <box size=".20 .10 .8" />
      </geometry>
      <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
    </collision>
  </link>

  <joint name="leg1connect" type="fixed">
    <origin xyz="0 .30 0" />
    <parent link="axis"/>
    <child link="leg1"/>
  </joint>

  <link name="leg2">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
      <origin/>
    </inertial>

    <visual>
      <origin xyz="0 0 -.3" />
      <geometry>
        <box size=".20 .10 .8" />
      </geometry>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 -.3" />
      <geometry>
        <box size=".20 .10 .8" />
      </geometry>
      <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
    </collision>
  </link>

  <joint name="leg2connect" type="fixed">
    <origin xyz="0 -.30 0" />
    <parent link="axis"/>
    <child link="leg2"/>
  </joint>

  <link name="body">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
      <origin/>
    </inertial>

    <visual>
      <origin xyz="0 0 -0.2" />
      <geometry>
        <cylinder radius=".20" length=".6"/>
      </geometry>
      <material name="white"/>
    </visual>

    <collision>
      <origin xyz="0 0 0.2" />
      <geometry>
        <cylinder radius=".20" length=".6"/>
      </geometry>
      <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
    </collision>
  </link>

  <joint name="tilt" type="revolute">
    <parent link="axis"/>
    <child link="body"/>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <axis xyz="0 1 0" />
    <limit upper="0" lower="-.5" effort="10" velocity="10" />
  </joint>

  <link name="head">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
      <origin/>
    </inertial>

    <visual>
      <geometry>
        <sphere radius=".4" />
      </geometry>
      <material name="white" />
    </visual>

    <collision>
      <origin/>
      <geometry>
        <sphere radius=".4" />
      </geometry>
      <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
    </collision>
  </link>

  <joint name="swivel" type="continuous">
    <origin xyz="0 0 0.1" />
    <axis xyz="0 0 1" />
    <parent link="body"/>
    <child link="head"/>
  </joint>

  <link name="rod">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
      <origin/>
    </inertial>

    <visual>
      <origin xyz="0 0 -.1" />
      <geometry>
        <cylinder radius=".02" length=".2" />
      </geometry>
      <material name="gray" />

    </visual>

    <collision>
      <origin/>
      <geometry>
        <cylinder radius=".02" length=".2" />
      </geometry>
      <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
    </collision>
  </link>

  <joint name="periscope" type="prismatic">
    <origin xyz=".12 0 .15" />
    <axis xyz="0 0 1" />
    <limit upper="0" lower="-.5" effort="10" velocity="10" />
    <parent link="head"/>
    <child link="rod"/>
  </joint>

  <link name="box">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
      <origin/>
    </inertial>

    <visual>
      <geometry>
        <box size=".05 .05 .05" />
      </geometry>
      <material name="blue" >
        <color rgba="0 0 1 1" />
      </material>
    </visual>

    <collision>
      <origin/>
      <geometry>
        <box size=".05 .05 .05" />
      </geometry>
      <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
    </collision>
  </link>

  <joint name="boxconnect" type="fixed">
    <origin xyz="0 0 0" />
    <parent link="rod"/>
    <child link="box"/>
  </joint>

</robot>

保存r2d2.rviz 内容如下（r2d2.rviz 通长是有rviz 自动导出的配置文件）：

Panels:
  - Class: rviz_common/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
        - /TF1
        - /RobotModel1
        - /RobotModel1/Description Topic1
      Splitter Ratio: 0.5
    Tree Height: 617
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
    Expanded:
      - /2D Goal Pose1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz_common/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz_default_plugins/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Class: rviz_default_plugins/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
        axis:
          Value: true
        body:
          Value: true
        box:
          Value: true
        head:
          Value: true
        leg1:
          Value: true
        leg2:
          Value: true
        odom:
          Value: true
        rod:
          Value: true
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: false
      Tree:
        odom:
          axis:
            body:
              head:
                rod:
                  box:
                    {}
            leg1:
              {}
            leg2:
              {}
      Update Interval: 0
      Value: true
    - Alpha: 1
      Class: rviz_default_plugins/RobotModel
      Collision Enabled: false
      Description File: ""
      Description Source: Topic
      Description Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /robot_description
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        axis:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        body:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        box:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        head:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        leg1:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        leg2:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        rod:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: odom
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz_default_plugins/Interact
      Hide Inactive Objects: true
    - Class: rviz_default_plugins/MoveCamera
    - Class: rviz_default_plugins/Select
    - Class: rviz_default_plugins/FocusCamera
    - Class: rviz_default_plugins/Measure
      Line color: 128; 128; 0
    - Class: rviz_default_plugins/SetInitialPose
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /initialpose
    - Class: rviz_default_plugins/SetGoal
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /goal_pose
    - Class: rviz_default_plugins/PublishPoint
      Single click: true
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /clicked_point
  Transformation:
    Current:
      Class: rviz_default_plugins/TF
  Value: true
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
      Distance: 10
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0
        Y: 0
        Z: 0
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.459797203540802
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 4.618575572967529
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 846
  Hide Left Dock: false
  Hide Right Dock: false
  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002cb000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false
  Width: 1340
  X: 72
  Y: 60

3. 发布状态

现在我们需要一种方法来指定机器人的状态。为此，我们必须明确三个关节的状态及其整体里程计信息。
打开您常用的编辑器，将以下代码粘贴到 second_ros2_ws/src/urdf_tutorial_r2d2/urdf_tutorial_r2d2/state_publisher.py 文件中。
StatePublisher 节点做了以下事：

• 初始化 rclpy 节点并创建 publisher（joint_states）与 tf 广播器；
• 在循环中以固定速率发布 JointState 消息和一个从 odom 到 axis 的 TransformStamped（机器人沿圆周运动）；
• 用 euler_to_quaternion 把 roll/pitch/yaw 转成四元数；
• 用局部变量维护关节角度、增量和周期振荡（tilt/height 摆动、swivel 与 angle 演化）。

from math import sin, cos, pi
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformStamped

class StatePublisher(Node):

    def __init__(self):
        rclpy.init() #初始化 ROS2 客户端库
        super().__init__('state_publisher') #调用 Node 的构造函数并设置节点名为 state_publisher

		# QoSProfile 是一个类， 它封装了ros2中 一个 topic 的所有 QoS 策略组合。
		# 创建 QoS 配置，只有设置 depth=10。这意味着 publisher 队列深度（历史缓存）为 10。其他 QoS 字段（reliability、durability）使用默认值（通常是 RELIABLE / VOLATILE，取决于实现）。
        qos_profile = QoSProfile(depth=10)
        # 创建关节状态/joint_states发布器，使用之前的Qos 配置
        self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
        # 创建TF2 广播器，发送TF2广播
        self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
        self.nodeName = self.get_name()
        self.get_logger().info("{0} started".format(self.nodeName))

		#degree：1° 的弧度量
        degree = pi / 180.0
        # 创建一个 Rate 对象，目标频率 30 Hz。然后在循环中用 loop_rate.sleep() 控制循环周期。
        loop_rate = self.create_rate(30)

        # 这些变量维护机器人/关节的当前状态与增量
        tilt = 0. #倾斜角度（弧度）
        tinc = degree # tilt 的增量（每步变化，初始为 degree）
        swivel = 0. #旋转角（可能映射到 swivel 关节）
        angle = 0. # 全局角度，用来计算机器人在平面上的位置（用于 transform 的 x,y）
        height = 0. #periscope（潜望镜/升降）高度
        hinc = 0.005 #height 的增量（0.005 单位/步）

        # 一个 TransformStamped 实例，用来表示从 odom 父坐标系到 axis 子坐标系的变换
        odom_trans = TransformStamped()
        odom_trans.header.frame_id = 'odom'
        odom_trans.child_frame_id = 'axis'
        # 一个 JointState 消息实例，后面会填充 header.stamp, name, position 并发布。
        joint_state = JointState()

        try:
            while rclpy.ok():
                rclpy.spin_once(self)

                # update joint_state
                now = self.get_clock().now() # 更新关节状态时间戳
                joint_state.header.stamp = now.to_msg()
                # tilt： 身体和底盘间的关节，前后俯仰
                # swivel： 身体和头部的关节，左右转动
                # periscope： 头和连杆的关节，上下伸缩

				# 添加3个关节名
                joint_state.name = ['swivel', 'tilt', 'periscope']
                # 更新关节状态
                joint_state.position = [swivel, tilt, height]

                # update transform
                # (moving in a circle with radius=2)
                odom_trans.header.stamp = now.to_msg()
                # 半径为2 的圆周运动。
                odom_trans.transform.translation.x = cos(angle)*2
                odom_trans.transform.translation.y = sin(angle)*2
                odom_trans.transform.translation.z = 0.7 #固定高度（0.7米）
                # 延Z轴旋转。
                odom_trans.transform.rotation = \
                    euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw

                # 发布关节状态 & TF2 坐标变换
                self.joint_pub.publish(joint_state)
                self.broadcaster.sendTransform(odom_trans)

                # Create new robot state
                tilt += tinc #tilt 加上增量 tinc（每步一个 degree），形成倾斜变化。
                if tilt < -0.5 or tilt > 0.0:
                    tinc *= -1
                height += hinc
                if height > 0.2 or height < 0.0:
                    hinc *= -1
                swivel += degree #swivel 每步增加 1°（弧度），持续旋转
                angle += degree/4 #angle 增加步长为 0.25°（弧度）

                # This will adjust as needed per iteration
                loop_rate.sleep()

        except KeyboardInterrupt:
            pass

def euler_to_quaternion(roll, pitch, yaw):
    qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
    qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
    qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
    qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
    return Quaternion(x=qx, y=qy, z=qz, w=qw)

def main():
    node = StatePublisher()

if __name__ == '__main__':
    main()

4. 创建一个launch文件

创建一个新的 second_ros2_ws/src/urdf_tutorial_r2d2/launch 文件夹。打开您的编辑器，粘贴以下代码，并将其保存为 second_ros2_ws/src/urdf_tutorial_r2d2/launch/demo.launch.py。

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import FileContent, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    urdf = FileContent( PathJoinSubstitution([FindPackageShare('urdf_tutorial_r2d2'), 'r2d2.urdf.xml']))

    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time, 'robot_description': urdf}],
            arguments=[urdf]),
        Node(
            package='urdf_tutorial_r2d2',
            executable='state_publisher',
            name='state_publisher',
            output='screen'),
    ])

5. 编辑 setup.py 文件

您必须告知 colcon 构建工具如何安装您的 Python 软件包。请按以下方式编辑 second_ros2_ws/src/urdf_tutorial_r2d2/setup.py 文件：

import os
from glob import glob
from setuptools import setup
from setuptools import find_packages
....
data_files=[
  ...
  (os.path.join('share', package_name, 'launch'), glob('launch/*')),
  (os.path.join('share', package_name), glob('urdf/*')),
],
...
'console_scripts': [
    'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
],

6. 编译&运行

cd second_ros2_ws
colcon build --symlink-install --packages-select urdf_tutorial_r2d2

#Source the setup files:
source install/setup.bash

启动这个包：

ros2 launch urdf_tutorial_r2d2 demo.launch.py

打开一个新终端，然后使用以下命令启动Rviz：

rviz2 -d `ros2 pkg prefix urdf_tutorial_r2d2 --share`/r2d2.rviz
#ros2 pkg prefix <package> → 输出指定 ROS2 包的安装路径
#--share → 返回该包的 share 目录路径
# 就是r2d2.rviz 的绝对路径

以上的内容是， 你创建了一个 JointState 发布节点，并将其与 robot_state_publisher 结合，用来模拟一台会行走的机器人。