API tools faq
paste
Advertisement
techpaste222

Untitled

techpaste222
Jul 8th, 2023
87
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 4.13 KB | None | 0 0
raw download clone embed print report
  1. #!/usr/bin/env python
  2. import rospy
  3. import time
  4. from geometry_msgs.msg import Twist
  5. from turtlesim.srv import TeleportAbsolute
  6.  
  7. def move_turtle(turns):
  8.     rospy.init_node('move_turtle_node', anonymous=True)
  9.     velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
  10.     teleport_service = rospy.ServiceProxy('/turtle1/teleport_absolute', TeleportAbsolute)
  11.     rate = rospy.Rate(10)
  12.  
  13.     # Rotate until the turtle is facing north
  14.     while not rospy.is_shutdown():
  15.         turns ="Left"
  16.         vel_msg = Twist()
  17.         vel_msg.angular.z = 1.0  # Adjust the angular speed as per your requirement
  18.         velocity_publisher.publish(vel_msg)
  19.  
  20.         # Check if the desired angle is reached (with a tolerance of 0.01 radians)
  21.         if rospy.Time.now().to_sec() > 2.0:  # Adjust the time as per your requirement
  22.             break
  23.  
  24.         rate.sleep()
  25.  
  26.  
  27.     for turn in turns:
  28.         turn_command = turn.split()[0].strip()  # Extract the first word as the turn command
  29.  
  30.         if turn_command == "Left":
  31.             twist = Twist()
  32.             twist.angular.z = 1  # Angular velocity for left turn
  33.             duration = rospy.Duration(1.5)  # Duration for turning (adjust as needed)
  34.             start_time = rospy.Time.now()
  35.  
  36.             while rospy.Time.now() - start_time < duration:
  37.                 velocity_publisher.publish(twist)
  38.                 rate.sleep()
  39.  
  40.             twist.angular.z = 0.0
  41.             velocity_publisher.publish(twist)
  42.  
  43.             # Delay for three second
  44.             time.sleep(3)
  45.  
  46.             # Going straight
  47.             twist.linear.x = 0.3 # Linear velocity for going straight
  48.             duration = rospy.Duration(2.0)  # Duration for going straight (adjust as needed)
  49.  
  50.             start_time = rospy.Time.now()
  51.             while rospy.Time.now() - start_time < duration:
  52.                 velocity_publisher.publish(twist)
  53.                 rate.sleep()
  54.  
  55.             # Stop the turtle
  56.             twist.linear.x = 0
  57.             velocity_publisher.publish(twist)
  58.  
  59.         elif turn_command == "Right":
  60.             twist = Twist()
  61.             twist.angular.z = -1 # Angular velocity for right turn
  62.             duration = rospy.Duration(1.5)  # Duration for turning (adjust as needed)
  63.             start_time = rospy.Time.now()
  64.  
  65.             while rospy.Time.now() - start_time < duration:
  66.                 velocity_publisher.publish(twist)
  67.                 rate.sleep()
  68.  
  69.             twist.angular.z = 0
  70.             velocity_publisher.publish(twist)
  71.  
  72.             # Delay for three second
  73.             time.sleep(3)
  74.  
  75.             # Going straight
  76.             twist.linear.x = 0.3  # Linear velocity for going straight
  77.             duration = rospy.Duration(2.0)  # Duration for going straight (adjust as needed)
  78.  
  79.             start_time = rospy.Time.now()
  80.             while rospy.Time.now() - start_time < duration:
  81.                 velocity_publisher.publish(twist)
  82.                 rate.sleep()
  83.  
  84.             # Stop the turtle
  85.             twist.linear.x = 0
  86.             velocity_publisher.publish(twist)
  87.  
  88.         elif turn_command == "Straight":
  89.             twist = Twist()
  90.             twist.linear.x = 0.3  # Linear velocity for going straight
  91.             duration = rospy.Duration(2.0)  # Duration for going straight (adjust as needed)
  92.  
  93.             start_time = rospy.Time.now()
  94.             while rospy.Time.now() - start_time < duration:
  95.                 velocity_publisher.publish(twist)
  96.                 rate.sleep()
  97.  
  98.             # Stop the turtle
  99.             twist.linear.x = 0
  100.             velocity_publisher.publish(twist)
  101.  
  102.         elif turn_command == "Stop":
  103.             twist = Twist()
  104.             twist.linear.x = 0.0
  105.             twist.angular.z = 0
  106.             velocity_publisher.publish(twist)
  107.  
  108.         else:
  109.             rospy.logerr("Invalid turn command: {}".format(turn_command))
  110.  
  111. if __name__ == '__main__':
  112.     turns = []
  113.  
  114.     with open('/home/atharva/catkin_ws/src/task_3/src/task.txt', 'r') as file:
  115.         lines = file.readlines()
  116.         for line in lines:
  117.             row = line.strip()
  118.             turns.append(row)
  119.  
  120.     move_turtle(turns)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!