#!/usr/bin/env python

""" Example code of how to access button state. """

import roslib; roslib.load_manifest('turtlebot_node')
import rospy

from turtlebot_node.msg import TurtlebotSensorState
from geometry_msgs.msg import Twist

class ButtonTest:
    BUTTON_ID = 2 ** 3  # Using digital input 3

    def buttonCb(self, msg):
        # is the button pressed?
        if not msg.user_digital_inputs & self.BUTTON_ID:
            # debounce it!
            if rospy.Time.now() > self.last + rospy.Duration(1.0):
                self.ok_to_move = not self.ok_to_move
                self.last = rospy.Time.now()

    def __init__(self):
        # we have to initialize the node, with a name in the ROS graph
        rospy.init_node('button_test')

        # subscribe to sensor state
        self.ok_to_move = False
        self.last = rospy.Time.now()
        rospy.Subscriber('/turtlebot_node/sensor_state', TurtlebotSensorState, self.buttonCb)

        # publisher for movement
        self.pub = rospy.Publisher('/turtlebot_node/cmd_vel', Twist)

        # move the base if we should
        rate = rospy.Rate(10.0)
        while not rospy.is_shutdown():
            if self.ok_to_move:
                msg = Twist()
                msg.linear.x = 0.1
                self.pub.publish(msg)
            else:
                self.pub.publish(Twist())
            rate.sleep()


if __name__ == "__main__":
    bt = ButtonTest()