ROS2的turtlesim画一个矩形

ROS2中的`turtlesim`是一个简单的3D机器人模拟环境，它主要用于教学和演示ROS的基本概念。要在`turtlesim`环境中绘制一个矩形，你可以使用`geometry_msgs/PoseArray`消息来控制turtle（海龟）的位置和姿态，以及`std_msgs/Header`来表示消息的时间戳。

首先，你需要创建一个名为`draw_rectangle.py`的Python脚本，然后导入必要的库，如`rclpy`, `geometry_msgs` 和 `tf2_ros`。接下来，编写代码来初始化节点、订阅和发布消息：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, PoseArray, Twist
from std_msgs.msg import Header
from tf2_ros import TransformBroadcaster

class DrawRectangle(Node):
    def __init__(self):
        super().__init__('draw_rectangle')

        # 创建消息和订阅者
        self.turtle_pose = PoseArray()
        self.create_subscription(
            PoseArray,
            '/turtle1/pose',
            self.update_turtle_pose,
            10)  # 订阅频率设置为每秒10次

        # 初始化变换广播器
        self.tf_broadcaster = TransformBroadcaster(self)
        
        # 矩形尺寸和位置的初始值
        self.rectangle_length = 1.0
        self.rectangle_width = 0.5
        self.rectangle_center_x = 0.0
        self.rectangle_center_y = 0.0

    def update_turtle_pose(self, msg: PoseArray):
        # 更新海龟位置到矩形中心点
        turtle_pose = PoseStamped()
        turtle_pose.header = msg.header
        turtle_pose.pose.position.x = self.rectangle_center_x
        turtle_pose.pose.position.y = self.rectangle_center_y

        # 发布变换，使得turtle看起来移动了
        transform = self.get_transform('world', 'turtle1', msg.header.stamp)
        turtle_pose.pose.position.x += transform.transform.translation.x
        turtle_pose.pose.position.y += transform.transform.translation.y

        # 以海龟为中心，画出矩形边框
        poses = [PoseStamped()]
        for side in range(4):
            angle = (side * math.pi / 2) % (math.pi * 2)
            pose = PoseStamped()
            pose.header = msg.header
            pose.pose.position.x = turtle_pose.pose.position.x + math.cos(angle) * self.rectangle_length / 2
            pose.pose.position.y = turtle_pose.pose.position.y + math.sin(angle) * self.rectangle_length / 2
            poses.append(pose)

            pose = PoseStamped()
            pose.header = msg.header
            pose.pose.position.x = turtle_pose.pose.position.x - math.cos(angle) * self.rectangle_width / 2
            pose.pose.position.y = turtle_pose.pose.position.y - math.sin(angle) * self.rectangle_width / 2
            poses.append(pose)

        self.publish_turtle_poses(poses)

    def publish_turtle_poses(self, poses: list):
        self.turtle_pose.poses = poses
        self.publish('/turtle1/pose', self.turtle_pose)

def main(args=None):
    rclpy.init(args=args)
    node = DrawRectangle()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

运行此脚本，并在`turtlesim`环境中订阅 `/turtle1/pose`话题，可以看到一个动态变化的矩形。注意这只是一个基础示例，实际应用中可能需要添加动画效果或者更复杂的轨迹规划。