创建功能包运行URDF

在任意位置创建功能包运行URDF

在之前的章节中，我们是在系统预装的功能包上进行修改。但在实际开发中，我们需要在自己的工作空间中从零开始构建机器人功能包。本节将展示如何创建一个独立的 ROS2 功能包，编写一个 Python 节点来模拟硬件驱动（发布关节数据），并编写 Launch 文件来启动整个系统。

创建功能包

假设工作空间位于 ~/ros2_ws，首先我们需要i创建一个 Python 类型的功能包：

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python --license Apache-2.0 my_urdf_test 

然后，将之前使用的模型文件和 Rviz 配置文件复制到新的功能包中。将 /opt/ros/jazzy/share/urdf_tutorial 中的rviz和 urdf 文件夹复制到此功能包中，此外在功能包中创建launch路径。完成后，文件的目录结构如下所示：

  my_urdf_test/
    launch/
    package.xml
    my_urdf_test/
    resource/
    setup.cfg
    setup.py
    test/
    rviz/
    urdf/

在实际机器人中，驱动程序会读取电机编码器并发布 /joint_states。在这里，我们编写一个 Python 节点来模拟这个过程，让机器人的头动起来，并让机器人沿圆形轨迹移动。

在 my_urdf_test/my_urdf_test/ 目录下创建文件 state_publisher.py：

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

class StatePublisher(Node):
    def __init__(self):
        super().__init__('state_publisher')

        # 1. 关节状态发布者 (模拟电机数据)
        self.joint_pub = self.create_publisher(JointState, 'joint_states', 10)
        
        # 2. TF 坐标广播器 (用于发布机器人在世界坐标系 odom 中的位置)
        self.tf_broadcaster = TransformBroadcaster(self)

        # 定义需要控制的关节名称 (必须与 URDF 文件中的 joint name 一致)
        self.joint_names = ['left_gripper_joint', 'head_swivel']
        
        # --- 运动参数初始化 ---
        self.degree = (pi / 180.0) * 0.2 
        self.tilt = 0.0             # 头部上下俯仰角度
        self.tilt_inc = self.degree # 增量
        self.swivel = 0.0           # 头部左右旋转角度
        
        # 机器人整体在地图上的位置
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # 创建 30Hz 的定时器
        self.timer = self.create_timer(1.0 / 30.0, self.timer_callback)

    def timer_callback(self):
        now = self.get_clock().now().to_msg()

        # --- A. 发布关节状态 (JointState) ---
        # 这部分告诉 robot_state_publisher：机器人的关节弯到了多少度
        joint_msg = JointState()
        joint_msg.header.stamp = now
        joint_msg.name = self.joint_names
        joint_msg.position = [self.swivel, self.tilt]
        self.joint_pub.publish(joint_msg)

        # --- B. 发布坐标变换 (TF: odom -> base_link) ---
        # 这部分告诉整个系统：机器人的底盘现在在世界的哪个位置
        t = TransformStamped()
        t.header.stamp = now
        t.header.frame_id = 'odom'        # 父坐标系：里程计/世界
        t.child_frame_id = 'base_link'    # 子坐标系：机器人基座

        # 计算圆形轨迹
        self.theta += 0.01
        self.x = sin(self.theta) * 2.0
        self.y = cos(self.theta) * 2.0

        t.transform.translation.x = self.x
        t.transform.translation.y = self.y
        t.transform.translation.z = 0.0
        
        # 这里仅演示位置移动，姿态保持不变 (四元数 w=1)
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0

        self.tf_broadcaster.sendTransform(t)

        # --- C. 更新下一帧的角度数据 ---
        # 头部像摇头风扇一样往返运动
        self.tilt += self.tilt_inc
        if self.tilt < -0.5 or self.tilt > 0.0:
            self.tilt_inc *= -1

        # 头部持续旋转
        self.swivel += self.degree

def main():
    rclpy.init()
    node = StatePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

这段代码用于动态发布两个关节的位置，使模型在 RViz 中显示“动起来”，并让机器人沿圆形轨迹移动。需要注意，joint_states 中的 name 必须与 URDF 中 <joint name="..."> 完全一致，否则关节不会运动。

接下来，我们需要一个launch文件来同时启动三个节点：

robot_state_publisher：解析 URDF 模型。
state_publisher (自己编写的)：让机器人动起来。
rviz2：可视化显示。

在 my_urdf_test/launch/ 目录下创建 display.launch.py，并编写如下代码：

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

