daily pastebin goal
31%
SHARE
TWEET

Untitled

a guest Oct 13th, 2017 53 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2.  
  3. import rospy
  4. from geometry_msgs.msg import Twist
  5. from kobuki_msgs.msg import BumperEvent
  6. global bump
  7. def processBump(data):
  8.     bump = False
  9.     rospy.loginfo("Bumper EVent", data.state)
  10.     if (data.state == BumperEvent.PRESSED):
  11.         bump = True
  12.     else:
  13.         bump = False
  14.  
  15. def publisher():
  16.     pub = rospy.Publisher('mobile_base/commands/velocity', Twist, queue_size = 10)
  17.     rospy.Subscriber('mobile_base/events/bumper', BumperEvent, processBump)
  18.     rospy.init_node('Walker', anonymous=True)
  19.     rate = rospy.Rate(10) #10hz
  20.     desired_velocity = Twist()
  21.     bump = False
  22.     while not rospy.is_shutdown():
  23.         if (bump == False): #If no bumps in the way
  24.             for i in range (100): # Forward in x
  25.                 if (bump == True): #Stop when a bump is encountered
  26.                     desired_velocity.linear.x = 0
  27.                     desired_velocity.angular.z = 0
  28.                     pub.publish(desired_velocity)
  29.                     rate.sleep()
  30.                 desired_velocity.linear.x = 0.2
  31.                 pub.publish(desired_velocity)
  32.                 rate.sleep()
  33.                 desired_velocity.linear.x = 0 # Stop moving in x
  34.         if (bump == True):
  35.             desired_velocity.linear.x = 0
  36.             desired_velocity.angular.z = 0
  37.             pub.publish(desired_velocity)
  38.             rate.sleep()
  39. if __name__ == "__main__":
  40.     try:
  41.         publisher()
  42.     except rospy.ROSInterruptException:
  43.         pass
RAW Paste Data
Top