Untitled
unknown
plain_text
16 days ago
2.3 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 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')
Editor is loading...
Leave a Comment