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.logwarn('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  = "Let's start the experiment. First you must know that during the interaction you'll help me to improve my face detections. I'll ask you  some questions if I have some doubts. If I am ok, press button 1, nevertheless, if I'm wrong press button 2. If you want to finish the experiment press button B!   Are you ready? I hope so! Please, look at me while I memorize your face. Let's start!'"
00173         sm.userdata.sm_hri_left_intro = ""
00174         sm.userdata.sm_hri_right_intro = ""
00175         rospy.logwarn('hri_before_intro') 
00176         smach.StateMachine.add('HRI_INTRO',
00177                               smach_ros.SimpleActionState('hri',
00178                                                            sequenceAction,
00179                                                            goal_cb=hri_goal_cb,
00180                                                            input_keys=['tts_in','left_in','right_in']),
00181                                transitions={'succeeded':'HAOD_INIT'},
00182                                remapping={'tts_in':'sm_hri_tts_intro',
00183                                           'left_in':'sm_hri_left_intro',
00184                                           'right_in':'sm_hri_right_intro'})
00185 
00186         smach.StateMachine.add('HAOD_INIT',
00187                                smach_ros.ServiceState('init',
00188                                                       haod_init,
00189                                                       response_cb=init_cb),
00190                                transitions={'succeeded':'HAOD_DETECT',
00191                                             'aborted':'HAOD_INIT',
00192                                             'preempted':'HRI_INIT'},
00193                                remapping={'init_count':'init_count'})
00194 
00195         sm.userdata.sm_hri_tts_init  = "Sorry, I can not detect your face, stand in front of me and do not move, thanks"
00196         sm.userdata.sm_hri_left_init = ""
00197         sm.userdata.sm_hri_right_init = ""
00198         smach.StateMachine.add('HRI_INIT',
00199                                smach_ros.SimpleActionState('hri',
00200                                                            sequenceAction,
00201                                                            goal_cb=hri_goal_cb,
00202                                                            input_keys=['tts_in','left_in','right_in']),
00203                                transitions={'succeeded':'HAOD_INIT'},
00204                                remapping={'tts_in':'sm_hri_tts_init',
00205                                           'left_in':'sm_hri_left_init',
00206                                           'right_in':'sm_hri_right_init'})
00207                                
00208         smach.StateMachine.add('HAOD_DETECT',
00209                                smach_ros.ServiceState('detect',
00210                                                       haod_detect,
00211                                                       response_cb=assistance_needed_cb),
00212                                transitions={'succeeded':'HAOD_DETECT',
00213                                             'aborted':'ADD_ASK_COUNTER',
00214                                             'preempted':'HRI_EXIT'},
00215                                remapping={'detect_count':'detect_count'})
00216 
00217 
00218         smach.StateMachine.add('ADD_ASK_COUNTER', 
00219                                AddCounter(), 
00220                                transitions={'out1':'HRI_QUESTION1',
00221                                             'out2':'HRI_QUESTION2',
00222                                             'out3':'HRI_QUESTION3',
00223                                             'out4':'HRI_QUESTION4',
00224                                             'out5':'HRI_QUESTION5',
00225                                             'out6':'HRI_QUESTION6',
00226                                             'out7':'HRI_QUESTION7',
00227                                             'out8':'HRI_QUESTION8',
00228                                             'out9':'HRI_QUESTION9',
00229                                             'out10':'HRI_QUESTION10'},
00230                                remapping={'counter_in':'ask_count'})
00231 
00232         sm.userdata.sm_hri_tts_q1  = " Is the detection correct?"
00233         sm.userdata.sm_hri_left_q1 = ""
00234         sm.userdata.sm_hri_right_q1 = ""
00235         smach.StateMachine.add('HRI_QUESTION1',
00236                                smach_ros.SimpleActionState('hri',
00237                                                            sequenceAction,
00238                                                            goal_cb=hri_goal_cb,
00239                                                            input_keys=['tts_in','left_in','right_in']),
00240                                transitions={'succeeded':'HAOD_USER_INPUT'},
00241                                remapping={'tts_in':'sm_hri_tts_q1',
00242                                           'left_in':'sm_hri_left_q1',
00243                                           'right_in':'sm_hri_right_q1'})
00244 
00245         sm.userdata.sm_hri_tts_q2  = " the blue rectangle is on your face?"
00246         sm.userdata.sm_hri_left_q2 = ""
00247         sm.userdata.sm_hri_right_q2 = ""
00248         smach.StateMachine.add('HRI_QUESTION2',
00249                                smach_ros.SimpleActionState('hri',
00250                                                            sequenceAction,
00251                                                            goal_cb=hri_goal_cb,
00252                                                            input_keys=['tts_in','left_in','right_in']),
00253                                transitions={'succeeded':'HAOD_USER_INPUT'},
00254                                remapping={'tts_in':'sm_hri_tts_q2',
00255                                           'left_in':'sm_hri_left_q2',
00256                                           'right_in':'sm_hri_right_q2'})
00257 
00258         sm.userdata.sm_hri_tts_q3  = " Am I detecting correctly?"
00259         sm.userdata.sm_hri_left_q3 = ""
00260         sm.userdata.sm_hri_right_q3 = ""
00261         smach.StateMachine.add('HRI_QUESTION3',
00262                                smach_ros.SimpleActionState('hri',
00263                                                            sequenceAction,
00264                                                            goal_cb=hri_goal_cb,
00265                                                            input_keys=['tts_in','left_in','right_in']),
00266                                transitions={'succeeded':'HAOD_USER_INPUT'},
00267                                remapping={'tts_in':'sm_hri_tts_q3',
00268                                           'left_in':'sm_hri_left_q3',
00269                                           'right_in':'sm_hri_right_q3'})
00270 
00271         sm.userdata.sm_hri_tts_q4  = " I have a doubt, tell me if is it correct?"
00272         sm.userdata.sm_hri_left_q4 = ""
00273         sm.userdata.sm_hri_right_q4 = ""
00274         smach.StateMachine.add('HRI_QUESTION4',
00275                                smach_ros.SimpleActionState('hri',
00276                                                            sequenceAction,
00277                                                            goal_cb=hri_goal_cb,
00278                                                            input_keys=['tts_in','left_in','right_in']),
00279                                transitions={'succeeded':'HAOD_USER_INPUT'},
00280                                remapping={'tts_in':'sm_hri_tts_q4',
00281                                           'left_in':'sm_hri_left_q4',
00282                                           'right_in':'sm_hri_right_q4'})
00283 
00284         sm.userdata.sm_hri_tts_q5  = " I need you to come closer.  help me  once more. Is it ok?"
00285         sm.userdata.sm_hri_left_q5 = ""
00286         sm.userdata.sm_hri_right_q5 = ""
00287         smach.StateMachine.add('HRI_QUESTION5',
00288                                smach_ros.SimpleActionState('hri',
00289                                                            sequenceAction,
00290                                                            goal_cb=hri_goal_cb,
00291                                                            input_keys=['tts_in','left_in','right_in']),
00292                                transitions={'succeeded':'HAOD_USER_INPUT'},
00293                                remapping={'tts_in':'sm_hri_tts_q5',
00294                                           'left_in':'sm_hri_left_q5',
00295                                           'right_in':'sm_hri_right_q5'})
00296 
00297         sm.userdata.sm_hri_tts_q6  = " You are moving too fast, and I need your help to improve my knowledge"
00298         sm.userdata.sm_hri_left_q6 = ""
00299         sm.userdata.sm_hri_right_q6 = ""
00300         smach.StateMachine.add('HRI_QUESTION6',
00301                                smach_ros.SimpleActionState('hri',
00302                                                            sequenceAction,
00303                                                            goal_cb=hri_goal_cb,
00304                                                            input_keys=['tts_in','left_in','right_in']),
00305                                transitions={'succeeded':'HAOD_USER_INPUT'},
00306                                remapping={'tts_in':'sm_hri_tts_q6',
00307                                           'left_in':'sm_hri_left_q6',
00308                                           'right_in':'sm_hri_right_q6'})
00309 
00310         sm.userdata.sm_hri_tts_q7  = " Correct me if I'm wrong please."
00311         sm.userdata.sm_hri_left_q7 = ""
00312         sm.userdata.sm_hri_right_q7 = ""
00313         smach.StateMachine.add('HRI_QUESTION7',
00314                                smach_ros.SimpleActionState('hri',
00315                                                            sequenceAction,
00316                                                            goal_cb=hri_goal_cb,
00317                                                            input_keys=['tts_in','left_in','right_in']),
00318                                transitions={'succeeded':'HAOD_USER_INPUT'},
00319                                remapping={'tts_in':'sm_hri_tts_q7',
00320                                           'left_in':'sm_hri_left_q7',
00321                                           'right_in':'sm_hri_right_q7'})
00322 
00323         sm.userdata.sm_hri_tts_q8  = " And now? Am I ok? "
00324         sm.userdata.sm_hri_left_q8 = ""
00325         sm.userdata.sm_hri_right_q8 = ""
00326         smach.StateMachine.add('HRI_QUESTION8',
00327                                smach_ros.SimpleActionState('hri',
00328                                                            sequenceAction,
00329                                                            goal_cb=hri_goal_cb,
00330                                                            input_keys=['tts_in','left_in','right_in']),
00331                                transitions={'succeeded':'HAOD_USER_INPUT'},
00332                                remapping={'tts_in':'sm_hri_tts_q8',
00333                                           'left_in':'sm_hri_left_q8',
00334                                           'right_in':'sm_hri_right_q8'})
00335 
00336         sm.userdata.sm_hri_tts_q9  = " Thanks to your help I'm getting better. come on! lend me a hand"
00337         sm.userdata.sm_hri_left_q9 = ""
00338         sm.userdata.sm_hri_right_q9 = ""
00339         smach.StateMachine.add('HRI_QUESTION9',
00340                                smach_ros.SimpleActionState('hri',
00341                                                            sequenceAction,
00342                                                            goal_cb=hri_goal_cb,
00343                                                            input_keys=['tts_in','left_in','right_in']),
00344                                transitions={'succeeded':'HAOD_USER_INPUT'},
00345                                remapping={'tts_in':'sm_hri_tts_q9',
00346                                           'left_in':'sm_hri_left_q9',
00347                                           'right_in':'sm_hri_right_q9'})
00348 
00349         sm.userdata.sm_hri_tts_q10  = " I know I'm asking a lot, but you are a good teacher!'"
00350         sm.userdata.sm_hri_left_q10 = ""
00351         sm.userdata.sm_hri_right_q10 = ""
00352         smach.StateMachine.add('HRI_QUESTION10',
00353                                smach_ros.SimpleActionState('hri',
00354                                                            sequenceAction,
00355                                                            goal_cb=hri_goal_cb,
00356                                                            input_keys=['tts_in','left_in','right_in']),
00357                                transitions={'succeeded':'HAOD_USER_INPUT'},
00358                                remapping={'tts_in':'sm_hri_tts_q10',
00359                                           'left_in':'sm_hri_left_q10',
00360                                           'right_in':'sm_hri_right_q10'})
00361 
00362         smach.StateMachine.add('HAOD_USER_INPUT',WaitUserInput(),
00363                                transitions={'valid_detection':'HAOD_LEARN',
00364                                             'invalid_detection':'HAOD_LEARN',
00365                                             'waiting':'HAOD_USER_INPUT',
00366                                             'no_user_input':'HAOD_DETECT'},
00367                                remapping={'valid_det':'valid'})
00368 
00369         smach.StateMachine.add('HAOD_LEARN',
00370                                smach_ros.ServiceState('learn',
00371                                                       haod_learn,
00372                                                       request_cb=learn_cb),
00373                                transitions={'succeeded':'HAOD_DETECT',
00374                                             'aborted':'aborted'},
00375                                remapping={'valid_det':'valid'})
00376 
00377         sm.userdata.sm_hri_tts_exit  = "Thanks for your help. I hope I see you soon! It's a pleasure to talk to you! Bye!"
00378         sm.userdata.sm_hri_left_exit = "left_arm_farewell.xml"
00379         sm.userdata.sm_hri_right_exit = "right_arm_farewell.xml"
00380         smach.StateMachine.add('HRI_EXIT',
00381                                smach_ros.SimpleActionState('hri',
00382                                                            sequenceAction,
00383                                                            goal_cb=hri_goal_cb,
00384                                                            input_keys=['tts_in','left_in','right_in']),
00385                                transitions={'succeeded':'succeeded'},
00386                                remapping={'tts_in':'sm_hri_tts_exit',
00387                                           'left_in':'sm_hri_left_exit',
00388                                           'right_in':'sm_hri_right_exit'})
00389                                
00390     # Create and start the introspection server
00391     sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00392     sis.start()
00393 
00394     # Execute the state machine
00395     outcome = sm.execute()
00396 
00397     # Wait for ctrl-c to stop the application
00398     rospy.spin()
00399     sis.stop()
00400 
00401 
00402 if __name__ == '__main__':
00403     main()
00404