Untitled

#!/usr/bin/env python
# encoding: utf8

import rospy
import turtlesim
from turtlesim.msg import Pose
from turtlesim.srv import SetPenRequest
from TurtlesimSIU import TurtlesimSIU
from geometry_msgs.msg import Twist
import math
import signal
import sys

def signal_handler(sig, frame):
    print("Terminating")
    sys.exit(0)

def calculate_vel_t_start(y):
    if 15 <= y <= 20:
        return (1.8 * (y - 15) + 1)
    elif 20 < y <= 25:
        return 10 - 1.8 * (y - 20)
    else:
        return 1

def green(v):
    return min(int(25.5 * (v - 1)), 255)

def blue(v):
    return int(255 - green(v))

if __name__ == "__main__":
    signal.signal(signal.SIGINT, signal_handler)
    rospy.init_node('siu_example', anonymous=False)

    turtle_api = TurtlesimSIU.TurtlesimSIU()
    rate = rospy.Rate(10)

    if turtle_api.hasTurtle('turtle1'):
        turtle_api.killTurtle('turtle1')

    turtle_api.spawnTurtle('turtle1', turtlesim.msg.Pose(x=30, y=15, theta=(math.pi / 2)))

    # Start: pionowa kreska T
    vel = Twist()
    pose = turtle_api.getPose('turtle1')

    while pose.y < 25:
        vel.linear.x = calculate_vel_t_start(pose.y)
        vel.angular.z = 0.0
        turtle_api.setVel('turtle1', vel)

        req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=3, off=0)
        turtle_api.setPen('turtle1', req)

        pose = turtle_api.getPose('turtle1')
        rate.sleep()

    # Zakręt łukiem w prawo (góra litery T)
    vel.linear.x = 2.5
    vel.angular.z = -1.5
    turn_start = rospy.Time.now().to_sec()
    while rospy.Time.now().to_sec() - turn_start < 1.2:
        turtle_api.setVel('turtle1', vel)
        req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=3, off=0)
        turtle_api.setPen('turtle1', req)
        pose = turtle_api.getPose('turtle1')
        rate.sleep()

    # Poprzeczka pozioma w prawo
    vel.linear.x = 2.0
    vel.angular.z = 0.0
    move_start = rospy.Time.now().to_sec()
    while rospy.Time.now().to_sec() - move_start < 2.5:
        turtle_api.setVel('turtle1', vel)
        req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=3, off=0)
        turtle_api.setPen('turtle1', req)
        pose = turtle_api.getPose('turtle1')
        rate.sleep()