ros2乌龟画圆角矩形

在ROS 2 (Robot Operating System) 中，乌龟图元通常用于演示简单的机器人行为或者可视化操作。要让乌龟绘制带圆角的矩形，你可以使用` turtlesim`包中的`turtlebot2d`节点，并配合`geometry_msgs/PoseArray`消息。以下是一个简单的步骤：

1. 首先，确保已安装`turtlesim`和相关依赖。可以使用`ros2 pkg list | grep turtlesim`检查是否已经安装。

2. 创建一个Python脚本，例如`draw_round_rectangle.py`，导入必要的库并设置乌龟的位置、方向以及圆角半径。

import rclpy
from rclpy.node import Node
from turtlesim.srv import SetPen, ClearScreen
from geometry_msgs.msg import PoseArray

class DrawRoundRectangle(Node):
    def __init__(self):
        super().__init__('draw_round_rectangle')
        self.set_pen_client = self.create_client(SetPen, 'turtle/set_pen')
        self.clear_screen_client = self.create_client(ClearScreen, 'turtle/clear')

        self.rounded_rectangle_pose_array = PoseArray()
        # 定义矩形的尺寸和圆角半径
        self.width = 5.0
        self.height = 4.0
        self.radius = 0.5

    def draw_rounded_rectangle(self):
        # 设置圆角笔
        request = SetPen.Request()
        request.color = 255
        request.fill = True
        request.width = 1.0
        self.get_logger().info('Setting pen for rounded rectangle...')
        future = self.set_pen_client.call_async(request)
        self.create_future(future)

        # 生成带圆角的矩形路径
        poses = []
        for x in range(int(self.width / self.radius)):
            for y in range(int(self.height / self.radius)):
                pose = PoseArray.poses.append(
                    Pose(position=self.create_point(x * self.radius, y * self.radius), orientation=Pose.orientation.z))
                poses.append(pose)

        # 发送每个点作为pose到Turtlebot
        for pose in poses:
            future = self.publish_pose(pose)
            self.create_future(future)

        # 清除画布
        request = ClearScreen.Request()
        self.clear_screen_client.call_async(request)

    def create_point(self, x, y):
        return Point(x=x, y=y)

    def publish_pose(self, pose_msg):
        future = self.get_node_base_interface().get_topic('/turtle_pose').publish(pose_msg)
        return future

def main(args=None):
    rclpy.init(args=args)
    node = DrawRoundRectangle()
    try:
        node.draw_rounded_rectangle()
    except Exception as e:
        node.get_logger().error('Error occurred while drawing:', cause=e)
    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    main()

运行这个脚本，乌龟会按照矩形路径绘制出带圆角的形状。注意，实际效果可能会因为乌龟移动的速度限制而显得不太圆滑。