前言
本篇将完成一个行走的机器人，并以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_packagesdata_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来解决）。
本篇完。
