持续创作，加速成长！这是我参与「日新计划 6 月更文挑战」的第28天，点击查看活动详情
tf2介绍
安装依赖
sudo apt-get install ros-foxy-turtle-tf2-py ros-foxy-tf2-tools
transforms3d（它提供四元数python编程和欧拉角转换功能）
pip3 install transforms3d
运行结python保留字果
启动turtle_tf2_demo.launch.py文件
ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py
键盘控制turtlesim
ros2 run turtlesim turtle_teleop_key
使python保留字用view_frames
ros2 run tf2_tools view_frames
生成的文件
使python123平台登录用tf2_echo(报告通过 ROS 广播的任意两帧之间的转换)
ros2 run tf2_ros tf2_echo [reference_frame] [target_frame]
turtle2框架相对于turtle1框架的变换
ros2 run tf2_ros tf2_echo turtle2 turtle1
可以在终端显示turtpython保留字le2框架相对于turtle1框架的变换
tf2广播器(broadcaster)
先决条件
创建工作python怎么读区python保留字间和功能包
创建一个turtle_tpython基础教程f2_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()
创建一个tupython保留字rtle_tf2_broadcaster.launch.py文件Python
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='py_package',
executable='turtle_tf2_broadcaster',
name='broadcaster1',
parameters=[
{'turtlename': 'turtle1'}
]
),
])
添加依赖
修改setup.py文件
'console_scripts': [
'turtle_tf2_broadcaster = py_package.turtle_tf2_broadcaster:main',
],
data_files=[
...
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
],
修改package.xml文件
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
编译
colcon build
. install/setup.bash
运行结果
运python怎么读行turtle_tf2_broadcaster.launch.py文件
(同时启动turtlesim节点和turtle_tfpython安装教程2_broadcaster节点)
ros2 launch learning_tf2_py turtle_tf2_broadcaster.launch.py
键盘控制turtlesim
ros2 run turtlesim turtle_teleop_key
使用tf2_echopython安装教程工具检测海龟位姿是否广播到tf2
ros2 run tf2_ros tf2_echo world turtle1
可以在终端显示world框架相对于turtle1框架的变换
tf2侦听器(listener)
创建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()
创建turtlepython123_tf2_listener.launch.py文件
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='py_package',
executable='turtle_tf2_listener',
name='listener',
parameters=[
{'target_frame': 'turtle2'}
]
),
])
添加依赖
'console_scripts': [
'turtle_tf2_listener = py_package.turtle_tf2_listener:main',
],
编译
colcon build
. install/setup.bash
运行Python结果
运行turtle_tf2_broadcaster.launch.ppython语言y文件
ros2 launch learning_tf2_py turtle_tf2_broadcaster.launch.py
运行turtle_tf2_listener.launch.py文件
ros2 launch py_package turtle_tf2_listener.launch.py
键盘控制turtlesim
ros2 run turtlesim turtle_teleop_key
