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
00084
00085 class ComputeSubGoal(smach.State):
00086 def __init__(self):
00087 smach.State.__init__(self,
00088 outcomes=['person_far','person_close','person_left'],
00089 input_keys=['goal_x_in','goal_y_in','counter_in'],
00090 output_keys=['goal_x_in','goal_y_in','counter_in'])
00091 def execute(self, userdata):
00092 rospy.loginfo('Executing state ComputeSubGoal')
00093 distance = math.sqrt(userdata.goal_x_in*userdata.goal_x_in+userdata.goal_y_in*userdata.goal_y_in)
00094 if userdata.counter_in > 12:
00095 userdata.counter_in = 0
00096 return 'person_far'
00097 else:
00098 if distance < 1.5:
00099 userdata.counter_in = 0
00100 return 'person_close'
00101 else:
00102 userdata.goal_x_in = userdata.goal_x_in*1.5/distance
00103 userdata.goal_y_in = userdata.goal_y_in*1.5/distance
00104 userdata.counter_in = userdata.counter_in+1
00105 return 'person_far'
00106
00107 class AddCounter(smach.State):
00108 def __init__(self):
00109 smach.State.__init__(self,
00110 outcomes=['outcome1','outcome2'],
00111 input_keys=['counter_in'],
00112 output_keys=['counter_in'])
00113
00114 def execute(self, userdata):
00115 rospy.loginfo('Executing state AddCounter')
00116 if userdata.counter_in < 1:
00117 userdata.counter_in=userdata.counter_in+1
00118 return 'outcome1'
00119 else:
00120 return 'outcome2'
00121
00122
00123 def main():
00124 rospy.init_node('concurrent_hri_move_base')
00125
00126
00127 sm = smach.StateMachine(['succeeded','aborted','preempted'])
00128
00129
00130
00131 sm.userdata.sm_goal_x_2 = 2.0
00132 sm.userdata.sm_goal_y_2 = 0.0
00133 sm.userdata.sm_goal_theta_2 = 0.0
00134 sm.userdata.sm_counter = 0
00135
00136
00137 def hri_mb_out_cb(outcome_map):
00138 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_MOVE_BASE'] == 'succeeded':
00139 return 'succeeded'
00140 else:
00141 return 'aborted'
00142
00143
00144 def hri_rb_out_cb(outcome_map):
00145 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_ROTATE_BASE'] == 'succeeded':
00146 return 'succeeded'
00147 else:
00148 return 'aborted'
00149
00150
00151 hri_mb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00152 default_outcome='aborted',
00153 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00154 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00155 child_termination_cb = lambda so: False,
00156 outcome_cb = hri_mb_out_cb)
00157 with hri_mb_cc:
00158 smach.Concurrence.add('CC_HRI',
00159 smach_ros.SimpleActionState('hri',
00160 sequenceAction,
00161 goal_cb=hri_goal_cb,
00162 input_keys=['tts_in','head_in']),
00163 remapping={'tts_in':'cc_hri_tts',
00164 'head_in':'cc_hri_head'})
00165
00166 smach.Concurrence.add('CC_MOVE_BASE',
00167 smach_ros.SimpleActionState('MoveBase',
00168 MoveBaseAction,
00169 goal_cb=move_base_goal_cb,
00170 input_keys=['frame_id_in', 'goal_x_in',
00171 'goal_y_in', 'goal_theta_in']),
00172 remapping={'frame_id_in':'cc_frame_id',
00173 'goal_x_in':'cc_goal_x',
00174 'goal_y_in':'cc_goal_y',
00175 'goal_theta_in':'cc_goal_theta'})
00176
00177
00178 hri_rb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00179 default_outcome='aborted',
00180 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00181 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00182 child_termination_cb = lambda so: False,
00183 outcome_cb = hri_rb_out_cb)
00184 with hri_rb_cc:
00185 smach.Concurrence.add('CC_HRI',
00186 smach_ros.SimpleActionState('hri',
00187 sequenceAction,
00188 goal_cb=hri_goal_cb,
00189 input_keys=['tts_in','head_in']),
00190 remapping={'tts_in':'cc_hri_tts',
00191 'head_in':'cc_hri_head'})
00192
00193 smach.Concurrence.add('CC_ROTATE_BASE',
00194 smach_ros.SimpleActionState('MoveBase',
00195 MoveBaseAction,
00196 goal_cb=rotate_base_goal_cb,
00197 input_keys=['frame_id_in', 'goal_x_in',
00198 'goal_y_in', 'goal_theta_in']),
00199 remapping={'frame_id_in':'cc_frame_id',
00200 'goal_x_in':'cc_goal_x',
00201 'goal_y_in':'cc_goal_y',
00202 'goal_theta_in':'cc_goal_theta'})
00203
00204
00205
00206
00207 with sm:
00208
00209 smach.StateMachine.add('AC_PT_0',
00210 smach_ros.SimpleActionState('people_tracking',
00211 peopleTrackAction,
00212 result_cb=pt_result_cb,
00213 outcomes=['min_dist','max_dist','no_people','people_far'],
00214 output_keys=['frame_id_pt_out','x_pt_out',
00215 'y_pt_out','id_pt_out','theta_pt_out']),
00216 transitions={'max_dist':'AC_PT_0',
00217 'min_dist':'CC_HRI_MB_PRESENTATION',
00218 'no_people':'AC_PT_0',
00219 'people_far':'AC_PT_0'},
00220 remapping={'frame_id_pt_out':'sm_frame_id',
00221 'x_pt_out':'sm_goal_x',
00222 'y_pt_out':'sm_goal_y',
00223 'id_pt_out':'sm_target_id',
00224 'theta_pt_out':'sm_goal_theta'})
00225
00226 sm.userdata.sm_hri_tts_presentation = "Hey, how are you? Im Tibby. I will be your guide. I will accompany you to the meeting point. Follow me please."
00227 sm.userdata.sm_hri_head_presentation = "head_random_2.xml"
00228 smach.StateMachine.add('CC_HRI_MB_PRESENTATION',
00229 hri_mb_cc,
00230 transitions={'succeeded':'AC_PT_01'},
00231 remapping={'cc_hri_tts':'sm_hri_tts_presentation',
00232 'cc_hri_head':'sm_hri_head_presentation',
00233 'cc_frame_id':'sm_frame_id',
00234 'cc_goal_x':'sm_goal_x',
00235 'cc_goal_y':'sm_goal_y',
00236 'cc_goal_theta':'sm_goal_theta'})
00237
00238
00239 smach.StateMachine.add('AC_PT_01',
00240 smach_ros.SimpleActionState('people_tracking',
00241 peopleTrackAction,
00242 result_cb=pt_result_cb,
00243 outcomes=['min_dist','max_dist','no_people','people_far'],
00244 output_keys=['frame_id_pt_out','x_pt_out',
00245 'y_pt_out','id_pt_out','theta_pt_out']),
00246 transitions={'max_dist':'FERNANDO',
00247 'min_dist':'FERNANDO',
00248 'no_people':'AC_PT_0',
00249 'people_far':'AC_PT_0'},
00250 remapping={'frame_id_pt_out':'sm_frame_id',
00251 'x_pt_out':'sm_goal_x',
00252 'y_pt_out':'sm_goal_y',
00253 'id_pt_out':'sm_target_id',
00254 'theta_pt_out':'sm_goal_theta'})
00255
00256
00257
00258
00259
00260
00261
00262 smach.StateMachine.add('AC_PT_1',
00263 smach_ros.SimpleActionState('people_tracking',
00264 peopleTrackAction,
00265 result_cb=pt_result_cb,
00266 outcomes=['min_dist','max_dist','no_people','people_far'],
00267 output_keys=['frame_id_pt_out','x_pt_out',
00268 'y_pt_out','id_pt_out','theta_pt_out']),
00269 transitions={'max_dist':'APPROACH_4',
00270 'min_dist':'HRI_MEET_1',
00271 'no_people':'HRI_FAIL',
00272 'people_far':'CC_HRI_RB_1'},
00273 remapping={'frame_id_pt_out':'sm_frame_id',
00274 'x_pt_out':'sm_goal_x',
00275 'y_pt_out':'sm_goal_y',
00276 'id_pt_out':'sm_target_id',
00277 'theta_pt_out':'sm_goal_theta'})
00278
00279
00280 sm.userdata.sm_hri_tts_2 = "I cannot see nobody here, I'll start the experiment again try again'"
00281 sm.userdata.sm_hri_head_2 = "head_random_4.xml"
00282 smach.StateMachine.add('HRI_FAIL',
00283 smach_ros.SimpleActionState('hri',
00284 sequenceAction,
00285 goal_cb=hri_goal_cb,
00286 input_keys=['tts_in', 'head_in']),
00287 transitions={'succeeded':'AC_PT_0'},
00288 remapping={'tts_in':'sm_hri_tts_2',
00289 'head_in':'sm_hri_head_2'})
00290
00291
00292
00293 sm.userdata.sm_hri_tts_approach_4 = "Why did you leave? You only have to follow me."
00294 sm.userdata.sm_hri_head_approach_4 = ""
00295 smach.StateMachine.add('APPROACH_4',
00296 smach_ros.SimpleActionState('hri',
00297 sequenceAction,
00298 goal_cb=hri_goal_cb,
00299 input_keys=['tts_in', 'head_in']),
00300 transitions={'succeeded':'AC_PT_FP_4'},
00301 remapping={'tts_in':'sm_hri_tts_approach_4',
00302 'head_in':'sm_hri_head_approach_4'})
00303
00304
00305 smach.StateMachine.add('AC_PT_FP_4',
00306 smach_ros.SimpleActionState('people_tracking',
00307 peopleTrackAction,
00308 result_cb=pt_result_cb,
00309 outcomes=['min_dist','max_dist','no_people','people_far'],
00310 output_keys=['frame_id_pt_out','x_pt_out',
00311 'y_pt_out','id_pt_out','theta_pt_out']),
00312 transitions={'max_dist':'COMPUTE_SUB_GOAL_4',
00313 'min_dist':'HRI_MEET_1',
00314 'no_people':'HRI_FAIL',
00315 'people_far':'CC_HRI_RB_1'},
00316 remapping={'frame_id_pt_out':'sm_frame_id',
00317 'x_pt_out':'sm_goal_x',
00318 'y_pt_out':'sm_goal_y',
00319 'id_pt_out':'sm_target_id',
00320 'theta_pt_out':'sm_goal_theta'})
00321
00322 smach.StateMachine.add('COMPUTE_SUB_GOAL_4',
00323 ComputeSubGoal(),
00324 transitions={'person_far':'MOVE_BASE_4',
00325 'person_close':'HRI_MEET_1',
00326 'person_left':'HRI_FINAL'},
00327 remapping={'goal_x_in':'sm_goal_x',
00328 'goal_y_in':'sm_goal_y',
00329 'counter_in':'sm_counter'})
00330
00331 sm.userdata.sm_hri_tts_nothing = ""
00332 sm.userdata.sm_hri_head_nothing = ""
00333 smach.StateMachine.add('MOVE_BASE_4',
00334 hri_mb_cc,
00335 transitions={'succeeded':'AC_PT_FP_4',
00336 'aborted':'HRI_BLOCKED'},
00337 remapping={'cc_hri_tts':'sm_hri_tts_nothing',
00338 'cc_hri_head':'sm_hri_head_nothing',
00339 'cc_frame_id':'sm_frame_id',
00340 'cc_goal_x':'sm_goal_x',
00341 'cc_goal_y':'sm_goal_y',
00342 'cc_goal_theta':'sm_goal_theta'})
00343
00344
00345 sm.userdata.sm_hri_tts_meet = "If do not walk next to me, I cannot see you. Lets continue."
00346 sm.userdata.sm_hri_head_meet = "head_random_2.xml"
00347 smach.StateMachine.add('HRI_MEET_1',
00348 hri_mb_cc,
00349 transitions={'succeeded':'AC_PT_01',
00350 'aborted':'HRI_BLOCKED'},
00351 remapping={'cc_hri_tts':'sm_hri_tts_meet',
00352 'cc_hri_head':'sm_hri_head_meet',
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 smach.StateMachine.add('AC_PT_2',
00360 smach_ros.SimpleActionState('people_tracking',
00361 peopleTrackAction,
00362 result_cb=pt_result_cb,
00363 outcomes=['min_dist','max_dist','no_people','people_far'],
00364 output_keys=['frame_id_pt_out','x_pt_out',
00365 'y_pt_out','id_pt_out','theta_pt_out']),
00366 transitions={'max_dist':'APPROACH_3',
00367 'min_dist':'HRI_NEAR_1',
00368 'no_people':'HRI_FAIL',
00369 'people_far':'CC_HRI_RB_1'},
00370 remapping={'frame_id_pt_out':'sm_frame_id',
00371 'x_pt_out':'sm_goal_x',
00372 'y_pt_out':'sm_goal_y',
00373 'id_pt_out':'sm_target_id',
00374 'theta_pt_out':'sm_goal_theta'})
00375
00376
00377 sm.userdata.sm_hri_tts_near = "We'll perform a short experiment. I need you to stay close to me. You only have to follow me."
00378 sm.userdata.sm_hri_head_near = "head_random_2.xml"
00379 smach.StateMachine.add('HRI_NEAR_1',
00380 hri_mb_cc,
00381 transitions={'succeeded':'AC_PT_3',
00382 'aborted':'HRI_BLOCKED'},
00383 remapping={'cc_hri_tts':'sm_hri_tts_near',
00384 'cc_hri_head':'sm_hri_head_near',
00385 'cc_frame_id':'sm_frame_id',
00386 'cc_goal_x':'sm_goal_x',
00387 'cc_goal_y':'sm_goal_y',
00388 'cc_goal_theta':'sm_goal_theta'})
00389
00390
00391 sm.userdata.sm_hri_tts_approach_3 = "I'm talking to you, if you start moving I cannot concentrate"
00392 sm.userdata.sm_hri_head_approach_3 = ""
00393 smach.StateMachine.add('APPROACH_3',
00394 hri_rb_cc,
00395 transitions={'succeeded':'AC_PT_FP_3',
00396 'aborted':'HRI_BLOCKED'},
00397 remapping={'cc_hri_tts':'sm_hri_tts_approach_3',
00398 'cc_hri_head':'sm_hri_head_approach_3',
00399 'cc_frame_id':'sm_frame_id',
00400 'cc_goal_x':'sm_goal_x',
00401 'cc_goal_y':'sm_goal_y',
00402 'cc_goal_theta':'sm_goal_theta'})
00403
00404 smach.StateMachine.add('AC_PT_FP_3',
00405 smach_ros.SimpleActionState('people_tracking',
00406 peopleTrackAction,
00407 result_cb=pt_result_cb,
00408 outcomes=['min_dist','max_dist','no_people','people_far'],
00409 output_keys=['frame_id_pt_out','x_pt_out',
00410 'y_pt_out','id_pt_out','theta_pt_out']),
00411 transitions={'max_dist':'COMPUTE_SUB_GOAL_3',
00412 'min_dist':'HRI_NEAR_1',
00413 'no_people':'HRI_FAIL',
00414 'people_far':'CC_HRI_RB_1'},
00415 remapping={'frame_id_pt_out':'sm_frame_id',
00416 'x_pt_out':'sm_goal_x',
00417 'y_pt_out':'sm_goal_y',
00418 'id_pt_out':'sm_target_id',
00419 'theta_pt_out':'sm_goal_theta'})
00420
00421 smach.StateMachine.add('COMPUTE_SUB_GOAL_3',
00422 ComputeSubGoal(),
00423 transitions={'person_far':'MOVE_BASE_3',
00424 'person_close':'HRI_NEAR_1',
00425 'person_left':'HRI_FINAL'},
00426 remapping={'goal_x_in':'sm_goal_x',
00427 'goal_y_in':'sm_goal_y',
00428 'counter_in':'sm_counter'})
00429
00430 sm.userdata.sm_hri_tts_nothing = ""
00431 sm.userdata.sm_hri_head_nothing = ""
00432 smach.StateMachine.add('MOVE_BASE_3',
00433 hri_mb_cc,
00434 transitions={'succeeded':'AC_PT_FP_3',
00435 'aborted':'HRI_BLOCKED'},
00436 remapping={'cc_hri_tts':'sm_hri_tts_nothing',
00437 'cc_hri_head':'sm_hri_head_nothing',
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
00444 smach.StateMachine.add('AC_PT_3',
00445 smach_ros.SimpleActionState('people_tracking',
00446 peopleTrackAction,
00447 result_cb=pt_result_cb,
00448 outcomes=['min_dist','max_dist','no_people','people_far'],
00449 output_keys=['frame_id_pt_out','x_pt_out',
00450 'y_pt_out','id_pt_out','theta_pt_out']),
00451 transitions={'max_dist':'HRI_FINAL',
00452 'min_dist':'HRI_NEAR_2',
00453 'no_people':'HRI_FAIL',
00454 'people_far':'CC_HRI_RB_2'},
00455 remapping={'frame_id_pt_out':'sm_frame_id',
00456 'x_pt_out':'sm_goal_x',
00457 'y_pt_out':'sm_goal_y',
00458 'id_pt_out':'sm_target_id',
00459 'theta_pt_out':'sm_goal_theta'})
00460
00461
00462 sm.userdata.sm_hri_tts_near2 = "Lets continue."
00463 sm.userdata.sm_hri_head_near2 = "head_random_2.xml"
00464 smach.StateMachine.add('HRI_NEAR_2',
00465 hri_rb_cc,
00466 transitions={'succeeded':'AC_PT_4',
00467 'aborted':'HRI_BLOCKED'},
00468 remapping={'cc_hri_tts':'sm_hri_tts_near2',
00469 'cc_hri_head':'sm_hri_head_near2',
00470 'cc_frame_id':'sm_frame_id',
00471 'cc_goal_x':'sm_goal_x',
00472 'cc_goal_y':'sm_goal_y',
00473 'cc_goal_theta':'sm_goal_theta'})
00474
00475
00476
00477
00478 smach.StateMachine.add('AC_PT_4',
00479 smach_ros.SimpleActionState('people_tracking',
00480 peopleTrackAction,
00481 result_cb=pt_result_cb,
00482 outcomes=['min_dist','max_dist','no_people','people_far'],
00483 output_keys=['frame_id_pt_out','x_pt_out',
00484 'y_pt_out','id_pt_out','theta_pt_out']),
00485 transitions={'max_dist':'CC_HRI_RB_2',
00486 'min_dist':'AC_PT_01',
00487 'no_people':'HRI_FAIL',
00488 'people_far':'CC_HRI_RB_1'},
00489 remapping={'frame_id_pt_out':'sm_frame_id',
00490 'x_pt_out':'sm_goal_x',
00491 'y_pt_out':'sm_goal_y',
00492 'id_pt_out':'sm_target_id',
00493 'theta_pt_out':'sm_goal_theta'})
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504 sm.userdata.sm_hri_tts_people_far = "you are so far, I'll look for another person."
00505 sm.userdata.sm_hri_head_people_far = "head_random_2.xml"
00506 smach.StateMachine.add('CC_HRI_RB_1',
00507 hri_rb_cc,
00508 transitions={'succeeded':'HRI_BEFORE_CC',
00509 'aborted':'HRI_BLOCKED'},
00510 remapping={'cc_hri_tts':'sm_hri_tts_people_far',
00511 'cc_hri_head':'sm_hri_head_people_far',
00512 'cc_frame_id':'sm_frame_id',
00513 'cc_goal_x':'sm_goal_x_2',
00514 'cc_goal_y':'sm_goal_y_2',
00515 'cc_goal_theta':'sm_goal_theta_2'})
00516
00517
00518 sm.userdata.sm_hri_tts_final = "I see you do not want to be with me. Good bye."
00519 sm.userdata.sm_hri_head_final = "head_home.xml"
00520 smach.StateMachine.add('HRI_FINAL',
00521 smach_ros.SimpleActionState('hri',
00522 sequenceAction,
00523 goal_cb=hri_goal_cb,
00524 input_keys=['tts_in', 'head_in']),
00525 transitions={'succeeded':'CC_HRI_RB_2'},
00526 remapping={'tts_in':'sm_hri_tts_final',
00527 'head_in':'sm_hri_head_final'})
00528
00529 sm.userdata.sm_hri_tts_people_left = "I'll look for someone else who wants to interact with me."
00530 sm.userdata.sm_hri_head_people_left = "head_random_2.xml"
00531 smach.StateMachine.add('CC_HRI_RB_2',
00532 hri_rb_cc,
00533 transitions={'succeeded':'HRI_BEFORE_CC',
00534 'aborted':'HRI_BLOCKED'},
00535 remapping={'cc_hri_tts':'sm_hri_tts_people_left',
00536 'cc_hri_head':'sm_hri_head_people_left',
00537 'cc_frame_id':'sm_frame_id',
00538 'cc_goal_x':'sm_goal_x_2',
00539 'cc_goal_y':'sm_goal_y_2',
00540 'cc_goal_theta':'sm_goal_theta_2'})
00541
00542 sm.userdata.sm_hri_tts_blocked = "Sergi, Fernando, Anaiis, I cannot move, please help me."
00543 sm.userdata.sm_hri_head_blocked = "head_home.xml"
00544 smach.StateMachine.add('HRI_BLOCKED',
00545 smach_ros.SimpleActionState('hri',
00546 sequenceAction,
00547 goal_cb=hri_goal_cb,
00548 input_keys=['tts_in', 'head_in']),
00549 transitions={'succeeded':'HRI_BEFORE_CC'},
00550 remapping={'tts_in':'sm_hri_tts_blocked',
00551 'head_in':'sm_hri_head_blocked'})
00552
00553
00554
00555
00556
00557 sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00558 sis.start()
00559
00560
00561 outcome = sm.execute()
00562
00563
00564 rospy.spin()
00565 sis.stop()
00566
00567
00568 if __name__ == '__main__':
00569 main()
00570