静态变换在机器人系统中非常有用,一般用来描述机器人基坐标系与相对其静止的其他坐标系之间的关系,比如雷达坐标系、相机坐标系等。本篇我们就来看一下,如何通过Python编程,实现静态坐标系变换的广播。
一、创建功能包
ros2 pkg create --build-type ament_python learning_tf2_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()
2.1 代码解析
from geometry_msgs.msg import TransformStamped
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
import tf_transformations
self._tf_publisher = StaticTransformBroadcaster(self)
self.make_transforms(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)
2.2 修改package.xml文件
<description>Examples of static transform broadcaster using rclcpp</description>
<maintainer email="huchunxu@guyuehome.com">Hu Chunxu</maintainer>
<license>Apache License 2.0</license>
<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>
2.3 添加程序入口
'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',
三、编译和运行
rosdep install -i --from-path src --rosdistro galactic -y
colcon build --packages-select learning_tf2_py

. install/setup.bash
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: 1622908754
nanosec: 208515730
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
四、静态坐标系变换更合适的广播方式
# 使用欧拉角描述姿态
ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
# 使用四元数描述姿态
ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id
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']
),
])
评论(0)
您还未登录,请登录后发表或查看评论