前言

??????? 本篇將完成一個行走的機器人，并以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來解決）。

本篇完。