#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from kobuki_msgs.msg import BumperEvent
global bump
def processBump(data):
    bump = False
    rospy.loginfo("Bumper EVent", data.state)
    if (data.state == BumperEvent.PRESSED):
        bump = True
    else:
        bump = False

def publisher():
    pub = rospy.Publisher('mobile_base/commands/velocity', Twist, queue_size = 10)
    rospy.Subscriber('mobile_base/events/bumper', BumperEvent, processBump)
    rospy.init_node('Walker', anonymous=True)
    rate = rospy.Rate(10) #10hz
    desired_velocity = Twist()
    bump = False
    while not rospy.is_shutdown():
        if (bump == False): #If no bumps in the way
            for i in range (100): # Forward in x
                if (bump == True): #Stop when a bump is encountered
                    desired_velocity.linear.x = 0
                    desired_velocity.angular.z = 0
                    pub.publish(desired_velocity)
                    rate.sleep()
                desired_velocity.linear.x = 0.2
                pub.publish(desired_velocity)
                rate.sleep()
                desired_velocity.linear.x = 0 # Stop moving in x
        if (bump == True):
            desired_velocity.linear.x = 0
            desired_velocity.angular.z = 0
            pub.publish(desired_velocity)
            rate.sleep()
if __name__ == "__main__":
    try:
        publisher()
    except rospy.ROSInterruptException:
        pass