00001 #! /usr/bin/env python 00002 # 00003 # Copyright (c) 2010, Willow Garage, Inc. 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions are met: 00008 # 00009 # * Redistributions of source code must retain the above copyright 00010 # notice, this list of conditions and the following disclaimer. 00011 # * Redistributions in binary form must reproduce the above copyright 00012 # notice, this list of conditions and the following disclaimer in the 00013 # documentation and/or other materials provided with the distribution. 00014 # * Neither the name of the Willow Garage, Inc. nor the names of its 00015 # contributors may be used to endorse or promote products derived from 00016 # this software without specific prior written permission. 00017 # 00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00028 # POSSIBILITY OF SUCH DAMAGE. 00029 00030 ##\author Kevin Watts 00031 ##\brief Restarts base driving when joy button is called 00032 00033 PKG = 'life_test' 00034 00035 import roslib; roslib.load_manifest(PKG) 00036 import rospy 00037 from joy.msg import Joy 00038 from std_srvs.srv import Empty 00039 00040 # Joy buttons. R2 is resume 00041 DEADMAN = 10 00042 RESUME = 9 00043 00044 _start_driving = rospy.ServiceProxy('reset_test', Empty) 00045 00046 last_restart = 0 00047 00048 def start_driving(): 00049 global last_restart 00050 if rospy.get_time() - last_restart < 0.5: 00051 return 00052 00053 last_restart = rospy.get_time() 00054 00055 try: 00056 _start_driving() 00057 rospy.loginfo('Restarted drive command') 00058 return True 00059 except Exception, e: 00060 rospy.logwarn('Unable to restart driving. Service may be unavailable') 00061 return False 00062 00063 def joy_cb(msg): 00064 if len(msg.buttons) < DEADMAN + 1 or len(msg.buttons) < RESUME + 1: 00065 return 00066 00067 if msg.buttons[DEADMAN] == 0 and msg.buttons[RESUME] == 1: 00068 start_driving() 00069 00070 00071 00072 if __name__ == '__main__': 00073 rospy.init_node('pr2_drive_restart') 00074 00075 joy_sub = rospy.Subscriber('joy', Joy, joy_cb) 00076 00077 rospy.spin()