Untitled

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

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

def draw_T(turtle_api):
    # Narysuj pionową linię (linia prosta)
    vel = Twist()
    vel.linear.x = 2
    turtle_api.setVel('turtle1', vel)
    
    # Rysowanie pionowej linii od y = 15 do y = 25
    for i in range(10):
        pose = turtle_api.getPose('turtle1')
        turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x, y=pose.y + 1, theta=pose.theta))  # Przesuwamy żółwia do góry
        rospy.sleep(0.1)
    
    # Teraz rysujemy poziomą linię
    vel.linear.x = 0  # Zatrzymujemy żółwia na chwilę, żeby rysować poziomo
    turtle_api.setVel('turtle1', vel)
    
    # Obrot o 90° w prawo
    turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x, y=pose.y, theta=math.pi / 2))  # Obrót o 90 stopni
    
    # Rysowanie poziomej linii od lewej do prawej
    vel.linear.x = 2  # Ustawiamy prędkość na 2 do rysowania poziomej linii
    turtle_api.setVel('turtle1', vel)
    for i in range(5):  # Narysuj linię poziomą
        pose = turtle_api.getPose('turtle1')
        turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x + 1, y=pose.y, theta=pose.theta))  # Rysowanie poziomo
        rospy.sleep(0.1)

def draw_W(turtle_api):
    # Narysuj literę W, składającą się z kilku linii
    vel = Twist()
    vel.linear.x = 2
    turtle_api.setVel('turtle1', vel)
    
    # Pierwsza linia diagonalna (od lewej do prawej)
    for i in range(7):
        pose = turtle_api.getPose('turtle1')
        turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x + 1, y=pose.y - 1, theta=pose.theta))  # Linia w dół i w prawo
        rospy.sleep(0.1)
    
    # Druga linia diagonalna (od prawej do lewej)
    for i in range(7):
        pose = turtle_api.getPose('turtle1')
        turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x + 1, y=pose.y + 1, theta=pose.theta))  # Linia w górę i w prawo
        rospy.sleep(0.1)
    
    # Trzecia linia pozioma (przechodzimy do końca "W")
    vel.linear.x = 2  # Ustawiamy prędkość na 2
    turtle_api.setVel('turtle1', vel)
    for i in range(7):  # Rysujemy poziomo
        pose = turtle_api.getPose('turtle1')
        turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x + 1, y=pose.y, theta=pose.theta))  # Linia pozioma
        rospy.sleep(0.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)

    # Ustawienia pióra (rysowanie)
    set_pen_req = turtlesim.srv.SetPenRequest(r=0, g=0, b=255, width=3, off=0)
    turtle_api.setPen('turtle1', set_pen_req)

    # Narysuj "T"
    draw_T(turtle_api)
    rospy.sleep(1)
    
    # Narysuj "W"
    draw_W(turtle_api)

    rospy.spin()