Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #! usr#!/usr/bin/env python
- import rospy
- from sensor_msgs.msg import LaserScan
- from geometry_msgs.msg import Twist
- import math
- import time
- def shutdown():
- # stop turtlebot
- rospy.loginfo("Stop TurtleBot")
- # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot
- pub.publish(Twist())
- # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script
- rospy.sleep(1)
- def callback(msg):
- print('=========================')
- print('s1 [0]')
- print(msg.ranges[0]) #distance at angle 0, which is directly in front of robot
- print('s2 [90]')
- print(msg.ranges[90]) #distance at angle 90 degrees
- print('s3 [180]')
- print(msg.ranges[180]) #distance at angle 180 degrees
- print('s4 [270]')
- print(msg.ranges[270]) #distance at angle 270 degrees
- print('s5 [359]')
- print(msg.ranges[359]) #distance at angle 359 degrees
- for i in range(15):
- if msg.ranges[i] > 0.5:
- move.linear.x = 0.3
- move.angular.z = 0.0
- print("Moving forward")
- elif msg.ranges[i] <= 0.5:
- move.linear.x = 0.0
- move.angular.z = 0.3
- print("Obstacle in front")
- for i in range(345,359):
- if msg.ranges[i] > 0.5:
- move.linear.x = 0.3
- move.angular.z = 0.0
- print("Moving forward")
- elif msg.ranges[i] <= 0.5:
- move.linear.x = 0.0
- move.angular.z = -0.3
- print("Obstacle in front")
- pub.publish(move)
- rospy.init_node('obstacle_avoidance') # Initiate node
- rospy.loginfo("To stop TurtleBot CTRL + C")
- rospy.on_shutdown(shutdown) #Specifies what the turtlebot should do once Ctrl+C is pressed
- sub = rospy.Subscriber('/scan', LaserScan, callback) #Create subscriber to laser/scan topic
- pub = rospy.Publisher('/cmd_vel', Twist)
- move = Twist()
- rospy.spin()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement