Go to the documentation of this file.00001
00002
00003 import roslib;
00004 roslib.load_manifest('rfid_demos')
00005 roslib.load_manifest('hrl_pr2_kinematics_tutorials')
00006
00007 roslib.load_manifest('std_srvs')
00008 roslib.load_manifest('joy')
00009 import rospy
00010
00011 from smach_ros import IntrospectionServer
00012 from rfid_servoing.srv import ServoSrv
00013 from rfid_behaviors.srv import HandoffSrv
00014 from rfid_behaviors.srv import String_Int32
00015 from rfid_behaviors.srv import FlapEarsSrv
00016
00017 from rfid_demos import sm_rfid_delivery
00018 from std_srvs.srv import Empty
00019 from joy.msg import Joy
00020 from cmd_process import CmdProcess
00021
00022 import numpy as np, math
00023 import time
00024
00025 class DemoNode():
00026 def __init__( self ):
00027 rospy.init_node( 'rfid_demo', anonymous = True )
00028 rospy.logout( 'demo_node: Initializing' )
00029 rospy.logout( 'demo_node: Waiting for services' )
00030 rospy.wait_for_service( '/rfid_servo/servo' )
00031 rospy.wait_for_service( '/rfid_orient/orient' )
00032 rospy.wait_for_service( '/rfid_orient/flap' )
00033 rospy.wait_for_service( '/rfid_handoff/initialize' )
00034 rospy.wait_for_service( '/rfid_handoff/handoff' )
00035 rospy.wait_for_service( '/rfid_handoff/wave' )
00036
00037 rospy.logout( 'demo_node: All services ready.' )
00038
00039 rospy.logout( 'demo_node: Setting up state machine.' )
00040 self.sm = sm_rfid_delivery.sm_delivery()
00041
00042 sis = IntrospectionServer('RFID_delivery', self.sm, '/SM_ROOT')
00043 sis.start()
00044
00045 rospy.logout( 'demo_node: Done setting up state machine.' )
00046
00047 self.last_button = time.time()
00048 self.run_demo = False
00049 self.run_hoinit = False
00050 self.run_handoff = False
00051
00052 self._joy_sub = rospy.Subscriber( 'joy', Joy, self.joy_process )
00053
00054 self._servo = rospy.ServiceProxy( '/rfid_servo/servo', ServoSrv )
00055 self.follow1 = lambda : self._servo( 'person ', 1 )
00056 self.follow = lambda : self._servo( 'person ', 0 )
00057
00058 self._handoff = rospy.ServiceProxy( '/rfid_handoff/handoff', HandoffSrv )
00059 self.handoff = lambda : self._handoff()
00060
00061 self._hoinit = rospy.ServiceProxy( '/rfid_handoff/initialize', HandoffSrv )
00062 self.hoinit = lambda : self._hoinit()
00063
00064 self._howave = rospy.ServiceProxy( '/rfid_handoff/wave', HandoffSrv )
00065 self.howave = lambda : self._howave()
00066
00067 self._flap = rospy.ServiceProxy( '/rfid_orient/flap', FlapEarsSrv )
00068 self.flap = lambda : self._flap( '' )
00069
00070 self._orient = rospy.ServiceProxy( '/rfid_orient/orient', String_Int32 )
00071 self.orient = lambda tagid: self._orient( tagid )
00072
00073
00074
00075
00076 self._service = rospy.Service( '/rfid_demo/demo', Empty, self.demo )
00077
00078
00079
00080 rospy.logout( 'demo_node: Demo service ready!' )
00081
00082 def joy_process( self, msg ):
00083 if msg.buttons[11] == 1 and time.time() - self.last_button > 1.0:
00084 if msg.buttons[12] == 1:
00085 rospy.logout( 'demo_node: joy_process: Calling demo service' )
00086 p = CmdProcess( ['rosservice', 'call', '/rfid_demo/demo'] )
00087 p.run()
00088 self.last_button = time.time()
00089 elif msg.buttons[13] == 1:
00090 rospy.logout( 'demo_node: joy_process: Calling handoff service' )
00091 p = CmdProcess( ['rosservice', 'call', '/rfid_handoff/handoff'] )
00092 p.run()
00093 self.last_button = time.time()
00094
00095
00096
00097
00098
00099
00100 elif msg.buttons[15] == 1:
00101 rospy.logout( 'demo_node: joy_process: Calling handoff initialization service' )
00102 p = CmdProcess( ['rosservice', 'call', '/rfid_handoff/initialize'] )
00103 p.run()
00104 self.last_button = time.time()
00105 elif msg.buttons[9] == 1:
00106 rospy.logout( 'demo_node: joy_process: Calling handoff wave service' )
00107 p = CmdProcess( ['rosservice', 'call', '/rfid_handoff/wave'] )
00108 p.run()
00109 self.last_button = time.time()
00110
00111
00112
00113
00114
00115 def demo( self, msg = None ):
00116 rospy.logout( 'demo_node: Calling.' )
00117 self.sm.userdata.tagid = 'person '
00118 outcome = self.sm.execute()
00119 print outcome
00120
00121 if __name__ == '__main__':
00122 dn = DemoNode()
00123 rospy.spin()