ros2 将2d位姿转换成nav2 geometry_msgs 的demo

你可以使用ROS2的tf2库来进行2D位姿转换，然后将其转换为nav2的geometry_msgs消息类型。下面是一个简单的示例代码：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
import tf2_ros
from tf2_geometry_msgs import do_transform_pose

class PoseTransformNode(Node):
    def __init__(self):
        super().__init__('pose_transform_node')
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
        self.pose_sub = self.create_subscription(
            PoseStamped,
            'input_pose',
            self.pose_callback,
            10
        )
        self.pose_pub = self.create_publisher(
            PoseStamped,
            'output_pose',
            10
        )

    def pose_callback(self, msg):
        try:
            transform = self.tf_buffer.lookup_transform(
                'map',  # 目标坐标系
                msg.header.frame_id,  # 源坐标系
                msg.header.stamp,  # 时间戳
                rclpy.duration.Duration(seconds=1.0)  # 超时时间
            )
            transformed_pose = do_transform_pose(msg, transform)
            self.pose_pub.publish(transformed_pose)
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            self.get_logger().error('Transform lookup failed!')

def main(args=None):
    rclpy.init(args=args)
    pose_transform_node = PoseTransformNode()
    rclpy.spin(pose_transform_node)
    pose_transform_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

这个示例代码创建了一个ROS2节点，订阅名为`input_pose`的PoseStamped消息，在回调函数中进行坐标转换，并将转换后的消息发布到名为`output_pose`的话题上。你需要将目标坐标系修改为你使用的坐标系，例如`map`。

希望这个示例对你有所帮助！