前言

        本篇将完成一个行走的机器人，并以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, TransformStampedclass 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.0loop_rate = self.create_rate(30)# robot statetilt = 0.tinc = degreeswivel = 0.angle = 0.height = 0.hinc = 0.005# message declarationsodom_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_statenow = 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)*2odom_trans.transform.translation.y = sin(angle)*2odom_trans.transform.translation.z = 0.7odom_trans.transform.rotation = \euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw# send the joint state and transformself.joint_pub.publish(joint_state)self.broadcaster.sendTransform(odom_trans)# Create new robot statetilt += tincif tilt < -0.5 or tilt > 0.0:tinc *= -1height += hincif height > 0.2 or height < 0.0:hinc *= -1swivel += degreeangle += degree/4# This will adjust as needed per iterationloop_rate.sleep()except KeyboardInterrupt:passdef 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 Nodedef 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来解决）。

本篇完。