Untitled
unknown
plain_text
21 days ago
8.0 kB
3
Indexable
#!/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)
Editor is loading...
Leave a Comment