【ROS2】中级:URDF-使用 URDF 与 robot_state_publisher

目标：模拟一个用 URDF 建模的行走机器人，并在 Rviz 中查看。

教程级别：中级

 时间：15 分钟

 目录

•  背景

•  先决条件

•  任务

  • 1 创建一个包

  • 2 创建 URDF 文件

  • 3 发布状态

  • 4 创建启动文件

  • 5 编辑 setup.py 文件

  • 6 安装软件包

  • 7 查看结果

•  摘要

 背景

本教程将向您展示如何建模一个行走机器人，将状态发布为 tf2 消息，并在 Rviz 中查看模拟。首先，我们创建描述机器人组装的 URDF 模型。接下来，我们编写一个节点来模拟运动并发布 JointState 和变换。然后我们使用 robot_state_publisher 将整个机器人状态发布到 /tf2 。

先决条件

• rviz2   https://index.ros.org/p/rviz2/

始终不要忘记在您打开的每个新终端中获取 ROS 2 的源。 

source /opt/ros/jazzy/setup.bash
echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc

 任务

1. 创建一个包

 创建目录：

mkdir -p second_ros2_ws/src

然后创建包：

cd second_ros2_ws/src
ros2 pkg create --build-type ament_python --license Apache-2.0 urdf_tutorial_r2d2 --dependencies rclpy
cd urdf_tutorial_r2d2

你现在应该看到一个 urdf_tutorial_r2d2 文件夹。接下来你将对其进行一些更改。

2 创建 URDF 文件

创建我们将存储一些资产的目录：

mkdir -p urdf

下载 URDF file 并将其保存为 second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.urdf.xml 。下载 Rviz configuration file 并将其保存为 second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.rviz 。

https://docs.ros.org/en/jazzy/_downloads/872802005223ffdb75b1ab7b25ad445b/r2d2.urdf.xml

https://docs.ros.org/en/jazzy/_downloads/96d68aef72c4f27f32af5961ef48c475/r2d2.rviz

3 发布状态 

现在我们需要一种方法来指定机器人所处的状态。为此，我们必须指定所有三个关节和整体里程计。

启动您喜欢的编辑器，并将以下代码粘贴到 second_ros2_ws/src/urdf_tutorial_r2d2/urdf_tutorial_r2d2/state_publisher.py 中

