ROS2 第四讲 tf2
turtle demo
安装turtle-tf2 demo:
sudo apt-get install ros-foxy-turtle-tf2-py ros-foxy-tf2-tools
以及可以转换四元数与欧拉角的包:
pip3 install transforms3d
我们来演示一下:
ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py
会看到两只小乌龟，其中一只会向位于中心的小乌龟移动:
使用键盘控制下小乌龟，另一只仍会跟随:
ros2 run turtlesim turtle_teleop_key
这个demo 展示了tf2的功能。此例中，tf2库创建三个坐标系，分别是世界(或环境)坐标系，turtle1坐标系与turtle2坐标系。tf2库中的broadcaster发布turtle的坐标系，然后listener计算二者的相对位置，然后将基中一个小乌龟移向另一个。
ROS2提供了tf2_tools查看tf2工作图:
ros2 run tf2_tools view_frames.py
会显示:
[INFO] [1631335696.355017201] [view_frames]: Listening to tf data during 5 seconds...
[INFO] [1631335701.358101073] [view_frames]: Generating graph in frames.pdf file...
[INFO] [1631335701.359303339] [view_frames]: Result:tf2_msgs.srv.FrameGraph_Response(frame_yaml="turtle1: \n parent: ‘world‘\n broadcaster: ‘default_authority‘\n rate: 62.705\n most_recent_transform: 1631335701.357549\n oldest_transform: 1631335696.349964\n buffer_length: 5.008\nturtle2: \n parent: ‘world‘\n broadcaster: ‘default_authority‘\n rate: 62.706\n most_recent_transform: 1631335701.357533\n oldest_transform: 1631335696.350012\n buffer_length: 5.008\n")
/opt/ros/foxy/lib/tf2_tools/view_frames.py:75: YAMLLoadWarning: calling yaml.load() without Loader=... is deprecated, as the default Loader is unsafe. Please read https://msg.pyyaml.org/load for full details.
data = yaml.load(result.frame_yaml)
然后在当前目录下，可以看到:
frames.gv
frames.pdf
打开frames.pdf:
可以看到，结构上world坐标系是两只小乌龟坐标系的父坐标系。上面还有一些接收到最老与最新坐系的时间，以及平均的接收频率。
使用tf2_echo时实查看两坐标系之间的转换:
# ros2 run tf2_ros tf2_echo [reference_frame] [target_frame]
ros2 run tf2_ros tf2_echo turtle2 turtle1
会显示类似:
At time 1631336659.214493548
- Translation: [0.015, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.206, 0.978]
At time 1631336660.222032437
- Translation: [0.008, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.208, 0.978]
使用rviz 查看:
ros2 run rviz2 rviz2 -d $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz
使用键盘移动小乌龟时，会看到rviz中的坐标系会在rviz中移动。
静态broadcaster
创建package:
cd dev_ws/src/
ros2 pkg create --build-type ament_python learning_tf2_py
cd learning_tf2_py/learning_tf2_py/
vim static_turtle_tf2_broadcaster.py
在文件中编写:
import sys
from geometry_msgs.msg import TransformStamped
import rclpy
from rclpy.node import Node
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
import tf_transformations
class StaticFramePublisher(Node):
"""
Broadcast transforms that never change.
This example publishes transforms from `world` to a static turtle frame.
The transforms are only published once at startup, and are constant for all
time.
"""
def __init__(self, transformation):
super().__init__(‘static_turtle_tf2_broadcaster‘)
self._tf_publisher = StaticTransformBroadcaster(self)
# Publish static transforms once at startup
self.make_transforms(transformation)
def make_transforms(self, transformation):
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = self.get_clock().now().to_msg()
static_transformStamped.header.frame_id = ‘world‘
static_transformStamped.child_frame_id = sys.argv[1]
static_transformStamped.transform.translation.x = float(sys.argv[2])
static_transformStamped.transform.translation.y = float(sys.argv[3])
static_transformStamped.transform.translation.z = float(sys.argv[4])
quat = tf_transformations.quaternion_from_euler(
float(sys.argv[5]), float(sys.argv[6]), float(sys.argv[7]))
static_transformStamped.transform.rotation.x = quat[0]
static_transformStamped.transform.rotation.y = quat[1]
static_transformStamped.transform.rotation.z = quat[2]
static_transformStamped.transform.rotation.w = quat[3]
self._tf_publisher.sendTransform(static_transformStamped)
def main():
logger = rclpy.logging.get_logger(‘logger‘)
# obtain parameters from command line arguments
if len(sys.argv) < 8:
logger.info(‘Invalid number of parameters. Usage: \n‘
‘$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster‘
‘child_frame_name x y z roll pitch yaw‘)
sys.exit(0)
else:
if sys.argv[1] == ‘world‘:
logger.info(‘Your static turtle name cannot be "world"‘)
sys.exit(0)
# pass parameters and initialize node
rclpy.init()
node = StaticFramePublisher(sys.argv)
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
rclpy.shutdown()
然后我们在package.xml中加入:
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf_transformations</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>turtlesim</exec_depend>
在setup.py中的console_scripts中加入:
‘static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main‘,
然后:
cd dev_ws
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_py
. install/setup.bash
为看到我们确实成功发布静态转换，先打开:
ros2 topic echo /tf_static
然后:
ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0
会在ros2 topic echo /tf_static 所在窗口中显示类似如下信息:
transforms:
- header:
stamp:
sec: 1631355927
nanosec: 346452972
frame_id: world
child_frame_id: mystaticturtle
transform:
translation:
x: 0.0
y: 0.0
z: 1.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
---
实际开发过程中，其实并不需要自己来写这样的发布代码， 只需使用static_transform_publisher, 它既可以当作命令行工具使用，也可将其当作一个节点，然后加入到launch文件中，启动:
ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
或者(其中，qx,qy,qz,qw是四元数):
ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id
或者加入到launch文件中:
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package=‘tf2_ros‘,
executable=‘static_transform_publisher‘,
arguments = [‘0‘, ‘0‘, ‘1‘, ‘0‘, ‘0‘, ‘0‘, ‘world‘, ‘mystaticturtle‘]
),
])
编写tf2 broadcaster
在dev_ws/src/learning_tf2_py/learning_tf2_py/中编写turtle_tf2_broadcaster.py
from geometry_msgs.msg import TransformStamped
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
import tf_transformations
from turtlesim.msg import Pose
class FramePublisher(Node):
def __init__(self):
super().__init__(‘turtle_tf2_frame_publisher‘)
# Declare and acquire `turtlename` parameter
self.declare_parameter(‘turtlename‘, ‘turtle‘)
self.turtlename = self.get_parameter(
‘turtlename‘).get_parameter_value().string_value
# Initialize the transform broadcaster
self.br = TransformBroadcaster(self)
# Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
# callback function on each message
self.subscription = self.create_subscription(
Pose,
f‘/{self.turtlename}/pose‘,
self.handle_turtle_pose,
1)
self.subscription
def handle_turtle_pose(self, msg):
t = TransformStamped()
# Read message content and assign it to
# corresponding tf variables
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = ‘world‘
t.child_frame_id = self.turtlename
# Turtle only exists in 2D, thus we get x and y translation
# coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg.x
t.transform.translation.y = msg.y
t.transform.translation.z = 0.0
# For the same reason, turtle can only rotate around one axis
# and this why we set rotation in x and y to 0 and obtain
# rotation in z axis from the message
q = tf_transformations.quaternion_from_euler(0, 0, msg.theta)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
# Send the transformation
self.br.sendTransform(t)
def main():
rclpy.init()
node = FramePublisher()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
rclpy.shutdown()
在package.xml及在setup.py分别加入:
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
‘turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main‘,
然后创建launch文件:
cd dev_ws/src/learning_tf2_py/learning_tf2_py/
mkdir launch
vim turtle_tf2_demo.launch.py
编写:
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package=‘turtlesim‘,
executable=‘turtlesim_node‘,
name=‘sim‘
),
Node(
package=‘learning_tf2_py‘,
executable=‘turtle_tf2_broadcaster‘,
name=‘broadcaster1‘,
parameters=[
{‘turtlename‘: ‘turtle1‘}
]
),
])
然后再在setup.py的data_files中加入:
(os.path.join(‘share‘, package_name, ‘launch‘), glob(os.path.join(‘launch‘, ‘*.launch.py‘))),
然后:
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_py
. install/setup.bash
ros2 launch learning_tf2_py turtle_tf2_demo.launch.py
使用键盘控制并查看发布的tf2信息:
ros2 run turtlesim turtle_teleop_key
ros2 run tf2_ros tf2_echo world turtle1
At time 1625137663.912474878
- Translation: [5.276, 7.930, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137664.950813527
- Translation: [3.750, 6.563, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137665.906280726
- Translation: [2.320, 5.282, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137666.850775673
- Translation: [2.153, 5.133, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.365, 0.931]
编写tf2 listener
学习如何访问tf2的坐标系变换.
在learning_tf2_py/learning_tf2_py中编写turtle_tf2_listener.py:
import math
from geometry_msgs.msg import Twist
import rclpy
from rclpy.node import Node
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from turtlesim.srv import Spawn
class FrameListener(Node):
def __init__(self):
super().__init__(‘turtle_tf2_frame_listener‘)
# Declare and acquire `target_frame` parameter
self.declare_parameter(‘target_frame‘, ‘turtle1‘)
self.target_frame = self.get_parameter(
‘target_frame‘).get_parameter_value().string_value
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# Create a client to spawn a turtle
self.spawner = self.create_client(Spawn, ‘spawn‘)
# Boolean values to store the information
# if the service for spawning turtle is available
self.turtle_spawning_service_ready = False
# if the turtle was successfully spawned
self.turtle_spawned = False
# Create turtle2 velocity publisher
self.publisher = self.create_publisher(Twist, ‘turtle2/cmd_vel‘, 1)
# Call on_timer function every second
self.timer = self.create_timer(1.0, self.on_timer)
def on_timer(self):
# Store frame names in variables that will be used to
# compute transformations
from_frame_rel = self.target_frame
to_frame_rel = ‘turtle2‘
if self.turtle_spawning_service_ready:
if self.turtle_spawned:
# Look up for the transformation between target_frame and turtle2 frames
# and send velocity commands for turtle2 to reach target_frame
try:
now = rclpy.time.Time()
trans = self.tf_buffer.lookup_transform(
to_frame_rel,
from_frame_rel,
now)
except TransformException as ex:
self.get_logger().info(
f‘Could not transform {to_frame_rel} to {from_frame_rel}: {ex}‘)
return
msg = Twist()
scale_rotation_rate = 1.0
msg.angular.z = scale_rotation_rate * math.atan2(
trans.transform.translation.y,
trans.transform.translation.x)
scale_forward_speed = 0.5
msg.linear.x = scale_forward_speed * math.sqrt(
trans.transform.translation.x ** 2 +
trans.transform.translation.y ** 2)
self.publisher.publish(msg)
else:
if self.result.done():
self.get_logger().info(
f‘Successfully spawned {self.result.result().name}‘)
self.turtle_spawned = True
else:
self.get_logger().info(‘Spawn is not finished‘)
else:
if self.spawner.service_is_ready():
# Initialize request with turtle name and coordinates
# Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
request = Spawn.Request()
request.name = ‘turtle2‘
request.x = float(4)
request.y = float(2)
request.theta = float(0)
# Call request
self.result = self.spawner.call_async(request)
self.turtle_spawning_service_ready = True
else:
# Check if the service is ready
self.get_logger().info(‘Service is not ready‘)
def main():
rclpy.init()
node = FrameListener()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
rclpy.shutdown()
在setup.py的console_scripts加入:
‘turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main‘,
然后在重新编写turtle_tf2_demo.launch.py:
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package=‘turtlesim‘,
executable=‘turtlesim_node‘,
name=‘sim‘
),
Node(
package=‘learning_tf2_py‘,
executable=‘turtle_tf2_broadcaster‘,
name=‘broadcaster1‘,
parameters=[
{‘turtlename‘: ‘turtle1‘}
]
),
# new added
DeclareLaunchArgument(
‘target_frame‘, default_value=‘turtle1‘,
description=‘Target frame name.‘
),
Node(
package=‘learning_tf2_py‘,
executable=‘turtle_tf2_broadcaster‘,
name=‘broadcaster2‘,
parameters=[
{‘turtlename‘: ‘turtle2‘}
]
),
Node(
package=‘learning_tf2_py‘,
executable=‘turtle_tf2_listener‘,
name=‘listener‘,
parameters=[
{‘target_frame‘: LaunchConfiguration(‘target_frame‘)}
]
),
])
然后构建、运行:
cd dev_ws/src
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_py
. install/setup.bash
ros2 launch learning_tf2_py turtle_tf2_demo.launch.py
使用teleop_key控制小乌龟:
ros2 run turtlesim turtle_teleop_key
现在小乌龟的运动方式应该如之前的demo演示的样子了。
添加坐标系
由上面的frames.pdf的树结构明显地显示，tf2将坐标系之间的关系组织成树结构，即一个坐标系其只可有一个父坐标系，而其可以有多个子坐标系。因此，如果要加入一个新的坐标系， 需要以tf2树中的某一个坐标系为父坐标系。
首先，尝试增加一个固定的坐标系, 即让新坐标系相对其父节点不变(本例中是carrot1相对于turtle1), 让它成为小乌龟turtle2的目标:
from geometry_msgs.msg import TransformStamped
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
class FixedFrameBroadcaster(Node):
def __init__(self):
super().__init__(‘fixed_frame_tf2_broadcaster‘)
self.br = TransformBroadcaster(self)
self.timer = self.create_timer(0.1, self.broadcast_timer_callback)
def broadcast_timer_callback(self):
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = ‘turtle1‘
t.child_frame_id = ‘carrot1‘
t.transform.translation.x = 0.0
t.transform.translation.y = 2.0
t.transform.translation.z = 0.0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1.0
self.br.sendTransform(t)
def main():
rclpy.init()
node = FixedFrameBroadcaster()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
rclpy.shutdown()
代码解释:
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = ‘turtle1‘
t.child_frame_id = ‘carrot1‘
t.transform.translation.x = 0.0
t.transform.translation.y = 2.0
t.transform.translation.z = 0.0
将carrot1的父节点设为turtle1, 并将其位置设置为在turtle1沿y轴方向两米处。
在setup.py的console_scripts中加入:
‘fixed_frame_tf2_broadcaster = learning_tf2_py.fixed_frame_tf2_broadcaster:main‘,
再在learning_tf2_py/launch目录中编写launch文件turtle_tf2_fixed_frame_demo.launch.py:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
demo_nodes = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(‘learning_tf2_py‘), ‘launch‘),
‘/turtle_tf2_demo.launch.py‘]),launch_arguments={‘target_frame‘: ‘carrot1‘}.items(),
)
return LaunchDescription([
demo_nodes,
Node(
package=‘learning_tf2_py‘,
executable=‘fixed_frame_tf2_broadcaster‘,
name=‘fixed_broadcaster‘,
),
])
此launch文件在启动turtle_tf2_demo.launch.py后，又将我们刚刚创建的carrot1启动，请注意此句launch_arguments={‘target_frame‘: ‘carrot1‘}.items(),将参数target_frame设置为carrot1, 这是很关键的一句， 因为此句， 小乌龟turtle2不再追求小乌龟turtle1而改为追寻carrot1。:
构建并启动:
cd dev_ws/
colcon build --packages-select learning_tf2_py
. install/setup.bash
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py
此处也可不用在.launch.py文件中加入launch_arguments...这句，但需要在启动时remap target_frame到carrort1,具体地:
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1
然后在另一个终端使用:
ros2 run tf2_tools view_frames.py
点击frames.pdf: