button_clutch.py
Go to the documentation of this file.00001 #! /usr/bin/env python 00002 # Copyright (c) 2009, Willow Garage, Inc. 00003 # All rights reserved. 00004 # 00005 # Redistribution and use in source and binary forms, with or without 00006 # modification, are permitted provided that the following conditions are met: 00007 # 00008 # * Redistributions of source code must retain the above copyright 00009 # notice, this list of conditions and the following disclaimer. 00010 # * Redistributions in binary form must reproduce the above copyright 00011 # notice, this list of conditions and the following disclaimer in the 00012 # documentation and/or other materials provided with the distribution. 00013 # * Neither the name of the Willow Garage, Inc. nor the names of its 00014 # contributors may be used to endorse or promote products derived from 00015 # this software without specific prior written permission. 00016 # 00017 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 # POSSIBILITY OF SUCH DAMAGE. 00028 00029 00030 import roslib 00031 roslib.load_manifest('teleop_microscribe') 00032 import rospy 00033 from joy.msg import * 00034 from std_srvs.srv import * 00035 00036 class Clutcher: 00037 def __init__(self, ms, teleop): 00038 self.engage = rospy.ServiceProxy("/%s/engage" % teleop, Empty) 00039 self.disengage = rospy.ServiceProxy("/%s/disengage" % teleop, Empty) 00040 rospy.Subscriber("/%s/buttons" % ms, Joy, self.buttons_cb) 00041 self.last_pushed = False 00042 self.engaged = False 00043 00044 def buttons_cb(self, msg): 00045 pushed = False 00046 for b in msg.buttons: 00047 if b > 0: 00048 pushed = True 00049 if pushed and not self.last_pushed: 00050 if self.engaged: 00051 self.disengage() 00052 print "Disengaged:", self.disengage.resolved_name 00053 self.engaged = False 00054 else: 00055 self.engage() 00056 print "Engaged:", self.engage.resolved_name 00057 self.engaged = True 00058 self.last_pushed = pushed 00059 00060 rospy.init_node('button_clutch') 00061 cl = Clutcher("microscribe_l", "teleop_microscribe_l") 00062 cr = Clutcher("microscribe_r", "teleop_microscribe_r") 00063 rospy.spin()