如何用rospy实现ros小海龟在（5.54,5.54），（5.54,8.54），（6.54,7.54），（5.54,6.54），（5.54,5.54）这几个坐标点中循环移动，要求是小海龟先转弯后直行，且能实现闭环控制

下面是一个简单的示例程序，演示如何使用rospy控制小海龟在指定坐标点间循环移动，其中包括转弯和直行。该程序使用turtlebot3作为小海龟模型，并使用了turtlebot3_msgs包中的一些消息类型。请确保已正确安装turtlebot3和相关软件包，且已连接小海龟。

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty
from math import atan2, pi

class TurtlebotController:
    def __init__(self):
        rospy.init_node('turtlebot_controller', anonymous=False)

        self.positions = [(5.54, 5.54), (5.54, 8.54), (6.54, 7.54), (5.54, 6.54), (5.54, 5.54)]
        self.current_position = None
        self.current_index = 0
        self.goal_tolerance = 0.1
        self.move_speed = 0.2
        self.rotate_speed = 0.2
        self.min_distance = 0.2
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.rate = rospy.Rate(10)

    def run(self):
        rospy.loginfo('Starting turtlebot_controller node...')

        # Wait for the gazebo simulation to start
        rospy.wait_for_service('/gazebo/unpause_physics')
        unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
        unpause_gazebo()

        while not rospy.is_shutdown():
            if self.current_position is None:
                self.current_position = self.positions[self.current_index]
                rospy.loginfo('Moving to position: (%.2f, %.2f)' % self.current_position)

            distance_to_goal = self.distance_to_goal()
            if distance_to_goal < self.goal_tolerance:
                self.current_index = (self.current_index + 1) % len(self.positions)
                self.current_position = None
                self.stop_moving()
                continue

            angle_to_goal = self.angle_to_goal()
            if abs(angle_to_goal) > 0.1:
                self.rotate(angle_to_goal)
            else:
                self.move_forward(distance_to_goal)

            self.rate.sleep()

    def move_forward(self, distance):
        twist_msg = Twist()
        twist_msg.linear.x = self.move_speed
        twist_msg.angular.z = 0.0
        self.cmd_vel_pub.publish(twist_msg)

    def rotate(self, angle):
        twist_msg = Twist()
        twist_msg.linear.x = 0.0
        twist_msg.angular.z = self.rotate_speed if angle > 0 else -self.rotate_speed
        self.cmd_vel_pub.publish(twist_msg)

    def stop_moving(self):
        twist_msg = Twist()
        twist_msg.linear.x = 0.0
        twist_msg.angular.z = 0.0
        self.cmd_vel_pub.publish(twist_msg)

    def distance_to_goal(self):
        x, y = self.current_position
        return ((x - self.current_pose.x) ** 2 + (y - self.current_pose.y) ** 2) ** 0.5

    def angle_to_goal(self):
        x, y = self.current_position
        angle_to_goal = atan2(y - self.current_pose.y, x - self.current_pose.x) - self.current_pose.theta
        if angle_to_goal > pi:
            angle_to_goal -= 2 * pi
        elif angle_to_goal < -pi:
            angle_to_goal += 2 * pi
        return angle_to_goal

    def pose_callback(self, msg):
        self.current_pose = msg.pose.position

if __name__ == '__main__':
    try:
        controller = TurtlebotController()
        rospy.Subscriber('/pose', PoseStamped, controller.pose_callback)
        controller.run()
    except rospy.ROSInterruptException:
        pass

在该程序中，我们使用了一个TurtlebotController类，其中包括一些用于控制小海龟移动的方法，例如move_forward和rotate。我们还定义了一些参数，例如move_speed和rotate_speed，用于控制小海龟的移动速度和转弯速度。在run方法中，我们使用一个while循环来不断更新小海龟的位置和姿态，并根据当前位置和目标位置之间的距离和角度来调用适当的移动方法。当小海龟到达目标位置时，我们将目标位置更新为下一个位置，并重置当前位置。我们还在程序开头使用rospy.wait_for_service等待gazebo模拟器启动，以确保小海龟已正确初始化。最后，我们使用rosrun命令运行该程序，并通过在终端中输入“rostopic pub /cmd_vel geometry_msgs/Twist”来手动控制小海龟移动。