Untitled
unknown
plain_text
24 days ago
3.1 kB
4
Indexable
import rospy import turtlesim from turtlesim.msg import Pose from turtlesim.srv import SetPenRequest from geometry_msgs.msg import Twist import math import signal import sys def signal_handler(sig, frame): print ("Terminating") sys.exit(0) def draw_T(turtle_api): # Narysuj pionową linię (linia prosta) vel = Twist() vel.linear.x = 2 turtle_api.setVel('turtle1', vel) # Rysowanie pionowej linii od y = 15 do y = 25 for i in range(10): pose = turtle_api.getPose('turtle1') turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x, y=pose.y + 1, theta=pose.theta)) # Przesuwamy żółwia do góry rospy.sleep(0.1) # Teraz rysujemy poziomą linię vel.linear.x = 0 # Zatrzymujemy żółwia na chwilę, żeby rysować poziomo turtle_api.setVel('turtle1', vel) # Obrot o 90° w prawo turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x, y=pose.y, theta=math.pi / 2)) # Obrót o 90 stopni # Rysowanie poziomej linii od lewej do prawej vel.linear.x = 2 # Ustawiamy prędkość na 2 do rysowania poziomej linii turtle_api.setVel('turtle1', vel) for i in range(5): # Narysuj linię poziomą pose = turtle_api.getPose('turtle1') turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x + 1, y=pose.y, theta=pose.theta)) # Rysowanie poziomo rospy.sleep(0.1) def draw_W(turtle_api): # Narysuj literę W, składającą się z kilku linii vel = Twist() vel.linear.x = 2 turtle_api.setVel('turtle1', vel) # Pierwsza linia diagonalna (od lewej do prawej) for i in range(7): pose = turtle_api.getPose('turtle1') turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x + 1, y=pose.y - 1, theta=pose.theta)) # Linia w dół i w prawo rospy.sleep(0.1) # Druga linia diagonalna (od prawej do lewej) for i in range(7): pose = turtle_api.getPose('turtle1') turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x + 1, y=pose.y + 1, theta=pose.theta)) # Linia w górę i w prawo rospy.sleep(0.1) # Trzecia linia pozioma (przechodzimy do końca "W") vel.linear.x = 2 # Ustawiamy prędkość na 2 turtle_api.setVel('turtle1', vel) for i in range(7): # Rysujemy poziomo pose = turtle_api.getPose('turtle1') turtle_api.setPose('turtle1', turtlesim.msg.Pose(x=pose.x + 1, y=pose.y, theta=pose.theta)) # Linia pozioma rospy.sleep(0.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) # Ustawienia pióra (rysowanie) set_pen_req = turtlesim.srv.SetPenRequest(r=0, g=0, b=255, width=3, off=0) turtle_api.setPen('turtle1', set_pen_req) # Narysuj "T" draw_T(turtle_api) rospy.sleep(1) # Narysuj "W" draw_W(turtle_api) rospy.spin()
Editor is loading...
Leave a Comment