ROS2获取map和base_link之间的机器人角度

要获取map和base_link之间的机器人角度，可以使用ROS2中的tf2库。tf2库提供了一个TransformListener类，它可以订阅tf2变换，并提供了一些方法来查询和转换变换。

以下是获取机器人角度的步骤：

1. 创建一个TransformListener对象：

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)

2. 在机器人运行时，订阅tf2中的/map到/base_link变换：

try:
    trans = tf_buffer.lookup_transform('map', 'base_link', rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
    rospy.logwarn("Failed to get transform from map to base_link")

3. 从变换中获取机器人的角度：

roll, pitch, yaw = tf.transformations.euler_from_quaternion([trans.transform.rotation.x, trans.transform.rotation.y,
                                                              trans.transform.rotation.z, trans.transform.rotation.w])

其中，roll、pitch和yaw是机器人在x、y和z轴上的旋转角度。

完整的代码示例如下：

import rospy
import tf2_ros
import tf

rospy.init_node('tf_listener')

tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)

while not rospy.is_shutdown():
    try:
        trans = tf_buffer.lookup_transform('map', 'base_link', rospy.Time())
        roll, pitch, yaw = tf.transformations.euler_from_quaternion([trans.transform.rotation.x, trans.transform.rotation.y,
                                                                      trans.transform.rotation.z, trans.transform.rotation.w])
        rospy.loginfo("Robot angle: %.2f degrees", yaw*180/3.14)
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
        rospy.logwarn("Failed to get transform from map to base_link")
    rospy.sleep(0.1)