SHARE
TWEET

Untitled

a guest Dec 6th, 2019 81 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import rospy
  2. from threading import Thread
  3. from geometry_msgs.msg import Twist
  4. from sensor_msgs.msg import LaserScan
  5. from nav_msgs.msg import Odometry
  6.  
  7. a = True
  8. x = True
  9. def callback(data):
  10.     global a, velocity_publisher, vel_msg
  11.     if(data.ranges[0] != float('inf') and a == True):
  12.         if(data.ranges[0] >= 0.39):
  13.             vel_msg.linear.x = 0.2
  14.             velocity_publisher.publish(vel_msg)
  15.         else:
  16.             a = False
  17.  
  18. def odomcallback(data1):
  19.     global velocity_publisher, x, vel_msg
  20.     if (data1.pose.pose.position.x > 0.000 and a == False):
  21.         vel_msg.linear.x = -0.2
  22.     elif(x == True and a == False):
  23.         vel_msg.linear.x = 0.0
  24.         x = False
  25.     velocity_publisher.publish(vel_msg)
  26.            
  27.  
  28.  
  29. if __name__ == '__main__':
  30.     velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
  31.     rospy.init_node('listener', anonymous=True)
  32.     vel_msg = Twist()
  33.     rospy.Subscriber("/scan", LaserScan, callback)
  34.     rospy.Subscriber('/odom',Odometry,odomcallback)
  35.     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