#!/usr/bin/env python
import rospy
import math
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
global flag
flag = False
def callback(msg):
    if msg.linear_velocity == 0 and msg.angular_velocity != 0:
        rospy.loginfo("x: %.2f, y: %.2f" % (msg.x, msg.y))

def move():
    rospy.init_node('robot', anonymous=True)
    velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    sub = rospy.Subscriber('/turtle1/pose', Pose, callback)
    speed = 0.5
    vel_msg = Twist()
    vel_msg.linear.x = abs(speed)
    vel_msg.linear.y = 0
    vel_msg.linear.z = 0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0
    pose = Pose()
    num_of_ver = input("Kol-vo vershin: ")
    distance = input("Dlina:")

    for i in range(num_of_ver):
        q=(360/num_of_ver)*2*math.pi/360
        t0 = rospy.Time.now().to_sec()
        current_distance = 0
        while(current_distance < distance):
            velocity_publisher.publish(vel_msg)
            t1=rospy.Time.now().to_sec()
            current_distance = speed*(t1-t0)
        vel_msg.angular.z = abs(speed)
        vel_msg.linear.x = 0
        current_angle = 0
        t0 = rospy.Time.now().to_sec()
        while(current_angle < q):
            velocity_publisher.publish(vel_msg)
            t1 = rospy.Time.now().to_sec()
            current_angle = 0.5*(t1-t0)
        vel_msg.angular.z = 0
        vel_msg.linear.x = speed
    velocity_publisher.publish(vel_msg)
if __name__ == '__main__':
    try:
        move()
    except rospy.ROSInterruptException: pass