#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
import math
import time

def draw_square():
    # 初始化ROS节点
    rospy.init_node('turtle_square_node', anonymous=True)
    
    # 创建Publisher，发布到turtle1的cmd_vel话题
    pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    
    # 设置循环频率
    rate = rospy.Rate(10)  # 10Hz
    
    # 创建Twist消息对象
    vel_msg = Twist()
    
    # 设置参数
    linear_speed = 1.0    # 线速度
    angular_speed = 1.0   # 角速度
    side_length = 2.0     # 正方形边长
    
    rospy.loginfo("开始让乌龟画正方形...")
    rospy.loginfo("按Ctrl+C停止")
    
    try:
        # 画4条边
        for i in range(4):
            rospy.loginfo(f"画第 {i+1} 条边...")
            
            # 直走：根据边长计算需要的时间
            move_time = side_length / linear_speed
            start_time = time.time()
            
            vel_msg.linear.x = linear_speed
            vel_msg.angular.z = 0.0
            
            while (time.time() - start_time) < move_time and not rospy.is_shutdown():
                pub.publish(vel_msg)
                rate.sleep()
            
            # 转向90度：计算需要的时间
            # 角度 = 角速度 * 时间，所以 时间 = 角度 / 角速度
            turn_time = (math.pi / 2) / angular_speed
            start_time = time.time()
            
            vel_msg.linear.x = 0.0
            vel_msg.angular.z = angular_speed
            
            while (time.time() - start_time) < turn_time and not rospy.is_shutdown():
                pub.publish(vel_msg)
                rate.sleep()
        
        rospy.loginfo("正方形绘制完成")
            
    except rospy.ROSInterruptException:
        rospy.loginfo("程序被用户中断")
    
    # 停止乌龟运动
    vel_msg.linear.x = 0.0
    vel_msg.angular.z = 0.0
    pub.publish(vel_msg)
    rospy.loginfo("乌龟停止运动")

if __name__ == '__main__':
    try:
        draw_square()
    except rospy.ROSInterruptException:
        pass