如何写一个rospy代码，让ros小海龟在(5.54,5.54), (5.54,8.54), (6.54,7.54), (5.54,6.54), (5.54,5.54)循环定点移动，要求小海龟先转弯后直行

以下是一个示例代码，可以让ROS小海龟在给定的5个点之间循环移动，每次到达一个点后，小海龟会先转弯再向下一个点移动。

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

# 定义目标点
points = [(5.54, 5.54), (5.54, 8.54), (6.54, 7.54), (5.54, 6.54), (5.54, 5.54)]

# 初始化当前目标点和小海龟的目标角度
current_point = 0
target_angle = 0

# 定义回调函数，获取小海龟的当前位置
def pose_callback(pose):
    global current_point, target_angle

    x = pose.x
    y = pose.y

    # 计算小海龟当前位置与目标点之间的距离
    distance_to_target = ((points[current_point][0] - x) ** 2 + (points[current_point][1] - y) ** 2) ** 0.5

    # 如果小海龟已经到达了当前目标点，则更新目标点和目标角度
    if distance_to_target < 0.1:
        current_point = (current_point + 1) % len(points)
        target_angle = get_target_angle(x, y, points[current_point][0], points[current_point][1])

    # 计算小海龟需要转动的角度
    angle_to_target = get_angle_to_target(pose.theta, target_angle)

    # 发布速度控制指令
    cmd_vel = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    vel_msg = Twist()
    vel_msg.linear.x = 1.0
    vel_msg.angular.z = angle_to_target
    cmd_vel.publish(vel_msg)

# 计算小海龟需要转动的角度
def get_angle_to_target(current_angle, target_angle):
    angle = target_angle - current_angle
    if angle > 3.14159:
        angle -= 2 * 3.14159
    elif angle < -3.14159:
        angle += 2 * 3.14159
    return angle

# 计算小海龟需要转向的目标角度
def get_target_angle(x1, y1, x2, y2):
    return math.atan2(y2 - y1, x2 - x1)

if __name__ == '__main__':
    rospy.init_node('turtle_waypoint_navigation', anonymous=True)

    # 订阅小海龟的位置信息
    pose_sub = rospy.Subscriber('/turtle1/pose', Pose, pose_callback)

    rospy.spin()

需要注意的是，这个代码中假设小海龟的初始位置在第一个目标点处。如果小海龟的初始位置不在第一个目标点处，需要在代码中进行一些修改。