【ROS】功能包订阅与发布TF坐标变换关系

1 发布map->base_link变换关系（仅用于本次测试）

#!/usr/bin/env python
import rospy
import tf_conversions
import tf2_ros
import geometry_msgs.msg

def handle_pose():
	br = tf2_ros.TransformBroadcaster()
	t = geometry_msgs.msg.TransformStamped()
	t.header.stamp = rospy.Time.now()
	t.header.frame_id = "map"
	t.child_frame_id = "base_link"
	t.transform.translation.x = 0
	t.transform.translation.y = 0
	t.transform.translation.z = 0.0
	q = tf_conversions.transformations.quaternion_from_euler(0, 0, 0)
	t.transform.rotation.x = q[0]
	t.transform.rotation.y = q[1]
	t.transform.rotation.z = q[2]
	t.transform.rotation.w = q[3]
	br.sendTransform(t)

if __name__ == '__main__':
	rospy.init_node('tf2_broadcaster')
	while not rospy.is_shutdown():
		handle_pose()
		rospy.sleep(1)

2 订阅map->base_link变换关系

#!/usr/bin/env python
import rospy
import tf2_ros

if __name__ == '__main__':
	rospy.init_node('tf2_listener')
	tfBuffer = tf2_ros.Buffer()
	listener = tf2_ros.TransformListener(tfBuffer)
	rate = rospy.Rate(10) # 设置循环频率为10Hz
	while not rospy.is_shutdown():
		try:
			transform = tfBuffer.lookup_transform("map", "base_link", rospy.Time())
			print(transform)
		except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
			rospy.logwarn("Failed to lookup transform")
			rate.sleep()

效果：