Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 import roslib
00031 roslib.load_manifest('teleop_microscribe')
00032 import rospy
00033 from joy.msg import *
00034 from pr2_controllers_msgs.msg import *
00035 from trajectory_msgs.msg import *
00036 from pr2_cockpit_msgs.srv import *
00037
00038 rospy.init_node('pedal_mode_switcher')
00039
00040 HEAD_PEDALS = 'head_pedals'
00041 DRIVE_PEDALS = 'drive_pedals'
00042
00043 enabled = [HEAD_PEDALS]
00044 all = {}
00045
00046 def send():
00047 global all, enabled
00048 for k, v in all.items():
00049 req = k in enabled
00050 print "Setting", k, "to", req
00051 try:
00052 v.call(req)
00053 except rospy.ServiceException, ex:
00054 print ex
00055
00056 def pedal_cb(msg):
00057 global enabled
00058 if msg.buttons[0]:
00059 enabled = [HEAD_PEDALS]
00060 elif msg.buttons[1]:
00061 enabled = [DRIVE_PEDALS]
00062
00063 print "Enabled:", enabled
00064 send()
00065
00066 def setup():
00067 global all, enabled
00068 all[HEAD_PEDALS] = rospy.ServiceProxy('/head_pedals/enable', Enable)
00069 all[DRIVE_PEDALS] = rospy.ServiceProxy('/teleop_base_pedals/enable', Enable)
00070 rospy.Subscriber('/l_pedals/joy', Joy, pedal_cb)
00071 send()
00072
00073 def main():
00074 rate = rospy.Rate(1.0)
00075 setup()
00076 while not rospy.is_shutdown():
00077 send()
00078 rate.sleep()
00079
00080
00081 if __name__ == '__main__':
00082 main()