Untitled
unknown
plain_text
13 days ago
3.5 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 import math import signal import sys def signal_handler(sig, frame): print("Terminating") sys.exit(0) def calculate_vel_curve(t, duration): midpoint = duration / 2.0 if t <= midpoint: return 1 + (9.0 * (t / midpoint)) else: return 10 - (9.0 * ((t - midpoint) / midpoint)) def clamp_color(val): return max(0, min(int(val), 255)) def green(v): return clamp_color(25.5 * (v - 1)) def blue(v): return clamp_color(255 - 25.5 * (v - 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) if turtle_api.hasTurtle('turtle1'): turtle_api.killTurtle('turtle1') turtle_api.spawnTurtle('turtle1', turtlesim.msg.Pose(x=30, y=15, theta=(math.pi / 2))) vel = Twist() pose = turtle_api.getPose('turtle1') # Rysowanie pionowej kreski while pose.y < 25: vel.linear.x = 1.8 * (pose.y - 15) + 1 if pose.y <= 20 else 10 - 1.8 * (pose.y - 20) vel.angular.z = 0.0 turtle_api.setVel('turtle1', vel) req = 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') rate.sleep() # Zapisz pozycję startową łuku start_pose = pose # Wyłączenie pisaka i cofnięcie się w bok (prawo) vel.linear.x = 0 vel.angular.z = 0 req = SetPenRequest(r=0, g=0, b=0, width=3, off=1) turtle_api.setPen('turtle1', req) for _ in range(10): vel.linear.x = 1.5 vel.angular.z = 0 turtle_api.setVel('turtle1', vel) rate.sleep() # Prawy łuk req = SetPenRequest(r=0, g=0, b=255, width=3, off=0) turtle_api.setPen('turtle1', req) duration = 0.7 t0 = rospy.Time.now().to_sec() while True: t = rospy.Time.now().to_sec() - t0 if t > duration: break vel.linear.x = calculate_vel_curve(t, duration) vel.angular.z = -1.9 turtle_api.setVel('turtle1', vel) req = SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=3, off=0) turtle_api.setPen('turtle1', req) rate.sleep() # Wyłączenie pisaka i cofnięcie się z powrotem w lewo req = SetPenRequest(r=0, g=0, b=0, width=3, off=1) turtle_api.setPen('turtle1', req) for _ in range(20): vel.linear.x = -1.5 vel.angular.z = 0 turtle_api.setVel('turtle1', vel) rate.sleep() # Lewy łuk (z pisakiem) req = SetPenRequest(r=0, g=0, b=255, width=3, off=0) turtle_api.setPen('turtle1', req) duration = 0.7 t0 = rospy.Time.now().to_sec() while True: t = rospy.Time.now().to_sec() - t0 if t > duration: break vel.linear.x = calculate_vel_curve(t, duration) vel.angular.z = 1.9 turtle_api.setVel('turtle1', vel) req = SetPenRequest(r=0, g=green(vel.linear.x), b=blue(vel.linear.x), width=3, off=0) turtle_api.setPen('turtle1', req) rate.sleep()
Editor is loading...
Leave a Comment