from math import sin, cos, pi  # 从math模块导入sin, cos, pi函数
import rclpy  # 导入rclpy库
from rclpy.node import Node  # 从rclpy.node模块导入Node类
from rclpy.qos import QoSProfile  # 从rclpy.qos模块导入QoSProfile类
from geometry_msgs.msg import Quaternion  # 从geometry_msgs.msg模块导入Quaternion消息类型
from sensor_msgs.msg import JointState  # 从sensor_msgs.msg模块导入JointState消息类型
from tf2_ros import TransformBroadcaster, TransformStamped  # 从tf2_ros模块导入TransformBroadcaster和TransformStamped类
 
 
class StatePublisher(Node):  # 定义StatePublisher类，继承自Node类
 
 
    def __init__(self):  # 初始化函数
        rclpy.init()  # 初始化rclpy
        super().__init__('state_publisher')  # 调用父类的初始化函数，并指定节点名称为'state_publisher'
 
 
        qos_profile = QoSProfile(depth=10)  # 创建一个QoSProfile对象，设置深度为10
        self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)  # 创建一个发布者，发布JointState消息到'joint_states'话题
        self.broadcaster = TransformBroadcaster(self, qos=qos_profile)  # 创建一个TransformBroadcaster对象
        self.nodeName = self.get_name()  # 获取节点名称
        self.get_logger().info("{0} started".format(self.nodeName))  # 打印日志信息，显示节点已启动
 
 
        degree = pi / 180.0  # 将角度转换为弧度
        loop_rate = self.create_rate(30)  # 创建一个循环频率对象，设置频率为30Hz
 
 
        # 机器人状态
        tilt = 0.  # 倾斜角度初始化为0
        tinc = degree  # 倾斜角度增量初始化为一个角度单位
        swivel = 0.  # 旋转角度初始化为0
        angle = 0.  # 角度初始化为0
        height = 0.  # 高度初始化为0
        hinc = 0.005  # 高度增量初始化为0.005
 
 
        # 消息声明
        odom_trans = TransformStamped()  # 创建一个TransformStamped对象
        odom_trans.header.frame_id = 'odom'  # 设置父坐标系为'odom'
        odom_trans.child_frame_id = 'axis'  # 设置子坐标系为'axis'
        joint_state = JointState()  # 创建一个JointState对象
 
 
        try:
            while rclpy.ok():  # 当rclpy处于正常状态时循环
                rclpy.spin_once(self)  # 处理一次ROS2的回调函数
 
 
                # 更新joint_state
                now = self.get_clock().now()  # 获取当前时间
                joint_state.header.stamp = now.to_msg()  # 设置joint_state的时间戳
                joint_state.name = ['swivel', 'tilt', 'periscope']  # 设置关节名称
                joint_state.position = [swivel, tilt, height]  # 设置关节位置
 
 
                # 更新变换
                # (以半径为2的圆运动)
                odom_trans.header.stamp = now.to_msg()  # 设置odom_trans的时间戳
                odom_trans.transform.translation.x = cos(angle)*2  # 设置变换的x坐标
                odom_trans.transform.translation.y = sin(angle)*2  # 设置变换的y坐标
                odom_trans.transform.translation.z = 0.7  # 设置变换的z坐标
                odom_trans.transform.rotation = \
                    euler_to_quaternion(0, 0, angle + pi/2)  # 设置变换的旋转（欧拉角转换为四元数）
 
 
                # 发送关节状态和变换
                self.joint_pub.publish(joint_state)  # 发布joint_state消息
                self.broadcaster.sendTransform(odom_trans)  # 发送odom_trans变换
 
 
                # 创建新的机器人状态
                tilt += tinc  # 更新倾斜角度
                if tilt < -0.5 or tilt > 0.0:  # 如果倾斜角度超出范围，反向倾斜角度增量
                    tinc *= -1
                height += hinc  # 更新高度
                if height > 0.2 or height < 0.0:  # 如果高度超出范围，反向高度增量
                    hinc *= -1
                swivel += degree  # 更新旋转角度
                angle += degree/4  # 更新角度
 
 
                # 根据需要调整每次迭代的频率
                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)  # 计算四元数的x分量
    qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)  # 计算四元数的y分量
    qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)  # 计算四元数的z分量
    qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)  # 计算四元数的w分量
    return Quaternion(x=qx, y=qy, z=qz, w=qw)  # 返回四元数对象
 
 
def main():  # 定义主函数
    node = StatePublisher()  # 创建StatePublisher节点
 
 
if __name__ == '__main__':  # 如果脚本是直接运行的
    main()  # 调用主函数

4 创建启动文件

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

