SHARE
TWEET

Untitled

a guest Oct 21st, 2019 113 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #! usr#!/usr/bin/env python
  2.  
  3. import rospy
  4. from sensor_msgs.msg import LaserScan
  5. from geometry_msgs.msg import Twist
  6.  
  7. import math
  8. import time
  9.  
  10.    
  11. def shutdown():
  12.     # stop turtlebot
  13.     rospy.loginfo("Stop TurtleBot")
  14.     # a default Twist has linear.x of 0 and angular.z of 0.  So it'll stop TurtleBot
  15.     pub.publish(Twist())
  16.     # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script
  17.     rospy.sleep(1)
  18.    
  19.  
  20. def callback(msg):
  21.     print('=========================')
  22.     print('s1 [0]')
  23.     print(msg.ranges[0])  #distance at angle 0, which is directly in front of robot
  24.    
  25.     print('s2 [90]')
  26.     print(msg.ranges[90])  #distance at angle 90 degrees
  27.    
  28.     print('s3 [180]')
  29.     print(msg.ranges[180])  #distance at angle 180 degrees
  30.    
  31.     print('s4 [270]')
  32.     print(msg.ranges[270])  #distance at angle 270 degrees
  33.    
  34.     print('s5 [359]')
  35.     print(msg.ranges[359])  #distance at angle 359 degrees
  36.    
  37.     for i in range(15):    
  38.         if msg.ranges[i] > 0.5:
  39.             move.linear.x = 0.3
  40.             move.angular.z = 0.0
  41.             print("Moving forward")
  42.        
  43.         elif msg.ranges[i] <= 0.5:
  44.             move.linear.x = 0.0
  45.             move.angular.z = 0.3
  46.             print("Obstacle in front")
  47.  
  48.     for i in range(345,359):    
  49.         if msg.ranges[i] > 0.5:
  50.             move.linear.x = 0.3
  51.             move.angular.z = 0.0
  52.             print("Moving forward")
  53.        
  54.         elif msg.ranges[i] <= 0.5:
  55.             move.linear.x = 0.0
  56.             move.angular.z = -0.3
  57.             print("Obstacle in front")
  58.  
  59.     pub.publish(move)
  60.    
  61.  
  62. rospy.init_node('obstacle_avoidance')  # Initiate node
  63. rospy.loginfo("To stop TurtleBot CTRL + C")
  64. rospy.on_shutdown(shutdown)                             #Specifies what the turtlebot should do once Ctrl+C is pressed
  65.  
  66. sub = rospy.Subscriber('/scan', LaserScan, callback)    #Create subscriber to laser/scan topic
  67. pub = rospy.Publisher('/cmd_vel', Twist)
  68. move = Twist()
  69.  
  70. rospy.spin()
RAW Paste Data
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
 
Top