tibi_dabo_smachs: assisted_vision

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('tibi_dabo_smachs')
00004 import rospy
00005 import smach
00006 import smach_ros
00007 
00008 from iri_human_assisted_object_detection.srv import haod_init,haod_detect,haod_learn
00009 from tibi_dabo_msgs.msg import sequenceAction, sequenceGoal
00010 from sensor_msgs.msg import Joy
00011 from wiimote import *
00012 from smach_ros import ServiceState
00013 import time
00014 import threading
00015 
00016 exit = 0
00017 
00018 @smach.cb_interface(input_keys=['init_count'],output_keys=['init_count'])
00019 def init_cb(userdata, response):
00020     exit = 0 
00021     if response.success == True:
00022       return 'succeeded'
00023     else:
00024       time.sleep(1);
00025       userdata.init_count = userdata.init_count + 1
00026       if userdata.init_count > 10:
00027         userdata.init_count = 0
00028         return 'preempted'
00029       else:
00030         return 'aborted'
00031 
00032 @smach.cb_interface(input_keys=['detect_count'],output_keys=['detect_count'])
00033 def assistance_needed_cb(userdata,response):
00034     if exit == 1:
00035       userdata.detect_count = 0
00036       return 'preempted'
00037     else:
00038       if userdata.detect_count == 1000:
00039         userdata.detect_count = 0
00040         return preempted
00041       else: 
00042         if response.success == False:
00043           return 'succeeded'
00044         else:
00045           return 'aborted'
00046 
00047 @smach.cb_interface(input_keys=['valid_det'])
00048 def learn_cb(userdata,request):
00049     request.valid = userdata.valid_det
00050 
00051 class WaitUserInput(smach.State):
00052     def __init__(self):
00053         smach.State.__init__(self, outcomes=['valid_detection', 'invalid_detection', 'waiting', 'no_user_input'],output_keys=['valid_det'])
00054 
00055         self.mutex = threading.Lock()
00056         self.user_input = False
00057         self.user_answer = -1
00058         self.waiting_user_input = False
00059         self.count = 0
00060 
00061         self.subscriber = rospy.Subscriber('joy', Joy, self.callback)
00062 
00063     def callback(self, data):
00064         global exit
00065         self.mutex.acquire()
00066         if self.waiting_user_input == True:
00067           if data.buttons[0] == 1:# the 1 button is pressed
00068             self.user_input = True 
00069             self.user_answer = 1
00070           else:
00071             if data.buttons[1] == 1: #the 2 button is pressed
00072               self.user_input = True 
00073               self.user_answer = 0
00074         if data.buttons[3] == 1:
00075           exit = 1
00076         self.mutex.release()
00077 
00078     def execute(self,userdata):
00079         #wait for a maximum of 5 seconds 
00080         self.mutex.acquire()
00081         self.waiting_user_input = True
00082         if self.user_input == True:
00083             if self.user_answer == 1:
00084                 self.waiting_user_input = False 
00085                 userdata.valid_det = True
00086                 self.user_input = False;
00087                 self.user_answer = -1;
00088                 self.count = 0
00089                 self.mutex.release()
00090                 return 'valid_detection'
00091             else:
00092                 self.waiting_user_input = False 
00093                 userdata.valid_det = False
00094                 self.user_input = False;
00095                 self.user_answer = -1;
00096                 self.count = 0
00097                 self.mutex.release()
00098                 return 'invalid_detection'
00099         self.count = self.count + 1
00100         self.mutex.release()
00101     
00102         time.sleep(.1)
00103         if self.count == 50:
00104             self.waiting_user_input = False 
00105             self.user_input = False;
00106             self.user_answer = -1;
00107             self.count = 0
00108             return 'no_user_input'
00109         else:
00110             return 'waiting'
00111 
00112 def hri_goal_cb(userdata, goal):
00113   rospy.loginfo('hri_goal_cb')
00114   tts_goal = sequenceGoal()
00115   tts_goal.sequence_file = [None]*5
00116   tts_goal.sequence_file[0] = userdata.tts_in
00117   tts_goal.sequence_file[1] = ""
00118   tts_goal.sequence_file[2] = ""
00119   tts_goal.sequence_file[3] = userdata.left_in
00120   tts_goal.sequence_file[4] = userdata.right_in
00121   return tts_goal
00122 
00123 class AddCounter(smach.State):
00124     def __init__(self):
00125         smach.State.__init__(self, 
00126                              outcomes=['out1','out2','out3','out4','out5','out6','out7','out8','out9','out10'],
00127                              input_keys=['counter_in'],
00128                              output_keys=['counter_in'])
00129     def execute(self, userdata):
00130         rospy.loginfo('Executing state AddCounter')
00131         if userdata.counter_in >= 9:
00132             userdata.counter_in = 0
00133             return 'out10'
00134         else:
00135             userdata.counter_in = userdata.counter_in + 1
00136             if userdata.counter_in ==1:
00137                 return 'out1'
00138             if userdata.counter_in ==2:
00139                 return 'out2'
00140             if userdata.counter_in ==3:
00141                 return 'out3'
00142             if userdata.counter_in ==4:
00143                 return 'out4'
00144             if userdata.counter_in ==5:
00145                 return 'out5'
00146             if userdata.counter_in ==6:
00147                 return 'out6'
00148             if userdata.counter_in ==7:
00149                 return 'out7'
00150             if userdata.counter_in ==8:
00151                 return 'out8'
00152             if userdata.counter_in ==9:
00153                 return 'out9'
00154 # main
00155 def main():
00156     rospy.init_node('human_assisted_object_detection')
00157 
00158     # Create a SMACH state machine
00159     sm = smach.StateMachine(['succeeded','aborted','preempted'])
00160     sm.userdata.valid = 0 
00161     sm.userdata.init_count = 0 
00162     sm.userdata.detect_count = 0
00163     sm.userdata.ask_count = 0
00164     # Define userdata
00165     #sm.userdata.sm_frame_id   = "/tibi/base_link"
00166     #sm.userdata.sm_goal_x     = 2.0
00167     #sm.userdata.sm_goal_y     = 0.0
00168     #sm.userdata.sm_goal_theta = 0.0
00169 
00170     # Add states to the State Machine
00171     with sm:
00172         sm.userdata.sm_hri_tts_intro  = "\language=Spanish \voice=Leonor Este experimento consiste en la detección y aprendizaje de caras. Colócate delante de mí para que pueda aprenderme la tuya. Si tengo dudas, te preguntaré, y con el mando podrás decirme si la detección es correcta, pulsando el botón uno, o si no es correcta, pulsando el botón 2. Cuando quieras acabar el experimento pulsa el gatillo posterior. ¿Estás preparado? ¡Empezamos!"
00173         sm.userdata.sm_hri_left_intro = ""
00174         sm.userdata.sm_hri_right_intro = ""
00175         smach.StateMachine.add('HRI_INTRO',
00176                                smach_ros.SimpleActionState('hri',
00177                                                            sequenceAction,
00178                                                            goal_cb=hri_goal_cb,
00179                                                            input_keys=['tts_in','left_in','right_in']),
00180                                transitions={'succeeded':'HAOD_INIT'},
00181                                remapping={'tts_in':'sm_hri_tts_intro',
00182                                           'left_in':'sm_hri_left_intro',
00183                                           'right_in':'sm_hri_right_intro'})
00184 
00185         smach.StateMachine.add('HAOD_INIT',
00186                                smach_ros.ServiceState('init',
00187                                                       haod_init,
00188                                                       response_cb=init_cb),
00189                                transitions={'succeeded':'HAOD_DETECT',
00190                                             'aborted':'HAOD_INIT',
00191                                             'preempted':'HRI_INIT'},
00192                                remapping={'init_count':'init_count'})
00193 
00194         sm.userdata.sm_hri_tts_init  = "\language=Spanish \voice=Leonor No he podido detectar tu cara para empezar. Lo volveré a intentar. Ponte delante y no te muevas. Gracias!"
00195         sm.userdata.sm_hri_left_init = ""
00196         sm.userdata.sm_hri_right_init = ""
00197         smach.StateMachine.add('HRI_INIT',
00198                                smach_ros.SimpleActionState('hri',
00199                                                            sequenceAction,
00200                                                            goal_cb=hri_goal_cb,
00201                                                            input_keys=['tts_in','left_in','right_in']),
00202                                transitions={'succeeded':'HAOD_INIT'},
00203                                remapping={'tts_in':'sm_hri_tts_init',
00204                                           'left_in':'sm_hri_left_init',
00205                                           'right_in':'sm_hri_right_init'})
00206                                
00207         smach.StateMachine.add('HAOD_DETECT',
00208                                smach_ros.ServiceState('detect',
00209                                                       haod_detect,
00210                                                       response_cb=assistance_needed_cb),
00211                                transitions={'succeeded':'HAOD_DETECT',
00212                                             'aborted':'ADD_ASK_COUNTER',
00213                                             'preempted':'HRI_EXIT'},
00214                                remapping={'detect_count':'detect_count'})
00215 
00216 
00217         smach.StateMachine.add('ADD_ASK_COUNTER', 
00218                                AddCounter(), 
00219                                transitions={'out1':'HRI_QUESTION1',
00220                                             'out2':'HRI_QUESTION2',
00221                                             'out3':'HRI_QUESTION3',
00222                                             'out4':'HRI_QUESTION4',
00223                                             'out5':'HRI_QUESTION5',
00224                                             'out6':'HRI_QUESTION6',
00225                                             'out7':'HRI_QUESTION7',
00226                                             'out8':'HRI_QUESTION8',
00227                                             'out9':'HRI_QUESTION9',
00228                                             'out10':'HRI_QUESTION10'},
00229                                remapping={'counter_in':'ask_count'})
00230 
00231         sm.userdata.sm_hri_tts_q1  = "\language=Spanish \voice=Leonor ¿Es correcta la detección?"
00232         sm.userdata.sm_hri_left_q1 = ""
00233         sm.userdata.sm_hri_right_q1 = ""
00234         smach.StateMachine.add('HRI_QUESTION1',
00235                                smach_ros.SimpleActionState('hri',
00236                                                            sequenceAction,
00237                                                            goal_cb=hri_goal_cb,
00238                                                            input_keys=['tts_in','left_in','right_in']),
00239                                transitions={'succeeded':'HAOD_USER_INPUT'},
00240                                remapping={'tts_in':'sm_hri_tts_q1',
00241                                           'left_in':'sm_hri_left_q1',
00242                                           'right_in':'sm_hri_right_q1'})
00243 
00244         sm.userdata.sm_hri_tts_q2  = "\language=Spanish \voice=Leonor ¿Es esto tu cara?"
00245         sm.userdata.sm_hri_left_q2 = ""
00246         sm.userdata.sm_hri_right_q2 = ""
00247         smach.StateMachine.add('HRI_QUESTION2',
00248                                smach_ros.SimpleActionState('hri',
00249                                                            sequenceAction,
00250                                                            goal_cb=hri_goal_cb,
00251                                                            input_keys=['tts_in','left_in','right_in']),
00252                                transitions={'succeeded':'HAOD_USER_INPUT'},
00253                                remapping={'tts_in':'sm_hri_tts_q2',
00254                                           'left_in':'sm_hri_left_q2',
00255                                           'right_in':'sm_hri_right_q2'})
00256 
00257         sm.userdata.sm_hri_tts_q3  = "\language=Spanish \voice=Leonor ¿Lo estoy haciendo bien?"
00258         sm.userdata.sm_hri_left_q3 = ""
00259         sm.userdata.sm_hri_right_q3 = ""
00260         smach.StateMachine.add('HRI_QUESTION3',
00261                                smach_ros.SimpleActionState('hri',
00262                                                            sequenceAction,
00263                                                            goal_cb=hri_goal_cb,
00264                                                            input_keys=['tts_in','left_in','right_in']),
00265                                transitions={'succeeded':'HAOD_USER_INPUT'},
00266                                remapping={'tts_in':'sm_hri_tts_q3',
00267                                           'left_in':'sm_hri_left_q3',
00268                                           'right_in':'sm_hri_right_q3'})
00269 
00270         sm.userdata.sm_hri_tts_q4  = "\language=Spanish \voice=Leonor ¿Está bien?"
00271         sm.userdata.sm_hri_left_q4 = ""
00272         sm.userdata.sm_hri_right_q4 = ""
00273         smach.StateMachine.add('HRI_QUESTION4',
00274                                smach_ros.SimpleActionState('hri',
00275                                                            sequenceAction,
00276                                                            goal_cb=hri_goal_cb,
00277                                                            input_keys=['tts_in','left_in','right_in']),
00278                                transitions={'succeeded':'HAOD_USER_INPUT'},
00279                                remapping={'tts_in':'sm_hri_tts_q4',
00280                                           'left_in':'sm_hri_left_q4',
00281                                           'right_in':'sm_hri_right_q4'})
00282 
00283         sm.userdata.sm_hri_tts_q5  = "\language=Spanish \voice=Leonor ¿Es esto correcto?"
00284         sm.userdata.sm_hri_left_q5 = ""
00285         sm.userdata.sm_hri_right_q5 = ""
00286         smach.StateMachine.add('HRI_QUESTION5',
00287                                smach_ros.SimpleActionState('hri',
00288                                                            sequenceAction,
00289                                                            goal_cb=hri_goal_cb,
00290                                                            input_keys=['tts_in','left_in','right_in']),
00291                                transitions={'succeeded':'HAOD_USER_INPUT'},
00292                                remapping={'tts_in':'sm_hri_tts_q5',
00293                                           'left_in':'sm_hri_left_q5',
00294                                           'right_in':'sm_hri_right_q5'})
00295 
00296         sm.userdata.sm_hri_tts_q6  = "\language=Spanish \voice=Leonor Corrígeme si está mal"
00297         sm.userdata.sm_hri_left_q6 = ""
00298         sm.userdata.sm_hri_right_q6 = ""
00299         smach.StateMachine.add('HRI_QUESTION6',
00300                                smach_ros.SimpleActionState('hri',
00301                                                            sequenceAction,
00302                                                            goal_cb=hri_goal_cb,
00303                                                            input_keys=['tts_in','left_in','right_in']),
00304                                transitions={'succeeded':'HAOD_USER_INPUT'},
00305                                remapping={'tts_in':'sm_hri_tts_q6',
00306                                           'left_in':'sm_hri_left_q6',
00307                                           'right_in':'sm_hri_right_q6'})
00308 
00309         sm.userdata.sm_hri_tts_q7  = "\language=Spanish \voice=Leonor ¿Está bien?"
00310         sm.userdata.sm_hri_left_q7 = ""
00311         sm.userdata.sm_hri_right_q7 = ""
00312         smach.StateMachine.add('HRI_QUESTION7',
00313                                smach_ros.SimpleActionState('hri',
00314                                                            sequenceAction,
00315                                                            goal_cb=hri_goal_cb,
00316                                                            input_keys=['tts_in','left_in','right_in']),
00317                                transitions={'succeeded':'HAOD_USER_INPUT'},
00318                                remapping={'tts_in':'sm_hri_tts_q7',
00319                                           'left_in':'sm_hri_left_q7',
00320                                           'right_in':'sm_hri_right_q7'})
00321 
00322         sm.userdata.sm_hri_tts_q8  = "\language=Spanish \voice=Leonor ¿Ahora es correcto?"
00323         sm.userdata.sm_hri_left_q8 = ""
00324         sm.userdata.sm_hri_right_q8 = ""
00325         smach.StateMachine.add('HRI_QUESTION8',
00326                                smach_ros.SimpleActionState('hri',
00327                                                            sequenceAction,
00328                                                            goal_cb=hri_goal_cb,
00329                                                            input_keys=['tts_in','left_in','right_in']),
00330                                transitions={'succeeded':'HAOD_USER_INPUT'},
00331                                remapping={'tts_in':'sm_hri_tts_q8',
00332                                           'left_in':'sm_hri_left_q8',
00333                                           'right_in':'sm_hri_right_q8'})
00334 
00335         sm.userdata.sm_hri_tts_q9  = "\language=Spanish \voice=Leonor ¿Está bien?"
00336         sm.userdata.sm_hri_left_q9 = ""
00337         sm.userdata.sm_hri_right_q9 = ""
00338         smach.StateMachine.add('HRI_QUESTION9',
00339                                smach_ros.SimpleActionState('hri',
00340                                                            sequenceAction,
00341                                                            goal_cb=hri_goal_cb,
00342                                                            input_keys=['tts_in','left_in','right_in']),
00343                                transitions={'succeeded':'HAOD_USER_INPUT'},
00344                                remapping={'tts_in':'sm_hri_tts_q9',
00345                                           'left_in':'sm_hri_left_q9',
00346                                           'right_in':'sm_hri_right_q9'})
00347 
00348         sm.userdata.sm_hri_tts_q10  = " \language=Spanish \voice=Leonor ¿Es correcto?"
00349         sm.userdata.sm_hri_left_q10 = ""
00350         sm.userdata.sm_hri_right_q10 = ""
00351         smach.StateMachine.add('HRI_QUESTION10',
00352                                smach_ros.SimpleActionState('hri',
00353                                                            sequenceAction,
00354                                                            goal_cb=hri_goal_cb,
00355                                                            input_keys=['tts_in','left_in','right_in']),
00356                                transitions={'succeeded':'HAOD_USER_INPUT'},
00357                                remapping={'tts_in':'sm_hri_tts_q10',
00358                                           'left_in':'sm_hri_left_q10',
00359                                           'right_in':'sm_hri_right_q10'})
00360 
00361         smach.StateMachine.add('HAOD_USER_INPUT',WaitUserInput(),
00362                                transitions={'valid_detection':'HAOD_LEARN',
00363                                             'invalid_detection':'HAOD_LEARN',
00364                                             'waiting':'HAOD_USER_INPUT',
00365                                             'no_user_input':'HAOD_DETECT'},
00366                                remapping={'valid_det':'valid'})
00367 
00368         smach.StateMachine.add('HAOD_LEARN',
00369                                smach_ros.ServiceState('learn',
00370                                                       haod_learn,
00371                                                       request_cb=learn_cb),
00372                                transitions={'succeeded':'HAOD_DETECT',
00373                                             'aborted':'aborted'},
00374                                remapping={'valid_det':'valid'})
00375 
00376         sm.userdata.sm_hri_tts_exit  = "\language=Spanish \voice=Leonor El experimento ha acabado. Gracias por tu ayuda, ha sido un placer. Adiós!"
00377         sm.userdata.sm_hri_left_exit = "left_arm_farewell.xml"
00378         sm.userdata.sm_hri_right_exit = "right_arm_farewell.xml"
00379         smach.StateMachine.add('HRI_EXIT',
00380                                smach_ros.SimpleActionState('hri',
00381                                                            sequenceAction,
00382                                                            goal_cb=hri_goal_cb,
00383                                                            input_keys=['tts_in','left_in','right_in']),
00384                                transitions={'succeeded':'succeeded'},
00385                                remapping={'tts_in':'sm_hri_tts_exit',
00386                                           'left_in':'sm_hri_left_exit',
00387                                           'right_in':'sm_hri_right_exit'})
00388                                
00389     # Create and start the introspection server
00390     sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00391     sis.start()
00392 
00393     # Execute the state machine
00394     outcome = sm.execute()
00395 
00396     # Wait for ctrl-c to stop the application
00397     rospy.spin()
00398     sis.stop()
00399 
00400 
00401 if __name__ == '__main__':
00402     main()
00403