import os  # 导入os模块，用于文件和路径操作
from ament_index_python.packages import get_package_share_directory  # 从ament_index_python.packages模块导入get_package_share_directory函数
from launch import LaunchDescription  # 从launch模块导入LaunchDescription类
from launch.actions import DeclareLaunchArgument  # 从launch.actions模块导入DeclareLaunchArgument类
from launch.substitutions import LaunchConfiguration  # 从launch.substitutions模块导入LaunchConfiguration类
from launch_ros.actions import Node  # 从launch_ros.actions模块导入Node类
 
 
def generate_launch_description():  # 定义generate_launch_description函数
 
 
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')  # 创建LaunchConfiguration对象，默认值为'false'
 
 
    urdf_file_name = 'r2d2.urdf.xml'  # 定义URDF文件名
    urdf = os.path.join(
        get_package_share_directory('urdf_tutorial_r2d2'),  # 获取urdf_tutorial_r2d2包的共享目录
        urdf_file_name)  # 拼接成URDF文件的完整路径
    with open(urdf, 'r') as infp:  # 打开URDF文件
        robot_desc = infp.read()  # 读取URDF文件内容
 
 
    return LaunchDescription([  # 返回LaunchDescription对象
        DeclareLaunchArgument(
            'use_sim_time',  # 声明一个启动参数'use_sim_time'
            default_value='false',  # 默认值为'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': robot_desc}],  # 设置节点参数
            arguments=[urdf]),  # 传递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 中

data_files=[
  ...
  (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
  (os.path.join('share', package_name), glob('urdf/*')),
],

• 修改 entry_points 表，以便您稍后可以从控制台运行‘state_publisher’

'console_scripts': [
    'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
],

保存 setup.py 文件并进行更改。

import os  # 导入os模块
from glob import glob  # 从glob模块导入glob函数
from setuptools import setup  # 从setuptools模块导入setup函数
from setuptools import find_packages  # 从setuptools模块导入find_packages函数
 
 
package_name = 'urdf_tutorial_r2d2'  # 定义包名称
 
 
setup(
    name=package_name,  # 设置包名称
    version='0.0.0',  # 设置包版本
    packages=find_packages(exclude=['test']),  # 查找包，排除'test'目录
    data_files=[
        ('share/ament_index/resource_index/packages',  # 设置数据文件目录
            ['resource/' + package_name]),  # 包资源文件
        ('share/' + package_name, ['package.xml']),  # 包描述文件
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),  # 启动文件
        (os.path.join('share', package_name), glob('urdf/*')),  # URDF文件
    ],
    install_requires=['setuptools'],  # 设置依赖项
    zip_safe=True,  # 设置是否安全打包为zip文件
    maintainer='cxy',  # 设置维护者
    maintainer_email='cxy@126.com',  # 设置维护者邮箱
    description='urdf tutorial r2d2',  # 设置包描述
    license='Apache-2.0',  # 设置许可证
    tests_require=['pytest'],  # 设置测试依赖项
    entry_points={
        'console_scripts': [
            'state_publisher = urdf_tutorial_r2d2.state_publisher:main'  # 设置控制台脚本入口点
        ],
    },
)

6 安装软件包

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

cxy@ubuntu2404-cxy:~/second_ros2_ws$ colcon build --symlink-install --packages-select urdf_tutorial_r2d2
Starting >>> urdf_tutorial_r2d2
--- stderr: urdf_tutorial_r2d2                   
/usr/lib/python3/dist-packages/setuptools/command/develop.py:40: EasyInstallDeprecationWarning: easy_install command is deprecated.
!!
 
 
        ********************************************************************************
        Please avoid running ``setup.py`` and ``easy_install``.
        Instead, use pypa/build, pypa/installer or other
        standards-based tools.
 
 
        See https://github.com/pypa/setuptools/issues/917 for details.
        ********************************************************************************
 
 
!!
  easy_install.initialize_options(self)
/usr/lib/python3/dist-packages/setuptools/command/easy_install.py:363: UserWarning: Unbuilt egg for pytest-repeat [unknown version] (/usr/lib/python3/dist-packages)
  self.local_index = Environment(self.shadow_path + sys.path)
---
Finished <<< urdf_tutorial_r2d2 [3.29s]
 
 
Summary: 1 package finished [3.50s]
  1 package had stderr output: urdf_tutorial_r2d2

获取安装文件：

source install/setup.bash

7 查看结果 

 启动包裹

ros2 launch urdf_tutorial_r2d2 demo_launch.py

打开一个新的终端，然后运行 Rviz

rviz2 -d second_ros2_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz

请参阅用户指南以了解如何使用 Rviz。http://wiki.ros.org/rviz/UserGuide 

 摘要

您创建了一个 JointState 发布节点，并将其与 robot_state_publisher 结合以模拟行走机器人。这些示例中使用的代码最初来自这里https://github.com/benbongalon/ros2-migration/tree/master/urdf_tutorial 。

向本 ROS 1 教程https://wiki.ros.org/urdf/Tutorials/Using%20urdf%20with%20robot_state_publisher 的作者致谢，其中部分内容被重复使用。