def generate_launch_description():
    # --- 1. 获取功能包路径 ---
    package_name = 'my_urdf_test'
    pkg_share = FindPackageShare(package_name)

    # --- 2. 声明启动参数 ---
    # 允许在命令行指定要加载的模型文件，默认为 06-flexible.urdf
    model_file_arg = DeclareLaunchArgument(
        name='model_file',
        default_value='06-flexible.urdf', 
        description='Name of the URDF/Xacro file inside the urdf/ directory'
    )

    # 参数 2: RViz 配置文件路径
    rviz_arg = DeclareLaunchArgument(
        name='rvizconfig',
        default_value=PathJoinSubstitution([pkg_share, 'rviz/urdf.rviz']),
        description='Absolute path to rviz config file'
    )

    # --- 3. 动态生成模型文件的完整路径 ---
    full_model_path = PathJoinSubstitution([
        pkg_share, 
        'urdf', 
        LaunchConfiguration('model_file')
    ])

    # --- 4. 定义三个核心节点 --- 
    # Node A: robot_state_publisher 
    # 作用：加载 URDF，并根据 joint_states 计算 TF 
    # 关键点：使用 xacro 命令处理文件，这样既支持 .urdf 也支持 .xacro
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{
            'robot_description': Command(['xacro ', full_model_path])
        }]
    )

  # Node B: 自定义驱动节点 
  # 作用：发布 /joint_states 和 odom 变换，替代了 joint_state_publisher_gui
    my_state_publisher_node = Node(
        package=package_name,
        executable='state_publisher',
        name='state_publisher',
        output='screen'
    )

    # Node C: RViz2
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
    )

    return LaunchDescription([
        model_file_arg, 
        rviz_arg,
        robot_state_publisher_node,
        my_state_publisher_node,
        rviz_node
    ])

构建功能包

在编译之前，需要对功能包进行配置。

1. 修改 setup.py 确保 data_files 部分包含了 launch、urdf 和 rviz 目录，并在 entry_points 中注册节点：

import os
from glob import glob
from setuptools import find_packages, setup

package_name = 'my_urdf_test'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        # 显式安装 launch, urdf 和 rviz 文件夹中的文件
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
        (os.path.join('share', package_name, 'urdf'), glob(os.path.join('urdf', '*'))),
        (os.path.join('share', package_name, 'rviz'), glob(os.path.join('rviz', '*.rviz'))),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='rooter',
    maintainer_email='rooter@todo.todo',
    description='ROS 2 URDF test package',
    license='Apache-2.0',
    tests_require=['pytest'],
    # --- 注册可执行节点 ---
    entry_points={
        'console_scripts': [
            'state_publisher = my_urdf_test.state_publisher:main'
        ],
    },
)

2. 修改 package.xml 添加运行时依赖，确保其他人安装你的包时能自动安装这些库：

 <exec_depend>rclpy</exec_depend>
 <exec_depend>xacro</exec_depend>
 <exec_depend>robot_state_publisher</exec_depend>
 <exec_depend>sensor_msgs</exec_depend>
 <exec_depend>tf2_ros</exec_depend>
 <exec_depend>geometry_msgs</exec_depend>
 <exec_depend>rviz2</exec_depend>

配置好文件后返回到工作空间下面（ros2_ws）运行如下代码编译功能包：

cd ~/ros2_ws
colcon build --packages-select my_urdf_test

继续在该终端加载工作空间

source install/setup.bash

加载完工作空间，运行如下命令：

ros2 launch my_urdf_test display.launch.py model:=urdf/08-macroed.urdf.xacro

预期效果：

RViz 窗口自动弹出。
你会看到一个 R2D2 形状的机器人。
关键点：机器人不再是静止的，它的头部在左右旋转，并且整个机器人在网格地图上沿着圆形轨迹自动行走（在Rviz中将fixed_frame修改为odom）。这证明 state_publisher 节点正在成功驱动模型。

总结

本章构建了一个完整的机器人建模闭环。内容首先从 URDF 的基础语法入手，详细解析了 link 和 Joint 的定义方式；随后引入了 Xacro 进阶技巧，利用宏（Macro）和数学计算极大地简化了模型文件的编写与维护；最后脱离系统安装功能包，展示了如何构建一个包含 Launch 启动文件、Python 驱动节点以及模型文件的独立 ROS 2 功能包。掌握这套从建模到驱动的完整流程，将为后续开发移动机器人或机械臂打下坚实的基础。。

在任意位置创建功能包运行URDF​