00001
00002
00003 import roslib; roslib.load_manifest('tibi_dabo_smachs')
00004 import rospy
00005 import smach
00006 import smach_ros
00007 import tf
00008 import math
00009
00010 from tf.transformations import euler_from_quaternion
00011 from geometry_msgs.msg import Point, Pose, Quaternion
00012 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00013 from tibi_dabo_msgs.msg import sequenceAction, sequenceGoal
00014 from iri_perception_msgs.msg import peopleTrackAction
00015 from actionlib import *
00016 from actionlib.msg import *
00017 from smach_ros import SimpleActionState, ServiceState
00018
00019
00020 def hri_goal_cb(userdata, goal):
00021 rospy.loginfo('hri_goal_cb')
00022 tts_goal = sequenceGoal()
00023 tts_goal.sequence_file = [None]*5
00024 tts_goal.sequence_file[0] = userdata.tts_in
00025 tts_goal.sequence_file[1] = ""
00026 tts_goal.sequence_file[2] = userdata.head_in
00027 tts_goal.sequence_file[3] = ""
00028 tts_goal.sequence_file[4] = ""
00029 return tts_goal
00030
00031
00032 def move_base_goal_cb(userdata, goal):
00033 rospy.loginfo('move_base_goal_cb')
00034 nav_goal = MoveBaseGoal()
00035 nav_goal.target_pose.header.stamp = rospy.Time.now()
00036 nav_goal.target_pose.header.frame_id = userdata.frame_id_in
00037 p = Point(userdata.goal_x_in, userdata.goal_y_in, 0)
00038 q = tf.transformations.quaternion_from_euler(0, 0, userdata.goal_theta_in)
00039 nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
00040 return nav_goal
00041
00042
00043 def rotate_base_goal_cb(userdata,goal):
00044 rospy.loginfo('rotate_base_goal_cb')
00045 nav_goal = MoveBaseGoal()
00046 nav_goal.target_pose.header.stamp = rospy.Time.now()
00047 nav_goal.target_pose.header.frame_id = userdata.frame_id_in
00048 p = Point(userdata.goal_x_in/10, userdata.goal_y_in/10, 0)
00049 q = tf.transformations.quaternion_from_euler(0, 0, userdata.goal_theta_in)
00050 nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
00051 return nav_goal
00052
00053
00054 def pt_result_cb(userdata, state,result):
00055 rospy.loginfo('result!! num_persons=%d',len(result.ps.peopleSet))
00056 if len(result.ps.peopleSet)<1:
00057 return 'no_people'
00058 else:
00059 i = 0
00060 distance = 100
00061 index = -1
00062 while (i<len(result.ps.peopleSet)):
00063 userdata.frame_id_pt_out = result.ps.header.frame_id
00064 distance_aux = result.ps.peopleSet[i].x*result.ps.peopleSet[i].x+result.ps.peopleSet[i].y*result.ps.peopleSet[i].y
00065 if distance_aux < distance:
00066 distance = distance_aux
00067 userdata.x_pt_out = result.ps.peopleSet[i].x
00068 userdata.y_pt_out = result.ps.peopleSet[i].y
00069 userdata.id_pt_out = result.ps.peopleSet[i].targetId
00070 userdata.theta_pt_out = 0
00071 index = i
00072 i = i+1
00073 else:
00074 i = i+1
00075 if distance > 36:
00076 return 'people_far'
00077 else:
00078 if distance > 1.7*1.7:
00079 return 'max_dist'
00080 else:
00081 return 'min_dist'
00082
00083 class ComputeSubGoal(smach.State):
00084 def __init__(self):
00085 smach.State.__init__(self,
00086 outcomes=['person_far','person_close','person_left'],
00087 input_keys=['goal_x_in','goal_y_in','counter_in'],
00088 output_keys=['goal_x_in','goal_y_in','counter_in'])
00089 def execute(self, userdata):
00090 rospy.loginfo('Executing state ComputeSubGoal')
00091 distance = math.sqrt(userdata.goal_x_in*userdata.goal_x_in+userdata.goal_y_in*userdata.goal_y_in)
00092 if userdata.counter_in > 12:
00093 userdata.counter_in = 0
00094 return 'person_far'
00095 else:
00096 if distance < 1.5:
00097 userdata.counter_in = 0
00098 return 'person_close'
00099 else:
00100 userdata.goal_x_in = userdata.goal_x_in*1.5/distance
00101 userdata.goal_y_in = userdata.goal_y_in*1.5/distance
00102 userdata.counter_in = userdata.counter_in+1
00103 return 'person_far'
00104
00105 class AddCounter(smach.State):
00106 def __init__(self):
00107 smach.State.__init__(self,
00108 outcomes=['outcome1','outcome2'],
00109 input_keys=['counter_in'],
00110 output_keys=['counter_in'])
00111
00112 def execute(self, userdata):
00113 rospy.loginfo('Executing state AddCounter')
00114 if userdata.counter_in < 1:
00115 userdata.counter_in=userdata.counter_in+1
00116 return 'outcome1'
00117 else:
00118 return 'outcome2'
00119
00120
00121 def main():
00122 rospy.init_node('concurrent_hri_move_base')
00123
00124
00125 sm = smach.StateMachine(['succeeded','aborted','preempted'])
00126
00127
00128
00129 sm.userdata.sm_goal_x_2 = 2.0
00130 sm.userdata.sm_goal_y_2 = 0.0
00131 sm.userdata.sm_goal_theta_2 = 0.0
00132 sm.userdata.sm_counter = 0
00133
00134
00135 def hri_mb_out_cb(outcome_map):
00136 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_MOVE_BASE'] == 'succeeded':
00137 return 'succeeded'
00138 else:
00139 return 'aborted'
00140
00141
00142 def hri_rb_out_cb(outcome_map):
00143 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_ROTATE_BASE'] == 'succeeded':
00144 return 'succeeded'
00145 else:
00146 return 'aborted'
00147
00148
00149 hri_mb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00150 default_outcome='aborted',
00151 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00152 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00153 child_termination_cb = lambda so: False,
00154 outcome_cb = hri_mb_out_cb)
00155 with hri_mb_cc:
00156 smach.Concurrence.add('CC_HRI',
00157 smach_ros.SimpleActionState('hri',
00158 sequenceAction,
00159 goal_cb=hri_goal_cb,
00160 input_keys=['tts_in','head_in']),
00161 remapping={'tts_in':'cc_hri_tts',
00162 'head_in':'cc_hri_head'})
00163
00164 smach.Concurrence.add('CC_MOVE_BASE',
00165 smach_ros.SimpleActionState('MoveBase',
00166 MoveBaseAction,
00167 goal_cb=move_base_goal_cb,
00168 input_keys=['frame_id_in', 'goal_x_in',
00169 'goal_y_in', 'goal_theta_in']),
00170 remapping={'frame_id_in':'cc_frame_id',
00171 'goal_x_in':'cc_goal_x',
00172 'goal_y_in':'cc_goal_y',
00173 'goal_theta_in':'cc_goal_theta'})
00174
00175
00176 hri_rb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00177 default_outcome='aborted',
00178 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00179 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00180 child_termination_cb = lambda so: False,
00181 outcome_cb = hri_rb_out_cb)
00182 with hri_rb_cc:
00183 smach.Concurrence.add('CC_HRI',
00184 smach_ros.SimpleActionState('hri',
00185 sequenceAction,
00186 goal_cb=hri_goal_cb,
00187 input_keys=['tts_in','head_in']),
00188 remapping={'tts_in':'cc_hri_tts',
00189 'head_in':'cc_hri_head'})
00190
00191 smach.Concurrence.add('CC_ROTATE_BASE',
00192 smach_ros.SimpleActionState('MoveBase',
00193 MoveBaseAction,
00194 goal_cb=rotate_base_goal_cb,
00195 input_keys=['frame_id_in', 'goal_x_in',
00196 'goal_y_in', 'goal_theta_in']),
00197 remapping={'frame_id_in':'cc_frame_id',
00198 'goal_x_in':'cc_goal_x',
00199 'goal_y_in':'cc_goal_y',
00200 'goal_theta_in':'cc_goal_theta'})
00201
00202
00203
00204
00205 with sm:
00206
00207 sm.userdata.sm_hri_tts_1 ="I'd like to know if there's anybody here, who wants to teach me to detect faces"
00208 sm.userdata.sm_hri_head_1 = "head_random_1.xml"
00209 smach.StateMachine.add('HRI_BEFORE_CC',
00210 smach_ros.SimpleActionState('hri',
00211 sequenceAction,
00212 goal_cb=hri_goal_cb,
00213 input_keys=['tts_in', 'head_in']),
00214 transitions={'succeeded':'AC_PT_1'},
00215 remapping={'tts_in':'sm_hri_tts_1',
00216 'head_in':'sm_hri_head_1'})
00217
00218
00219 smach.StateMachine.add('AC_PT_1',
00220 smach_ros.SimpleActionState('people_tracking',
00221 peopleTrackAction,
00222 result_cb=pt_result_cb,
00223 outcomes=['min_dist','max_dist','no_people','people_far'],
00224 output_keys=['frame_id_pt_out','x_pt_out',
00225 'y_pt_out','id_pt_out','theta_pt_out']),
00226 transitions={'max_dist':'APPROACH_4',
00227 'min_dist':'HRI_MEET_1',
00228 'no_people':'HRI_FAIL',
00229 'people_far':'CC_HRI_RB_1'},
00230 remapping={'frame_id_pt_out':'sm_frame_id',
00231 'x_pt_out':'sm_goal_x',
00232 'y_pt_out':'sm_goal_y',
00233 'id_pt_out':'sm_target_id',
00234 'theta_pt_out':'sm_goal_theta'})
00235
00236
00237 sm.userdata.sm_hri_tts_2 = "I cannot see nobody here, I'll try again'"
00238 sm.userdata.sm_hri_head_2 = "head_random_4.xml"
00239 smach.StateMachine.add('HRI_FAIL',
00240 smach_ros.SimpleActionState('hri',
00241 sequenceAction,
00242 goal_cb=hri_goal_cb,
00243 input_keys=['tts_in', 'head_in']),
00244 transitions={'succeeded':'HRI_BEFORE_CC'},
00245 remapping={'tts_in':'sm_hri_tts_2',
00246 'head_in':'sm_hri_head_2'})
00247
00248
00249
00250 sm.userdata.sm_hri_tts_approach_4 = "I find someone. Please wait for me. I'm coming!"
00251 sm.userdata.sm_hri_head_approach_4 = ""
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263 smach.StateMachine.add('APPROACH_4',
00264 smach_ros.SimpleActionState('hri',
00265 sequenceAction,
00266 goal_cb=hri_goal_cb,
00267 input_keys=['tts_in', 'head_in']),
00268 transitions={'succeeded':'AC_PT_FP_4'},
00269 remapping={'tts_in':'sm_hri_tts_approach_4',
00270 'head_in':'sm_hri_head_approach_4'})
00271
00272
00273 smach.StateMachine.add('AC_PT_FP_4',
00274 smach_ros.SimpleActionState('people_tracking',
00275 peopleTrackAction,
00276 result_cb=pt_result_cb,
00277 outcomes=['min_dist','max_dist','no_people','people_far'],
00278 output_keys=['frame_id_pt_out','x_pt_out',
00279 'y_pt_out','id_pt_out','theta_pt_out']),
00280 transitions={'max_dist':'COMPUTE_SUB_GOAL_4',
00281 'min_dist':'HRI_MEET_1',
00282 'no_people':'HRI_FAIL',
00283 'people_far':'CC_HRI_RB_1'},
00284 remapping={'frame_id_pt_out':'sm_frame_id',
00285 'x_pt_out':'sm_goal_x',
00286 'y_pt_out':'sm_goal_y',
00287 'id_pt_out':'sm_target_id',
00288 'theta_pt_out':'sm_goal_theta'})
00289
00290 smach.StateMachine.add('COMPUTE_SUB_GOAL_4',
00291 ComputeSubGoal(),
00292 transitions={'person_far':'MOVE_BASE_4',
00293 'person_close':'HRI_MEET_1',
00294 'person_left':'HRI_FINAL'},
00295 remapping={'goal_x_in':'sm_goal_x',
00296 'goal_y_in':'sm_goal_y',
00297 'counter_in':'sm_counter'})
00298
00299 sm.userdata.sm_hri_tts_nothing = ""
00300 sm.userdata.sm_hri_head_nothing = ""
00301 smach.StateMachine.add('MOVE_BASE_4',
00302 hri_mb_cc,
00303 transitions={'succeeded':'AC_PT_FP_4',
00304 'aborted':'HRI_BLOCKED'},
00305 remapping={'cc_hri_tts':'sm_hri_tts_nothing',
00306 'cc_hri_head':'sm_hri_head_nothing',
00307 'cc_frame_id':'sm_frame_id',
00308 'cc_goal_x':'sm_goal_x',
00309 'cc_goal_y':'sm_goal_y',
00310 'cc_goal_theta':'sm_goal_theta'})
00311
00312
00313 sm.userdata.sm_hri_tts_meet = "Hey, how are you? Im Tibby. I'm trying to learn to detect faces."
00314 sm.userdata.sm_hri_head_meet = "head_random_2.xml"
00315 smach.StateMachine.add('HRI_MEET_1',
00316 hri_mb_cc,
00317 transitions={'succeeded':'AC_PT_2',
00318 'aborted':'HRI_BLOCKED'},
00319 remapping={'cc_hri_tts':'sm_hri_tts_meet',
00320 'cc_hri_head':'sm_hri_head_meet',
00321 'cc_frame_id':'sm_frame_id',
00322 'cc_goal_x':'sm_goal_x',
00323 'cc_goal_y':'sm_goal_y',
00324 'cc_goal_theta':'sm_goal_theta'})
00325
00326
00327 smach.StateMachine.add('AC_PT_2',
00328 smach_ros.SimpleActionState('people_tracking',
00329 peopleTrackAction,
00330 result_cb=pt_result_cb,
00331 outcomes=['min_dist','max_dist','no_people','people_far'],
00332 output_keys=['frame_id_pt_out','x_pt_out',
00333 'y_pt_out','id_pt_out','theta_pt_out']),
00334 transitions={'max_dist':'APPROACH_3',
00335 'min_dist':'HRI_NEAR_1',
00336 'no_people':'HRI_FAIL',
00337 'people_far':'CC_HRI_RB_1'},
00338 remapping={'frame_id_pt_out':'sm_frame_id',
00339 'x_pt_out':'sm_goal_x',
00340 'y_pt_out':'sm_goal_y',
00341 'id_pt_out':'sm_target_id',
00342 'theta_pt_out':'sm_goal_theta'})
00343
00344
00345 sm.userdata.sm_hri_tts_near = "We'll perform a short experiment. I need you to stay in front of me, while I start the detection process."
00346 sm.userdata.sm_hri_head_near = "head_random_2.xml"
00347 smach.StateMachine.add('HRI_NEAR_1',
00348 hri_mb_cc,
00349 transitions={'succeeded':'AC_PT_3',
00350 'aborted':'HRI_BLOCKED'},
00351 remapping={'cc_hri_tts':'sm_hri_tts_near',
00352 'cc_hri_head':'sm_hri_head_near',
00353 'cc_frame_id':'sm_frame_id',
00354 'cc_goal_x':'sm_goal_x',
00355 'cc_goal_y':'sm_goal_y',
00356 'cc_goal_theta':'sm_goal_theta'})
00357
00358
00359 sm.userdata.sm_hri_tts_approach_3 = "I'm talking to you, if you start moving I cannot concentrate"
00360 sm.userdata.sm_hri_head_approach_3 = ""
00361 smach.StateMachine.add('APPROACH_3',
00362 hri_rb_cc,
00363 transitions={'succeeded':'AC_PT_FP_3',
00364 'aborted':'HRI_BLOCKED'},
00365 remapping={'cc_hri_tts':'sm_hri_tts_approach_3',
00366 'cc_hri_head':'sm_hri_head_approach_3',
00367 'cc_frame_id':'sm_frame_id',
00368 'cc_goal_x':'sm_goal_x',
00369 'cc_goal_y':'sm_goal_y',
00370 'cc_goal_theta':'sm_goal_theta'})
00371
00372 smach.StateMachine.add('AC_PT_FP_3',
00373 smach_ros.SimpleActionState('people_tracking',
00374 peopleTrackAction,
00375 result_cb=pt_result_cb,
00376 outcomes=['min_dist','max_dist','no_people','people_far'],
00377 output_keys=['frame_id_pt_out','x_pt_out',
00378 'y_pt_out','id_pt_out','theta_pt_out']),
00379 transitions={'max_dist':'COMPUTE_SUB_GOAL_3',
00380 'min_dist':'HRI_NEAR_1',
00381 'no_people':'HRI_FAIL',
00382 'people_far':'CC_HRI_RB_1'},
00383 remapping={'frame_id_pt_out':'sm_frame_id',
00384 'x_pt_out':'sm_goal_x',
00385 'y_pt_out':'sm_goal_y',
00386 'id_pt_out':'sm_target_id',
00387 'theta_pt_out':'sm_goal_theta'})
00388
00389 smach.StateMachine.add('COMPUTE_SUB_GOAL_3',
00390 ComputeSubGoal(),
00391 transitions={'person_far':'MOVE_BASE_3',
00392 'person_close':'HRI_NEAR_1',
00393 'person_left':'HRI_FINAL'},
00394 remapping={'goal_x_in':'sm_goal_x',
00395 'goal_y_in':'sm_goal_y',
00396 'counter_in':'sm_counter'})
00397
00398 sm.userdata.sm_hri_tts_nothing = ""
00399 sm.userdata.sm_hri_head_nothing = ""
00400 smach.StateMachine.add('MOVE_BASE_3',
00401 hri_mb_cc,
00402 transitions={'succeeded':'AC_PT_FP_3',
00403 'aborted':'HRI_BLOCKED'},
00404 remapping={'cc_hri_tts':'sm_hri_tts_nothing',
00405 'cc_hri_head':'sm_hri_head_nothing',
00406 'cc_frame_id':'sm_frame_id',
00407 'cc_goal_x':'sm_goal_x',
00408 'cc_goal_y':'sm_goal_y',
00409 'cc_goal_theta':'sm_goal_theta'})
00410
00411
00412 smach.StateMachine.add('AC_PT_3',
00413 smach_ros.SimpleActionState('people_tracking',
00414 peopleTrackAction,
00415 result_cb=pt_result_cb,
00416 outcomes=['min_dist','max_dist','no_people','people_far'],
00417 output_keys=['frame_id_pt_out','x_pt_out',
00418 'y_pt_out','id_pt_out','theta_pt_out']),
00419 transitions={'max_dist':'APPROACH_2',
00420 'min_dist':'HRI_NEAR_2',
00421 'no_people':'HRI_FAIL',
00422 'people_far':'CC_HRI_RB_1'},
00423 remapping={'frame_id_pt_out':'sm_frame_id',
00424 'x_pt_out':'sm_goal_x',
00425 'y_pt_out':'sm_goal_y',
00426 'id_pt_out':'sm_target_id',
00427 'theta_pt_out':'sm_goal_theta'})
00428
00429
00430 sm.userdata.sm_hri_tts_near2 = "During the process, I'll ask you some questions if I have some doubts, I guess you can help me.'"
00431 sm.userdata.sm_hri_head_near2 = "head_random_2.xml"
00432 smach.StateMachine.add('HRI_NEAR_2',
00433 hri_rb_cc,
00434 transitions={'succeeded':'AC_PT_4',
00435 'aborted':'HRI_BLOCKED'},
00436 remapping={'cc_hri_tts':'sm_hri_tts_near2',
00437 'cc_hri_head':'sm_hri_head_near2',
00438 'cc_frame_id':'sm_frame_id',
00439 'cc_goal_x':'sm_goal_x',
00440 'cc_goal_y':'sm_goal_y',
00441 'cc_goal_theta':'sm_goal_theta'})
00442
00443 sm.userdata.sm_hri_tts_near2_1 = "As I was explaining to you, before you moved. I need you to stay in front of me."
00444 sm.userdata.sm_hri_head_near2_1 = "head_random_2.xml"
00445 smach.StateMachine.add('HRI_NEAR_2_1',
00446 hri_rb_cc,
00447 transitions={'succeeded':'AC_PT_4',
00448 'aborted':'HRI_BLOCKED'},
00449 remapping={'cc_hri_tts':'sm_hri_tts_near2_1',
00450 'cc_hri_head':'sm_hri_head_near2_1',
00451 'cc_frame_id':'sm_frame_id',
00452 'cc_goal_x':'sm_goal_x',
00453 'cc_goal_y':'sm_goal_y',
00454 'cc_goal_theta':'sm_goal_theta'})
00455
00456 sm.userdata.sm_hri_tts_approach_2 = "Are you nervous? please, wait until I finish, it's just a moment"
00457 sm.userdata.sm_hri_head_approach_2 = ""
00458 smach.StateMachine.add('APPROACH_2',
00459 hri_rb_cc,
00460 transitions={'succeeded':'AC_PT_FP_2',
00461 'aborted':'HRI_BLOCKED'},
00462 remapping={'cc_hri_tts':'sm_hri_tts_approach_2',
00463 'cc_hri_head':'sm_hri_head_approach_2',
00464 'cc_frame_id':'sm_frame_id',
00465 'cc_goal_x':'sm_goal_x',
00466 'cc_goal_y':'sm_goal_y',
00467 'cc_goal_theta':'sm_goal_theta'})
00468
00469 smach.StateMachine.add('AC_PT_FP_2',
00470 smach_ros.SimpleActionState('people_tracking',
00471 peopleTrackAction,
00472 result_cb=pt_result_cb,
00473 outcomes=['min_dist','max_dist','no_people','people_far'],
00474 output_keys=['frame_id_pt_out','x_pt_out',
00475 'y_pt_out','id_pt_out','theta_pt_out']),
00476 transitions={'max_dist':'COMPUTE_SUB_GOAL_2',
00477 'min_dist':'HRI_NEAR_2_1',
00478 'no_people':'HRI_FAIL',
00479 'people_far':'CC_HRI_RB_1'},
00480 remapping={'frame_id_pt_out':'sm_frame_id',
00481 'x_pt_out':'sm_goal_x',
00482 'y_pt_out':'sm_goal_y',
00483 'id_pt_out':'sm_target_id',
00484 'theta_pt_out':'sm_goal_theta'})
00485
00486 smach.StateMachine.add('COMPUTE_SUB_GOAL_2',
00487 ComputeSubGoal(),
00488 transitions={'person_far':'MOVE_BASE_2',
00489 'person_close':'HRI_NEAR_2_1',
00490 'person_left':'HRI_FINAL'},
00491 remapping={'goal_x_in':'sm_goal_x',
00492 'goal_y_in':'sm_goal_y',
00493 'counter_in':'sm_counter'})
00494
00495 sm.userdata.sm_hri_tts_nothing = ""
00496 sm.userdata.sm_hri_head_nothing = ""
00497 smach.StateMachine.add('MOVE_BASE_2',
00498 hri_mb_cc,
00499 transitions={'succeeded':'AC_PT_FP_2',
00500 'aborted':'HRI_BLOCKED'},
00501 remapping={'cc_hri_tts':'sm_hri_tts_nothing',
00502 'cc_hri_head':'sm_hri_head_nothing',
00503 'cc_frame_id':'sm_frame_id',
00504 'cc_goal_x':'sm_goal_x',
00505 'cc_goal_y':'sm_goal_y',
00506 'cc_goal_theta':'sm_goal_theta'})
00507
00508
00509
00510 smach.StateMachine.add('AC_PT_4',
00511 smach_ros.SimpleActionState('people_tracking',
00512 peopleTrackAction,
00513 result_cb=pt_result_cb,
00514 outcomes=['min_dist','max_dist','no_people','people_far'],
00515 output_keys=['frame_id_pt_out','x_pt_out',
00516 'y_pt_out','id_pt_out','theta_pt_out']),
00517 transitions={'max_dist':'APPROACH_1',
00518 'min_dist':'HRI_START_EXP',
00519 'no_people':'HRI_FAIL',
00520 'people_far':'CC_HRI_RB_1'},
00521 remapping={'frame_id_pt_out':'sm_frame_id',
00522 'x_pt_out':'sm_goal_x',
00523 'y_pt_out':'sm_goal_y',
00524 'id_pt_out':'sm_target_id',
00525 'theta_pt_out':'sm_goal_theta'})
00526
00527 sm.userdata.sm_hri_tts_start_exp = " Now, we are ready to start the experiment. Do not forget to stay in front of me."
00528 sm.userdata.sm_hri_head_start_exp = "head_home.xml"
00529 smach.StateMachine.add('HRI_START_EXP',
00530 hri_rb_cc,
00531 transitions={'succeeded':'succeeded',
00532 'aborted':'HRI_BLOCKED'},
00533 remapping={'cc_hri_tts':'sm_hri_tts_start_exp',
00534 'cc_hri_head':'sm_hri_head_start_exp',
00535 'cc_frame_id':'sm_frame_id',
00536 'cc_goal_x':'sm_goal_x',
00537 'cc_goal_y':'sm_goal_y',
00538 'cc_goal_theta':'sm_goal_theta'})
00539
00540
00541
00542 sm.userdata.sm_hri_tts_approach_1 = "please, don't you want to help me? I'll be so greatful. As I said, it's just a moment"
00543 sm.userdata.sm_hri_head_approach_1 = ""
00544 smach.StateMachine.add('APPROACH_1',
00545 hri_rb_cc,
00546 transitions={'succeeded':'AC_PT_FP_1',
00547 'aborted':'HRI_BLOCKED'},
00548 remapping={'cc_hri_tts':'sm_hri_tts_approach_1',
00549 'cc_hri_head':'sm_hri_head_approach_1',
00550 'cc_frame_id':'sm_frame_id',
00551 'cc_goal_x':'sm_goal_x',
00552 'cc_goal_y':'sm_goal_y',
00553 'cc_goal_theta':'sm_goal_theta'})
00554
00555 smach.StateMachine.add('AC_PT_FP_1',
00556 smach_ros.SimpleActionState('people_tracking',
00557 peopleTrackAction,
00558 result_cb=pt_result_cb,
00559 outcomes=['min_dist','max_dist','no_people','people_far'],
00560 output_keys=['frame_id_pt_out','x_pt_out',
00561 'y_pt_out','id_pt_out','theta_pt_out']),
00562 transitions={'max_dist':'COMPUTE_SUB_GOAL_1',
00563 'min_dist':'HRI_START_EXP',
00564 'no_people':'HRI_FAIL',
00565 'people_far':'CC_HRI_RB_1'},
00566 remapping={'frame_id_pt_out':'sm_frame_id',
00567 'x_pt_out':'sm_goal_x',
00568 'y_pt_out':'sm_goal_y',
00569 'id_pt_out':'sm_target_id',
00570 'theta_pt_out':'sm_goal_theta'})
00571
00572 smach.StateMachine.add('COMPUTE_SUB_GOAL_1',
00573 ComputeSubGoal(),
00574 transitions={'person_far':'MOVE_BASE_1',
00575 'person_close':'HRI_START_EXP',
00576 'person_left':'HRI_FINAL'},
00577 remapping={'goal_x_in':'sm_goal_x',
00578 'goal_y_in':'sm_goal_y',
00579 'counter_in':'sm_counter'})
00580
00581 sm.userdata.sm_hri_tts_nothing = ""
00582 sm.userdata.sm_hri_head_nothing = ""
00583 smach.StateMachine.add('MOVE_BASE_1',
00584 hri_mb_cc,
00585 transitions={'succeeded':'AC_PT_FP_1',
00586 'aborted':'HRI_BLOCKED'},
00587 remapping={'cc_hri_tts':'sm_hri_tts_nothing',
00588 'cc_hri_head':'sm_hri_head_nothing',
00589 'cc_frame_id':'sm_frame_id',
00590 'cc_goal_x':'sm_goal_x',
00591 'cc_goal_y':'sm_goal_y',
00592 'cc_goal_theta':'sm_goal_theta'})
00593
00594
00595
00596
00597
00598
00599
00600 sm.userdata.sm_hri_tts_people_far = "you are so far, I'll look for another person."
00601 sm.userdata.sm_hri_head_people_far = "head_random_2.xml"
00602 smach.StateMachine.add('CC_HRI_RB_1',
00603 hri_rb_cc,
00604 transitions={'succeeded':'HRI_BEFORE_CC',
00605 'aborted':'HRI_BLOCKED'},
00606 remapping={'cc_hri_tts':'sm_hri_tts_people_far',
00607 'cc_hri_head':'sm_hri_head_people_far',
00608 'cc_frame_id':'sm_frame_id',
00609 'cc_goal_x':'sm_goal_x_2',
00610 'cc_goal_y':'sm_goal_y_2',
00611 'cc_goal_theta':'sm_goal_theta_2'})
00612
00613
00614 sm.userdata.sm_hri_tts_final = "I see you do not want to talk to me. Good bye."
00615 sm.userdata.sm_hri_head_final = "head_home.xml"
00616 smach.StateMachine.add('HRI_FINAL',
00617 smach_ros.SimpleActionState('hri',
00618 sequenceAction,
00619 goal_cb=hri_goal_cb,
00620 input_keys=['tts_in', 'head_in']),
00621 transitions={'succeeded':'CC_HRI_RB_2'},
00622 remapping={'tts_in':'sm_hri_tts_final',
00623 'head_in':'sm_hri_head_final'})
00624
00625 sm.userdata.sm_hri_tts_people_left = "I'll look for someone who wants to interact with me."
00626 sm.userdata.sm_hri_head_people_left = "head_random_2.xml"
00627 smach.StateMachine.add('CC_HRI_RB_2',
00628 hri_rb_cc,
00629 transitions={'succeeded':'HRI_BEFORE_CC',
00630 'aborted':'HRI_BLOCKED'},
00631 remapping={'cc_hri_tts':'sm_hri_tts_people_left',
00632 'cc_hri_head':'sm_hri_head_people_left',
00633 'cc_frame_id':'sm_frame_id',
00634 'cc_goal_x':'sm_goal_x_2',
00635 'cc_goal_y':'sm_goal_y_2',
00636 'cc_goal_theta':'sm_goal_theta_2'})
00637
00638 sm.userdata.sm_hri_tts_blocked = "Sergi, Fernando, Anaiis, I cannot move, please help me."
00639 sm.userdata.sm_hri_head_blocked = "head_home.xml"
00640 smach.StateMachine.add('HRI_BLOCKED',
00641 smach_ros.SimpleActionState('hri',
00642 sequenceAction,
00643 goal_cb=hri_goal_cb,
00644 input_keys=['tts_in', 'head_in']),
00645 transitions={'succeeded':'HRI_BEFORE_CC'},
00646 remapping={'tts_in':'sm_hri_tts_blocked',
00647 'head_in':'sm_hri_head_blocked'})
00648
00649
00650
00651
00652
00653 sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00654 sis.start()
00655
00656
00657 outcome = sm.execute()
00658
00659
00660 rospy.spin()
00661 sis.stop()
00662
00663
00664 if __name__ == '__main__':
00665 main()
00666