Untitled

#!/usr/bin/env python

import rospy
import math
import signal
import sys

from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from turtlesim.srv import SetPenRequest
from TurtlesimSIU import TurtlesimSIU

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

def calculate_velocity(pos, start, mid, end):
    if start <= pos <= mid:
        return 1 + 9 * ((pos - start) / (mid - start))
    elif mid < pos <= end:
        return 1 + 9 * ((end - pos) / (end - mid))
    return 1

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

def blue(v):
    return max(min(255 - green(v), 255), 0)

def draw_vertical(turtle_api):
    vel = Twist()
    pose = turtle_api.getPose('turtle1')

    while pose.y < 25:
        v = calculate_velocity(pose.y, 15, 20, 25)
        vel.linear.x = v
        vel.angular.z = 0
        turtle_api.setVel('turtle1', vel)

        req = SetPenRequest(r=0, g=green(v), b=blue(v), width=3, off=0)
        turtle_api.setPen('turtle1', req)

        pose = turtle_api.getPose('turtle1')

def draw_arc(turtle_api, direction='left'):
    # Setup
    vel = Twist()
    pose = turtle_api.getPose('turtle1')
    radius = 2.0
    angle_span = math.pi / 2  # 90 degrees
    arc_length = radius * angle_span
    traveled = 0

    # Direction
    sign = -1 if direction == 'left' else 1
    vel.linear.y = 0

    # Estimate how far we've gone
    rate = rospy.Rate(20)
    last_pose = pose
    while traveled < arc_length:
        pose = turtle_api.getPose('turtle1')

        # Track arc progress
        dx = pose.x - last_pose.x
        dy = pose.y - last_pose.y
        step = math.sqrt(dx**2 + dy**2)
        traveled += step
        last_pose = pose

        v = calculate_velocity(traveled, 0, arc_length / 2, arc_length)
        vel.linear.x = v
        vel.angular.z = sign * v / radius

        turtle_api.setVel('turtle1', vel)

        req = SetPenRequest(r=0, g=green(v), b=blue(v), width=3, off=0)
        turtle_api.setPen('turtle1', req)

        rate.sleep()

    # Stop
    vel.linear.x = 0
    vel.angular.z = 0
    turtle_api.setVel('turtle1', vel)

def move_to_start_of_arc(turtle_api, offset):
    pose = turtle_api.getPose('turtle1')

    vel = Twist()
    vel.linear.x = -2
    vel.angular.z = 0
    turtle_api.setPen('turtle1', SetPenRequest(r=0, g=0, b=0, width=3, off=1))  # pisak off

    rate = rospy.Rate(10)
    while pose.x > offset:
        turtle_api.setVel('turtle1', vel)
        pose = turtle_api.getPose('turtle1')
        rate.sleep()

    vel.linear.x = 0
    turtle_api.setVel('turtle1', vel)
    turtle_api.setPen('turtle1', SetPenRequest(r=0, g=0, b=255, width=3, off=0))  # pisak on

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

    turtle_api = TurtlesimSIU.TurtlesimSIU()

    if turtle_api.hasTurtle('turtle1'):
        turtle_api.killTurtle('turtle1')
    turtle_api.spawnTurtle('turtle1', x=30, y=15, theta=math.pi / 2)

    # Start from clean
    turtle_api.clear()

    # Rysuj pion
    draw_vertical(turtle_api)

    # Lewy łuk
    move_to_start_of_arc(turtle_api, 27)
    draw_arc(turtle_api, direction='right')

    # Wróć do środka (pisak off), przesuwamy się do 30
    move_to_start_of_arc(turtle_api, 30)

    # Prawy łuk
    draw_arc(turtle_api, direction='left')