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

from sensor_msgs.msg import Image

import math

import signal

import sys

import numpy as np




VISUALIZE = True



def signal_handler(sig, frame):

    print ("Terminating")

    sys.exit(0)
    
        
def green(v):
    value = min(int(25.5 * (v - 1)), 255)
    return (value if value in range(0, 255) else 0)
def blue(v):
    return int(255 - green(v))



if __name__ == "__main__":

    # Initialize ROS node
    
    signal.signal(signal.SIGINT, signal_handler)

    rospy.init_node('siu_example', anonymous=False)

    turtle_api = TurtlesimSIU.TurtlesimSIU()

    rate = rospy.Rate(10)

    set_pen_req = turtlesim.srv.SetPenRequest(r=255, g=255, b=255, width=5, off=0)

    #set_pen_req = turtlesim.srv.SetPenRequest(r=255, g=255, b=255, width=5, off=1)

    if turtle_api.hasTurtle('turtle1'):

        turtle_api.killTurtle('turtle1')

        # rospy.sleep(2)

    if not turtle_api.hasTurtle('turtle1'):

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

    req = turtlesim.srv.SetPenRequest(r=0, g=0, b=255, width=5, off=0)
    turtle_api.setPen('turtle1', req)
    
    
    # Rysowanie "T"
    vel = Twist()
    vel.linear.x = 1
    vel.linear.y = 0
    vel.angular.z = 0
    
    pose = turtle_api.getPose('turtle1')
    
    # Rysujemy pionową linię (dodałem zakrzywienie z uwagi na treść zadania)
    while pose.y < 25:
    
        vel.linear.x = -9/25 * (pose.y - 20) ** 2 + 10
        vel.angular.z = (0.1 * vel.linear.x if pose.y < 20 else -0.1 * vel.linear.x)
        turtle_api.setVel('turtle1', vel)
        
        req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=5, off=0)
        turtle_api.setPen('turtle1', req)
        
        pose = turtle_api.getPose('turtle1')
     
    # Obrót w lewo   
    while round(pose.theta, 2) < round(math.pi, 2):
        vel.linear.x = 0
        vel.linear.y = 0
        vel.angular.z = (0.1 if pose.theta > 3 else 1)
        
        pose = turtle_api.getPose('turtle1')
        turtle_api.setVel('turtle1', vel)

    # Schowanie pisaka
    req = turtlesim.srv.SetPenRequest(r=0, g=0, b=255, width=5, off=1)
    turtle_api.setPen('turtle1', req)
    
    #Przesunięcie do lewej
    while pose.x > 23:
        vel.linear.x = 5
        vel.linear.y = 0
        vel.angular.z = 0
        
        pose = turtle_api.getPose('turtle1')
        turtle_api.setVel('turtle1', vel)
        
        
    # Obrót w prawo   
    while pose.theta > 0:
        vel.linear.x = 0
        vel.linear.y = 0
        vel.angular.z = (-0.01 if pose.theta < 0.05 else -1)
        
        pose = turtle_api.getPose('turtle1')
        turtle_api.setVel('turtle1', vel)
        
    
    
    # Rysujemy poziomą linię
    while pose.x < 31:
    
        vel.linear.x = (-9/16) * (pose.x - 27) ** 2 + 10
        vel.linear.y = 0
        vel.angular.z = 0
        turtle_api.setVel('turtle1', vel)
        
        req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=5, off=0)
        turtle_api.setPen('turtle1', req)
        
        pose = turtle_api.getPose('turtle1')
    
    
    # Schowanie pisaka
    req = turtlesim.srv.SetPenRequest(r=0, g=0, b=255, width=5, off=1)
    turtle_api.setPen('turtle1', req)
    
    
    #Przesunięcie do prawej
    while pose.x < 33:
        vel.linear.x = 5
        vel.linear.y = 0
        vel.angular.z = 0
        
        pose = turtle_api.getPose('turtle1')
        turtle_api.setVel('turtle1', vel)
        

    # Rysowanie W
    for _ in range(2):
    # Obrót w prawo   
        while pose.theta > -1.2:
            vel.linear.x = 0
            vel.linear.y = 0
            vel.angular.z = -0.5
        
            pose = turtle_api.getPose('turtle1')
            turtle_api.setVel('turtle1', vel)
        
        # Pierwsza linia
        while pose.y > 15:
    
            vel.linear.x = -9/25 * (pose.y - 20) ** 2 + 10
            vel.linear.y = 0
            vel.angular.z = 0
            turtle_api.setVel('turtle1', vel)
        
            req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=5, off=0)
            turtle_api.setPen('turtle1', req)
        
            pose = turtle_api.getPose('turtle1')
        # Łuk
        while pose.theta < math.pi /2:
            vel.linear.x = -4.694 * (pose.theta - 0.185) ** 2 + 10
            vel.linear.y = 0
            vel.angular.z = vel.linear.x
        
            turtle_api.setVel('turtle1', vel)
        
            req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=5, off=0)
            turtle_api.setPen('turtle1', req)
        
            pose = turtle_api.getPose('turtle1')
        
        
                
        while pose.y < 25:
    
            vel.linear.x = -9/25 * (pose.y - 20) ** 2 + 10
            vel.angular.z = 0
            turtle_api.setVel('turtle1', vel)
        
            req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=5, off=0)
            turtle_api.setPen('turtle1', req)
        
            pose = turtle_api.getPose('turtle1')
            
            
    # Obrót w prawo   
    while pose.theta > 0:
        vel.linear.x = 0
        vel.linear.y = 0
        vel.angular.z = (-0.01 if pose.theta < 0.05 else -1)
        
        pose = turtle_api.getPose('turtle1')
        turtle_api.setVel('turtle1', vel)
        
    # Schowanie pisaka
    req = turtlesim.srv.SetPenRequest(r=0, g=0, b=255, width=5, off=1)
    turtle_api.setPen('turtle1', req)
    
    # Przesunięcie do prawej
    while pose.x < 52:
        vel.linear.x = 5
        vel.linear.y = 0
        vel.angular.z = 0
        
        pose = turtle_api.getPose('turtle1')
        turtle_api.setVel('turtle1', vel)
    
    # Rysowanie "q"
    
    
    # Obrót w dół   
    while pose.theta > (-1 * math.pi)/2:
        vel.linear.x = 0
        vel.linear.y = 0
        vel.angular.z = -1 if pose.theta > -1.2 else -0.1
        
        pose = turtle_api.getPose('turtle1')
        turtle_api.setVel('turtle1', vel)
    
    # Pionowa linia
    
    while pose.y > 15:
    
        vel.linear.x = -9/25 * (pose.y - 20) ** 2 + 10
        vel.linear.y = 0
        vel.angular.z = 0
        turtle_api.setVel('turtle1', vel)
        
        req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=5, off=0)
        turtle_api.setPen('turtle1', req)
        
        pose = turtle_api.getPose('turtle1')
        
    # Obrót w górę   
    while pose.theta < math.pi /2:
        vel.linear.x = 0
        vel.linear.y = 0
        vel.angular.z = 1 if pose.theta < 1.2 else 0.1
        
        pose = turtle_api.getPose('turtle1')
        turtle_api.setVel('turtle1', vel)
        
    # Schowanie pisaka
    req = turtlesim.srv.SetPenRequest(r=0, g=0, b=255, width=5, off=1)
    turtle_api.setPen('turtle1', req)
    
    
    
    # Powrót na górę
    
    while pose.y < 23:
    
        vel.linear.x = 5
        vel.linear.y = 0
        vel.angular.z = 0
        turtle_api.setVel('turtle1', vel)
        
        
        pose = turtle_api.getPose('turtle1')
        
    # Pierwsza część koła
    circle_drawn = 0
    
    while True:
        vel.linear.x = -2.25 * (pose.x - 50) ** 2 + 10
        vel.linear.y = 0
        vel.angular.z = vel.linear.x * 0.5
        
        if pose.x < 50: circle_drawn = 1
        if circle_drawn and pose.x > 52: break
        
        pose = turtle_api.getPose('turtle1')
        turtle_api.setVel('turtle1', vel)
        req = turtlesim.srv.SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=5, off=0)
        turtle_api.setPen('turtle1', req)
        print(pose.x)