ROS实现小乌龟做五边形运动，并输出位姿

好的，以下是实现小乌龟做五边形运动并输出位姿的 ROS Python 代码：

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

class TurtleController():
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('turtle_controller', anonymous=True)

        # 创建小乌龟速度发布者
        self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

        # 创建小乌龟位姿订阅者
        self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, self.pose_callback)

        # 初始化小乌龟位置和朝向角度
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # 休眠1秒等待ROS节点初始化完成
        rospy.sleep(1)

    def pose_callback(self, pose):
        # 更新小乌龟位置和朝向角度
        self.x = pose.x
        self.y = pose.y
        self.theta = pose.theta

    def move(self):
        # 创建Twist消息并设置线速度和角速度
        vel_msg = Twist()
        vel_msg.linear.x = 1.0
        vel_msg.angular.z = 72.0 / 180.0 * 3.1416

        # 计算五边形需要移动的时间
        time = 100.0 / vel_msg.linear.x

        # 获取当前时间
        start_time = rospy.Time.now().to_sec()

        # 循环移动小乌龟
        while rospy.Time.now().to_sec() - start_time < time:
            self.velocity_publisher.publish(vel_msg)

        # 停止小乌龟运动
        vel_msg.linear.x = 0.0
        vel_msg.angular.z = 0.0
        self.velocity_publisher.publish(vel_msg)

        # 输出小乌龟的位置和朝向角度
        rospy.loginfo("位姿：x={}, y={}, theta={}".format(self.x, self.y, self.theta))

if __name__ == '__main__':
    try:
        # 创建TurtleController对象并移动小乌龟
        turtle_controller = TurtleController()
        turtle_controller.move()
    except rospy.ROSInterruptException:
        pass

运行代码后，小乌龟会在ROS环境下画出一个五边形，并输出小乌龟的位置和朝向角度。注意需要在另一个终端中运行`roscore`命令开启ROS环境。