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

def draw_circle():
    # 初始化ROS节点
    rospy.init_node('turtle_circle_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   # 角速度
    radius = 1.0          # 圆半径
    
    # 计算合适的线速度和角速度比例，使乌龟画圆
    vel_msg.linear.x = linear_speed
    vel_msg.linear.y = 0.0
    vel_msg.linear.z = 0.0
    vel_msg.angular.x = 0.0
    vel_msg.angular.y = 0.0
    vel_msg.angular.z = angular_speed
    
    rospy.loginfo("开始让乌龟画圆...")
    rospy.loginfo("按Ctrl+C停止")
    
    try:
        while not rospy.is_shutdown():
            # 发布速度指令
            pub.publish(vel_msg)
            
            # 按照设定频率循环
            rate.sleep()
            
    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_circle()
    except rospy.ROSInterruptException:
        pass