使用 URDF robot_state_publisher

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

教程级别： 中级

时间： 15 分钟

背景

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

先决条件

• rviz2

与往常一样，不要忘记在 every new terminal you open.

任务

1 创建包

创建目录：

LinuxmacOSWindows

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

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

LinuxmacOSWindows

mkdir -p urdf

下载 URDF file and save it as second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.urdf.xml. 下载 Rviz configuration file and save it as second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.rviz.

3 发布状态

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

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

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()
        super().__init__('state_publisher')

        qos_profile = QoSProfile(depth=10)
        self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
        self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
        self.nodeName = self.get_name()
        self.get_logger().info("{0} started".format(self.nodeName))

        degree = pi / 180.0
        loop_rate = self.create_rate(30)

        # robot state
        tilt = 0.
        tinc = degree
        swivel = 0.
        angle = 0.
        height = 0.
        hinc = 0.005

        # message declarations
        odom_trans = TransformStamped()
        odom_trans.header.frame_id = 'odom'
        odom_trans.child_frame_id = 'axis'
        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()
                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()
                odom_trans.transform.translation.x = cos(angle)*2
                odom_trans.transform.translation.y = sin(angle)*2
                odom_trans.transform.translation.z = 0.7
                odom_trans.transform.rotation = \
                    euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw

                # send the joint state and transform
                self.joint_pub.publish(joint_state)
                self.broadcaster.sendTransform(odom_trans)

                # Create new robot state
                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

                # 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 创建启动文件

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

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    urdf_file_name = 'r2d2.urdf.xml'
    urdf = os.path.join(
        get_package_share_directory('urdf_tutorial_r2d2'),
        urdf_file_name)
    with open(urdf, 'r') as infp:
        robot_desc = infp.read()

    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': robot_desc}],
            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

• append these 2 lines inside 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/*')),
],

• modify the entry_points table so you can later run ‘state_publisher’ from a console

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

保存“setup.py”文件并保存您的更改。

6 安装软件包

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

获取安装文件：

LinuxmacOSWindows

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 教程 的作者，其中部分内容被重复使用。