您必须为 urdf 文件中定义的所有关节广播您的 tf 转换。您可以使用 ROS 节点发布这些:
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
import math
class TFBroadcaster(Node):
def __init__(self):
super().__init__('tf_broadcaster')
# Create a transform broadcaster
self.broadcaster = TransformBroadcaster(self)
# Create a timer to publish transforms periodically
self.timer = self.create_timer(0.1, self.broadcast_transforms) # 10Hz
# Initialize joint states (you might want to subscribe to actual joint states)
self.slider_position = 0.0
self.arm_angle = 0.0
def broadcast_transforms(self):
current_time = self.get_clock().now().to_msg()
# Base link transform
t = TransformStamped()
t.header.stamp = current_time
t.header.frame_id = 'world'
t.child_frame_id = 'base_link'
t.transform.translation.x = 1.5
t.transform.translation.y = 1.0
t.transform.translation.z = 0.0
t.transform.rotation.w = 1.0
self.broadcaster.sendTransform(t)
# Slider link transform
t = TransformStamped()
t.header.stamp = current_time
t.header.frame_id = 'base_link'
t.child_frame_id = 'slider_link'
t.transform.translation.x = -1.25 + self.slider_position # Add slider position
t.transform.translation.y = 0.0
t.transform.translation.z = 0.1
t.transform.rotation.w = 1.0
self.broadcaster.sendTransform(t)
# Arm link transform
t = TransformStamped()
t.header.stamp = current_time
t.header.frame_id = 'slider_link'
t.child_frame_id = 'arm_link'
t.transform.translation.x = 0.25
t.transform.translation.y = 0.0
t.transform.translation.z = 0.15
# Convert arm angle to quaternion (rotation around Y axis)
t.transform.rotation.w = math.cos(self.arm_angle/2)
t.transform.rotation.x = 0.0
t.transform.rotation.y = -math.sin(self.arm_angle/2) # Negative because of axis direction
t.transform.rotation.z = 0.0
self.broadcaster.sendTransform(t)
# Camera link transform
t = TransformStamped()
t.header.stamp = current_time
t.header.frame_id = 'arm_link'
t.child_frame_id = 'camera_link'
t.transform.translation.x = 2.0 # arm_length
t.transform.translation.y = 0.0
t.transform.translation.z = 0.175 # arm_radius + 0.075
t.transform.rotation.w = 1.0
self.broadcaster.sendTransform(t)
def main():
rclpy.init()
node = TFBroadcaster()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
启动包
ros2 run <your-package> <your-node>