Untitled
unknown
plain_text
20 days ago
2.4 kB
4
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_t_start(y): if 15 <= 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__": 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))) # Start: pionowa kreska T vel = Twist() pose = turtle_api.getPose('turtle1') while pose.y < 25: vel.linear.x = calculate_vel_t_start(pose.y) vel.angular.z = 0.0 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') rate.sleep() # Zakręt łukiem w prawo (góra litery T) vel.linear.x = 2.5 vel.angular.z = -1.5 turn_start = rospy.Time.now().to_sec() while rospy.Time.now().to_sec() - turn_start < 1.2: 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') rate.sleep() # Poprzeczka pozioma w prawo vel.linear.x = 2.0 vel.angular.z = 0.0 move_start = rospy.Time.now().to_sec() while rospy.Time.now().to_sec() - move_start < 2.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') rate.sleep()
Editor is loading...
Leave a Comment