ROS 2边学边练（48）-- 将URDF与robot_state_publisher一起使用_robotstatepublisher

前言

        本篇将完成一个行走的机器人，并以tf2消息的方式实时发布机器人状态，以便我们在Rviz中同步查看。

        首先，我们创建描述机器人装配的URDF模型。接下来，我们编写一个节点，用于模拟运动并发布JointState和位姿变换。然后，我们使用robot_state_publisher将整个robot状态发布到/tf2。

动动手

创建新包

        执行如下命令创建新的工作空间second_ros2_ws，并在其下创建src文件夹：

$mkdir -p second_ros2_ws/src

        再在src下创建urdf_tutorial_r2d2包：

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

        在urdf_tutorial_r2d2包根路径下，创建urdf文件夹，以放置保存一些组件：

$mkdir -p urdf

        分别下载URDF文件(r2d2.urdf.xml)和Rviz配置文件(r2d2.rviz)到second_ros2_ws/src/urdf_tutorial_r2d2/urdf/路径下。

发布状态

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

        创建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()

创建启动文件

        创建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'),
    ])

编辑setup.py文件

        我们必须告诉colcon build工具如何安装指定的python包。这可以通过修改python包根路径下的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(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'
],

安装python包

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

        最后配置下环境变量：

$source install/setup.bash

查看结果

$ros2 launch urdf_tutorial_r2d2 demo_launch.py

        打开另外一个终端，运行rviz2进行查看(rviz使用参考)。

$source /opt/ros/iron/setup.bash
$rviz2 -d second_ros2_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz

        不出大意外的话，大家运行后的结果应该像下面这样（需要手动点击左下角的Add按钮添加TF及机器人模型），机器人“隐身”了。 

 

        如果我们换个命令,则一切正常了（其实前面的命令最后调用的还是下面这个路径的rviz文件）

$rviz2 -d src/urdf_tutorial_r2d2/urdf/r2d2.rviz

或加上绝对路径，

$rviz2 -d ~/Desktop/second_ros2_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz

 

        此例原始代码工程在这（只不过编译运行后会报错误，需要增加dummpy link来解决）。

本篇完。