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_curve(t, duration):
    midpoint = duration / 2.0
    if t <= midpoint:
        return 1 + (9.0 * (t / midpoint))
    else:
        return 10 - (9.0 * ((t - midpoint) / midpoint))

def clamp_color(val):
    return max(0, min(int(val), 255))

def green(v):
    return clamp_color(25.5 * (v - 1))

def blue(v):
    return clamp_color(255 - 25.5 * (v - 1))

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)))

    vel = Twist()
    pose = turtle_api.getPose('turtle1')

    # Rysowanie pionowej kreski
    while pose.y < 25:
        vel.linear.x = 1.8 * (pose.y - 15) + 1 if pose.y <= 20 else 10 - 1.8 * (pose.y - 20)
        vel.angular.z = 0.0
        turtle_api.setVel('turtle1', vel)
        req = 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()

    # Zapisz pozycję startową łuku
    start_pose = pose

    # Wyłączenie pisaka i cofnięcie się w bok (prawo)
    vel.linear.x = 0
    vel.angular.z = 0
    req = SetPenRequest(r=0, g=0, b=0, width=3, off=1)
    turtle_api.setPen('turtle1', req)
    for _ in range(10):
        vel.linear.x = 1.5
        vel.angular.z = 0
        turtle_api.setVel('turtle1', vel)
        rate.sleep()

    # Prawy łuk
    req = SetPenRequest(r=0, g=0, b=255, width=3, off=0)
    turtle_api.setPen('turtle1', req)

    duration = 0.7
    t0 = rospy.Time.now().to_sec()
    while True:
        t = rospy.Time.now().to_sec() - t0
        if t > duration:
            break
        vel.linear.x = calculate_vel_curve(t, duration)
        vel.angular.z = -1.9
        turtle_api.setVel('turtle1', vel)
        req = SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=3, off=0)
        turtle_api.setPen('turtle1', req)
        rate.sleep()

    # Wyłączenie pisaka i cofnięcie się z powrotem w lewo
    req = SetPenRequest(r=0, g=0, b=0, width=3, off=1)
    turtle_api.setPen('turtle1', req)
    for _ in range(20):
        vel.linear.x = -1.5
        vel.angular.z = 0
        turtle_api.setVel('turtle1', vel)
        rate.sleep()

    # Lewy łuk (z pisakiem)
    req = SetPenRequest(r=0, g=0, b=255, width=3, off=0)
    turtle_api.setPen('turtle1', req)

    duration = 0.7
    t0 = rospy.Time.now().to_sec()
    while True:
        t = rospy.Time.now().to_sec() - t0
        if t > duration:
            break
        vel.linear.x = calculate_vel_curve(t, duration)
        vel.angular.z = 1.9
        turtle_api.setVel('turtle1', vel)
        req = SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=3, off=0)
        turtle_api.setPen('turtle1', req)
        rate.sleep()