Untitled
unknown
plain_text
19 days ago
3.5 kB
3
Indexable
#!/usr/bin/env python import rospy import math import signal import sys from geometry_msgs.msg import Twist from turtlesim.msg import Pose from turtlesim.srv import SetPenRequest from TurtlesimSIU import TurtlesimSIU def signal_handler(sig, frame): print("Terminating") sys.exit(0) def calculate_velocity(pos, start, mid, end): if start <= pos <= mid: return 1 + 9 * ((pos - start) / (mid - start)) elif mid < pos <= end: return 1 + 9 * ((end - pos) / (end - mid)) return 1 def green(v): return min(max(int(25.5 * (v - 1)), 0), 255) def blue(v): return max(min(255 - green(v), 255), 0) def draw_vertical(turtle_api): vel = Twist() pose = turtle_api.getPose('turtle1') while pose.y < 25: v = calculate_velocity(pose.y, 15, 20, 25) vel.linear.x = v vel.angular.z = 0 turtle_api.setVel('turtle1', vel) req = SetPenRequest(r=0, g=green(v), b=blue(v), width=3, off=0) turtle_api.setPen('turtle1', req) pose = turtle_api.getPose('turtle1') def draw_arc(turtle_api, direction='left'): # Setup vel = Twist() pose = turtle_api.getPose('turtle1') radius = 2.0 angle_span = math.pi / 2 # 90 degrees arc_length = radius * angle_span traveled = 0 # Direction sign = -1 if direction == 'left' else 1 vel.linear.y = 0 # Estimate how far we've gone rate = rospy.Rate(20) last_pose = pose while traveled < arc_length: pose = turtle_api.getPose('turtle1') # Track arc progress dx = pose.x - last_pose.x dy = pose.y - last_pose.y step = math.sqrt(dx**2 + dy**2) traveled += step last_pose = pose v = calculate_velocity(traveled, 0, arc_length / 2, arc_length) vel.linear.x = v vel.angular.z = sign * v / radius turtle_api.setVel('turtle1', vel) req = SetPenRequest(r=0, g=green(v), b=blue(v), width=3, off=0) turtle_api.setPen('turtle1', req) rate.sleep() # Stop vel.linear.x = 0 vel.angular.z = 0 turtle_api.setVel('turtle1', vel) def move_to_start_of_arc(turtle_api, offset): pose = turtle_api.getPose('turtle1') vel = Twist() vel.linear.x = -2 vel.angular.z = 0 turtle_api.setPen('turtle1', SetPenRequest(r=0, g=0, b=0, width=3, off=1)) # pisak off rate = rospy.Rate(10) while pose.x > offset: turtle_api.setVel('turtle1', vel) pose = turtle_api.getPose('turtle1') rate.sleep() vel.linear.x = 0 turtle_api.setVel('turtle1', vel) turtle_api.setPen('turtle1', SetPenRequest(r=0, g=0, b=255, width=3, off=0)) # pisak on if __name__ == "__main__": signal.signal(signal.SIGINT, signal_handler) rospy.init_node('siu_t_drawer', anonymous=False) turtle_api = TurtlesimSIU.TurtlesimSIU() if turtle_api.hasTurtle('turtle1'): turtle_api.killTurtle('turtle1') turtle_api.spawnTurtle('turtle1', x=30, y=15, theta=math.pi / 2) # Start from clean turtle_api.clear() # Rysuj pion draw_vertical(turtle_api) # Lewy łuk move_to_start_of_arc(turtle_api, 27) draw_arc(turtle_api, direction='right') # Wróć do środka (pisak off), przesuwamy się do 30 move_to_start_of_arc(turtle_api, 30) # Prawy łuk draw_arc(turtle_api, direction='left')
Editor is loading...
Leave a Comment