• API
• FAQ
• Tools
• Archive
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
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.

Top