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 calculate_vel_t_start(y):
    if 15 <= y and 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__":

    # 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=3, 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')
    while pose.y < 25:
    
        vel.linear.x = calculate_vel_t_start(pose.y)
        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')
        
    while pose.x > 26:
    
	    vel.linear.x = 5
	    vel.angular.z = 1.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')