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 import sys
00010
00011 from tf.transformations import euler_from_quaternion
00012 from geometry_msgs.msg import Point, Pose, Quaternion
00013 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00014 from tibi_dabo_msgs.msg import sequenceAction, sequenceGoal
00015 from tibi_dabo_msgs.msg import guideGoalAction, guideGoalGoal
00016 from iri_perception_msgs.msg import peopleTrackAction
00017 from actionlib import *
00018 from actionlib.msg import *
00019 from smach_ros import SimpleActionState, ServiceState
00020
00021
00022 def hri_goal_cb(userdata, goal):
00023 rospy.loginfo('hri_goal_cb')
00024 tts_goal = sequenceGoal()
00025 tts_goal.sequence_file = [None]*5
00026 tts_goal.sequence_file[0] = userdata.tts_in
00027 tts_goal.sequence_file[1] = ""
00028 tts_goal.sequence_file[2] = userdata.head_in
00029 tts_goal.sequence_file[3] = ""
00030 tts_goal.sequence_file[4] = ""
00031 return tts_goal
00032
00033
00034 def move_base_goal_cb(userdata, goal):
00035 rospy.loginfo('move_base_goal_cb')
00036 nav_goal = MoveBaseGoal()
00037 nav_goal.target_pose.header.stamp = rospy.Time.now()
00038 nav_goal.target_pose.header.frame_id = userdata.frame_id_in
00039 p = Point(userdata.goal_x_in, userdata.goal_y_in, 0)
00040 q = tf.transformations.quaternion_from_euler(0, 0, userdata.goal_theta_in)
00041 nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
00042 return nav_goal
00043
00044
00045 def guide_goal_cb(userdata, goal):
00046 rospy.loginfo('guide_goal_cb')
00047 guide_goal = guideGoalGoal()
00048 guide_goal.target_pose.header.stamp = rospy.Time.now()
00049 guide_goal.target_pose.header.frame_id = userdata.frame_id_in
00050 p = Point(userdata.goal_x_in, userdata.goal_y_in, 0)
00051 q = tf.transformations.quaternion_from_euler(0, 0, userdata.goal_theta_in)
00052 guide_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
00053 guide_goal.target_id = userdata.id_pt_in
00054 return guide_goal
00055
00056 def guide_result_cb(userdata,state,result):
00057 rospy.loginfo('guide_result_cb')
00058 if result.result_code == 0 :
00059 rospy.loginfo('tibi is lost')
00060 return 'lost'
00061 if result.result_code == 1 :
00062 rospy.loginfo('tibi is slowing down')
00063 return 'slow_down'
00064 if result.result_code == 2 :
00065 rospy.loginfo('tibi is far')
00066 return 'far'
00067 if result.result_code == 10 :
00068 rospy.loginfo('tibi has finished')
00069 return 'done'
00070
00071
00072
00073
00074 def rotate_base_goal_cb(userdata,goal):
00075 rospy.loginfo('rotate_base_goal_cb')
00076 nav_goal = MoveBaseGoal()
00077 nav_goal.target_pose.header.stamp = rospy.Time.now()
00078 nav_goal.target_pose.header.frame_id = userdata.frame_id_in
00079 p = Point(userdata.goal_x_in/10, userdata.goal_y_in/10, 0)
00080 q = tf.transformations.quaternion_from_euler(0, 0, userdata.goal_theta_in)
00081 nav_goal.target_pose.pose = Pose(position=p, orientation=Quaternion(*q))
00082 return nav_goal
00083
00084
00085 def pt_result_cb(userdata, state,result):
00086 sys.stdout.write('result callback');
00087 sys.stdout.flush()
00088 rospy.loginfo('result!! num_persons=%d',len(result.ps.peopleSet))
00089 if len(result.ps.peopleSet)<1:
00090 return 'no_people'
00091 else:
00092 i = 0
00093 distance = 100
00094 index = -1
00095 while (i<len(result.ps.peopleSet)):
00096 userdata.frame_id_pt_out = result.ps.header.frame_id
00097 distance_aux = result.ps.peopleSet[i].x*result.ps.peopleSet[i].x+result.ps.peopleSet[i].y*result.ps.peopleSet[i].y
00098 if distance_aux < distance:
00099 distance = distance_aux
00100 userdata.x_pt_out = result.ps.peopleSet[i].x
00101 userdata.y_pt_out = result.ps.peopleSet[i].y
00102 userdata.id_pt_out = result.ps.peopleSet[i].targetId
00103 userdata.theta_pt_out = 0
00104 index = i
00105 i = i+1
00106 else:
00107 i = i+1
00108 if distance > 36:
00109 return 'people_far'
00110 else:
00111 if distance > 1.7*1.7:
00112 return 'max_dist'
00113 else:
00114 return 'min_dist'
00115
00116
00117 def pt_behind_result_cb(userdata, state,result):
00118 sys.stdout.write('result callback');
00119 sys.stdout.flush()
00120 rospy.loginfo('result!! num_persons=%d',len(result.ps.peopleSet))
00121 if len(result.ps.peopleSet)<1:
00122 return 'no_people'
00123 else:
00124 i = 0
00125 distance = 100
00126 index = -1
00127 while (i<len(result.ps.peopleSet)):
00128 userdata.frame_id_pt_out = result.ps.header.frame_id
00129 distance_aux = result.ps.peopleSet[i].x*result.ps.peopleSet[i].x+result.ps.peopleSet[i].y*result.ps.peopleSet[i].y
00130 if distance_aux < distance:
00131 distance = distance_aux
00132 userdata.x_pt_out = result.ps.peopleSet[i].x
00133 userdata.y_pt_out = result.ps.peopleSet[i].y
00134 userdata.id_pt_out = result.ps.peopleSet[i].targetId
00135 userdata.theta_pt_out = 0
00136 index = i
00137 i = i+1
00138 else:
00139 i = i+1
00140 if distance > 1.5*1.5:
00141 return 'people_far'
00142 else:
00143 if i>=len(result.ps.peopleSet):
00144 if result.ps.peopleSet[i-1].x > 0:
00145 return 'people_front'
00146 else:
00147 return 'min_dist'
00148
00149
00150 def pt_behind2_result_cb(userdata, state,result):
00151 sys.stdout.write('result callback');
00152 sys.stdout.flush()
00153 rospy.loginfo('result!! num_persons=%d',len(result.ps.peopleSet))
00154 if len(result.ps.peopleSet)<1:
00155 return 'no_people'
00156 else:
00157 i = 0
00158 distance = 100
00159 index = -1
00160 while (i<len(result.ps.peopleSet)):
00161 userdata.frame_id_pt_out = result.ps.header.frame_id
00162 distance_aux = result.ps.peopleSet[i].x*result.ps.peopleSet[i].x+result.ps.peopleSet[i].y*result.ps.peopleSet[i].y
00163 if distance_aux < distance:
00164 distance = distance_aux
00165 userdata.x_pt_out = result.ps.peopleSet[i].x
00166 userdata.y_pt_out = result.ps.peopleSet[i].y
00167 userdata.id_pt_out = result.ps.peopleSet[i].targetId
00168 userdata.theta_pt_out = 0
00169 index = i
00170 i = i+1
00171 else:
00172 i = i+1
00173 if distance > 16:
00174 return 'people_far'
00175 else:
00176 if i>=len(result.ps.peopleSet):
00177 if result.ps.peopleSet[i-1].x > 0:
00178 return 'people_front'
00179 else:
00180 if distance>4:
00181 return 'people_war'
00182 else:
00183 return 'min_dist'
00184
00185
00186 def pt_front_result_cb(userdata, state,result):
00187 sys.stdout.write('result callback');
00188 sys.stdout.flush()
00189 rospy.loginfo('result!! num_persons=%d',len(result.ps.peopleSet))
00190 if len(result.ps.peopleSet)<1:
00191 return 'no_people'
00192 else:
00193 i = 0
00194 distance = 100
00195 index = -1
00196 while (i<len(result.ps.peopleSet)):
00197 userdata.frame_id_pt_out = result.ps.header.frame_id
00198 distance_aux = result.ps.peopleSet[i].x*result.ps.peopleSet[i].x+result.ps.peopleSet[i].y*result.ps.peopleSet[i].y
00199 if distance_aux < distance:
00200 distance = distance_aux
00201 userdata.x_pt_out = result.ps.peopleSet[i].x
00202 userdata.y_pt_out = result.ps.peopleSet[i].y
00203 userdata.id_pt_out = result.ps.peopleSet[i].targetId
00204 userdata.theta_pt_out = 0
00205 index = i
00206 i = i+1
00207 else:
00208 i = i+1
00209
00210 if i>=len(result.ps.peopleSet):
00211 if result.ps.peopleSet[i-1].x < 0:
00212 return 'people_back'
00213 else:
00214 if distance>4:
00215 return 'people_far'
00216 else:
00217 return 'people_close'
00218
00219
00220
00221
00222
00223 class ComputeSubGoal(smach.State):
00224 def __init__(self):
00225 smach.State.__init__(self,
00226 outcomes=['person_far','person_close','person_left'],
00227 input_keys=['goal_x_in','goal_y_in','counter_in'],
00228 output_keys=['goal_x_in','goal_y_in','counter_in'])
00229 def execute(self, userdata):
00230 rospy.loginfo('Executing state ComputeSubGoal')
00231 distance = math.sqrt(userdata.goal_x_in*userdata.goal_x_in+userdata.goal_y_in*userdata.goal_y_in)
00232 if userdata.counter_in > 12:
00233 userdata.counter_in = 0
00234 return 'person_far'
00235 else:
00236 if distance < 1.5:
00237 userdata.counter_in = 0
00238 return 'person_close'
00239 else:
00240 userdata.goal_x_in = userdata.goal_x_in*1.5/distance
00241 userdata.goal_y_in = userdata.goal_y_in*1.5/distance
00242 userdata.counter_in = userdata.counter_in+1
00243 return 'person_far'
00244 class ComputeGoalToApproach(smach.State):
00245 def __init__(self):
00246 smach.State.__init__(self,
00247 outcomes=['person_far','person_close'],
00248 input_keys=['goal_x_in','goal_y_in'],
00249 output_keys=['goal_x_in','goal_y_in'])
00250 def execute(self, userdata):
00251 rospy.loginfo('Executing state ComputeSubGoal')
00252 distance = math.sqrt(userdata.goal_x_in*userdata.goal_x_in+userdata.goal_y_in*userdata.goal_y_in)
00253 if distance < 1.:
00254 return 'person_close'
00255 else:
00256 userdata.goal_x_in = userdata.goal_x_in*.75
00257 userdata.goal_y_in = userdata.goal_y_in*.75
00258 return 'person_far'
00259
00260 class AddCounter(smach.State):
00261 def __init__(self):
00262 smach.State.__init__(self,
00263 outcomes=['outcome1','outcome2'],
00264 input_keys=['counter_in'],
00265 output_keys=['counter_in'])
00266
00267 def execute(self, userdata):
00268 rospy.loginfo('Executing state AddCounter')
00269 if userdata.counter_in < 1:
00270 userdata.counter_in=userdata.counter_in+1
00271 return 'outcome1'
00272 else:
00273 return 'outcome2'
00274
00275
00276 class Counting_iterations(smach.State):
00277 def __init__(self):
00278 smach.State.__init__(self,
00279 outcomes=['wait','abort'],
00280 input_keys=['counter_it'],
00281 output_keys=['counter_it'])
00282
00283 def execute(self, userdata):
00284 rospy.loginfo('Executing state Counting iterations')
00285 if userdata.counter_it < 1000:
00286 userdata.counter_it=userdata.counter_it+1
00287 return 'wait'
00288 else:
00289 userdata.counter_it=0
00290 return 'abort'
00291
00292
00293 def main():
00294 rospy.init_node('concurrent_hri_move_base')
00295
00296
00297 sm = smach.StateMachine(['succeeded','aborted','preempted'])
00298
00299
00300
00301 sm.userdata.sm_goal_x_2 = 2.0
00302 sm.userdata.sm_goal_y_2 = 0.0
00303 sm.userdata.sm_goal_theta_2 = 0.0
00304 sm.userdata.sm_counter = 0
00305 sm.userdata.sm_counter_iterations = 0
00306
00307 def hri_mb_out_cb(outcome_map):
00308 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_MOVE_BASE'] == 'succeeded':
00309 return 'succeeded'
00310 else:
00311 return 'aborted'
00312
00313
00314 def hri_rb_out_cb(outcome_map):
00315 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_ROTATE_BASE'] == 'succeeded':
00316 return 'succeeded'
00317 else:
00318 return 'aborted'
00319
00320
00321 hri_mb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00322 default_outcome='aborted',
00323 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00324 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00325 child_termination_cb = lambda so: False,
00326 outcome_cb = hri_mb_out_cb)
00327 with hri_mb_cc:
00328 smach.Concurrence.add('CC_HRI',
00329 smach_ros.SimpleActionState('hri',
00330 sequenceAction,
00331 goal_cb=hri_goal_cb,
00332 input_keys=['tts_in','head_in']),
00333 remapping={'tts_in':'cc_hri_tts',
00334 'head_in':'cc_hri_head'})
00335
00336 smach.Concurrence.add('CC_MOVE_BASE',
00337 smach_ros.SimpleActionState('MoveBase',
00338 MoveBaseAction,
00339 goal_cb=move_base_goal_cb,
00340 input_keys=['frame_id_in', 'goal_x_in',
00341 'goal_y_in', 'goal_theta_in']),
00342 remapping={'frame_id_in':'cc_frame_id',
00343 'goal_x_in':'cc_goal_x',
00344 'goal_y_in':'cc_goal_y',
00345 'goal_theta_in':'cc_goal_theta'})
00346
00347
00348 hri_rb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00349 default_outcome='aborted',
00350 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00351 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00352 child_termination_cb = lambda so: False,
00353 outcome_cb = hri_rb_out_cb)
00354 with hri_rb_cc:
00355 smach.Concurrence.add('CC_HRI',
00356 smach_ros.SimpleActionState('hri',
00357 sequenceAction,
00358 goal_cb=hri_goal_cb,
00359 input_keys=['tts_in','head_in']),
00360 remapping={'tts_in':'cc_hri_tts',
00361 'head_in':'cc_hri_head'})
00362
00363 smach.Concurrence.add('CC_ROTATE_BASE',
00364 smach_ros.SimpleActionState('MoveBase',
00365 MoveBaseAction,
00366 goal_cb=rotate_base_goal_cb,
00367 input_keys=['frame_id_in', 'goal_x_in',
00368 'goal_y_in', 'goal_theta_in']),
00369 remapping={'frame_id_in':'cc_frame_id',
00370 'goal_x_in':'cc_goal_x',
00371 'goal_y_in':'cc_goal_y',
00372 'goal_theta_in':'cc_goal_theta'})
00373
00374
00375
00376
00377 with sm:
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389 sm.userdata.sm_hri_tts_start = "Starting the guiding experiment."
00390 sm.userdata.sm_hri_head_start = ""
00391 smach.StateMachine.add('HRI_START',
00392 smach_ros.SimpleActionState('hri',
00393 sequenceAction,
00394 goal_cb=hri_goal_cb,
00395 input_keys=['tts_in', 'head_in']),
00396 transitions={'succeeded':'AC_PT_0'},
00397 remapping={'tts_in':'sm_hri_tts_start',
00398 'head_in':'sm_hri_head_start'})
00399
00400
00401
00402
00403 smach.StateMachine.add('AC_PT_0',
00404 smach_ros.SimpleActionState('people_tracking',
00405 peopleTrackAction,
00406 result_cb=pt_behind_result_cb,
00407 outcomes=['min_dist','people_front','no_people','people_far'],
00408 output_keys=['frame_id_pt_out','x_pt_out',
00409 'y_pt_out','id_pt_out','theta_pt_out']),
00410
00411 transitions={'min_dist':'AC_PT_0',
00412 'people_front':'HRI_PRESENTATION',
00413 'no_people':'AC_PT_0',
00414 'people_far':'AC_PT_0'},
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
00422 sm.userdata.sm_hri_tts_presentation = "Hi! I'm Tiby, let me guide you until our car"
00423 sm.userdata.sm_hri_head_presentation = ""
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436 smach.StateMachine.add('HRI_PRESENTATION',
00437 smach_ros.SimpleActionState('hri',
00438 sequenceAction,
00439 goal_cb=hri_goal_cb,
00440 input_keys=['tts_in', 'head_in']),
00441 transitions={'succeeded':'AC_PT_01'},
00442 remapping={'tts_in':'sm_hri_tts_presentation',
00443 'head_in':'sm_hri_head_presentation'})
00444
00445 smach.StateMachine.add('AC_PT_01',
00446 smach_ros.SimpleActionState('people_tracking',
00447 peopleTrackAction,
00448 result_cb=pt_result_cb,
00449 outcomes=['min_dist','max_dist','no_people','people_far'],
00450 output_keys=['frame_id_pt_out','x_pt_out',
00451 'y_pt_out','id_pt_out','theta_pt_out']),
00452
00453 transitions={'max_dist':'AC_PT_01',
00454 'min_dist':'GUIDE_1',
00455 'no_people':'AC_PT_01',
00456 'people_far':'AC_PT_01'},
00457 remapping={'frame_id_pt_out':'sm_frame_id',
00458 'x_pt_out':'sm_goal_x',
00459 'y_pt_out':'sm_goal_y',
00460 'id_pt_out':'sm_target_id',
00461 'theta_pt_out':'sm_goal_theta'})
00462
00463 sm.userdata.sm_frame_id_map = '/map'
00464 sm.userdata.sm_goal_x_map = -50.0
00465 sm.userdata.sm_goal_y_map = -3.0
00466 sm.userdata.sm_goal_theta_map = 3.14
00467
00468 smach.StateMachine.add('GUIDE_1',
00469 smach_ros.SimpleActionState('guiding',
00470 guideGoalAction,
00471 goal_cb=guide_goal_cb,
00472 result_cb=guide_result_cb,
00473 outcomes=['lost','far','done','slow_down'],
00474 input_keys=['frame_id_in', 'id_pt_in','goal_x_in','goal_y_in','goal_theta_in']),
00475 transitions={'lost':'HRI_LOST',
00476 'far':'HRI_FAR',
00477 'done':'HRI_DONE',
00478 'slow_down':'HRI_SLOW_DOWN'},
00479 remapping={'frame_id_in':'sm_frame_id_map',
00480 'id_pt_in' : 'sm_target_id',
00481 'goal_x_in' : 'sm_goal_x_map',
00482 'goal_y_in' : 'sm_goal_y_map',
00483 'goal_theta_in': 'sm_goal_theta_map' })
00484
00485
00486
00487 sm.userdata.sm_hri_tts_lost = "I lost the person"
00488 sm.userdata.sm_hri_head_lost = ""
00489 smach.StateMachine.add('HRI_LOST',
00490 smach_ros.SimpleActionState('hri',
00491 sequenceAction,
00492 goal_cb=hri_goal_cb,
00493 input_keys=['tts_in', 'head_in']),
00494 transitions={'succeeded':'AC_PT_BEHIND'},
00495 remapping={'tts_in':'sm_hri_tts_lost',
00496 'head_in':'sm_hri_head_lost'})
00497
00498
00499
00500
00501 sm.userdata.sm_hri_tts_far = "You are too far"
00502 sm.userdata.sm_hri_head_far = ""
00503 smach.StateMachine.add('HRI_FAR',
00504 hri_rb_cc,
00505 transitions={'succeeded':'AC_PT_FAR'},
00506 remapping={'cc_hri_tts':'sm_hri_tts_far',
00507 'cc_hri_head':'sm_hri_head_far',
00508 'cc_frame_id':'sm_frame_id',
00509 'cc_goal_x':'sm_goal_x',
00510 'cc_goal_y':'sm_goal_y',
00511 'cc_goal_theta':'sm_goal_theta'})
00512
00513
00514
00515
00516 sm.userdata.sm_hri_tts_done = "Here we are"
00517 sm.userdata.sm_hri_head_done = ""
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528 smach.StateMachine.add('HRI_DONE',
00529 smach_ros.SimpleActionState('hri',
00530 sequenceAction,
00531 goal_cb=hri_goal_cb,
00532 input_keys=['tts_in', 'head_in']),
00533 transitions={'succeeded':'HRI_FAREWELL'},
00534 remapping={'tts_in':'sm_hri_tts_done',
00535 'head_in':'sm_hri_head_done'})
00536 smach.StateMachine.add('HRI_FAREWELL',
00537 smach_ros.SimpleActionState('hri',
00538 sequenceAction,
00539 goal_cb=hri_goal_cb,
00540 input_keys=['tts_in', 'head_in']),
00541 transitions={'succeeded':'succeeded'},
00542 remapping={'tts_in':'sm_hri_tts_farewell',
00543 'head_in':'sm_hri_head_farewell'})
00544
00545 sm.userdata.sm_hri_tts_farewell = "Get in the car, please. See you later"
00546 sm.userdata.sm_hri_head_farewell = "head_farewell.xml"
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557 sm.userdata.sm_hri_tts_slow = "you are too far, come closer please"
00558 sm.userdata.sm_hri_head_slow = ""
00559 smach.StateMachine.add('HRI_SLOW_DOWN',
00560 smach_ros.SimpleActionState('hri',
00561 sequenceAction,
00562 goal_cb=hri_goal_cb,
00563 input_keys=['tts_in', 'head_in']),
00564 transitions={'succeeded':'AC_PT_SLOW'},
00565 remapping={'tts_in':'sm_hri_tts_slow',
00566 'head_in':'sm_hri_head_slow'})
00567
00568 smach.StateMachine.add('AC_PT_BEHIND',
00569 smach_ros.SimpleActionState('people_tracking',
00570 peopleTrackAction,
00571 result_cb=pt_behind_result_cb,
00572 outcomes=['min_dist','people_front','no_people','people_far'],
00573 output_keys=['frame_id_pt_out','x_pt_out',
00574 'y_pt_out','id_pt_out','theta_pt_out']),
00575
00576 transitions={'people_front':'ADD_ASK_COUNTER_LOST',
00577 'min_dist':'GUIDE_1',
00578 'no_people':'ADD_ASK_COUNTER_LOST',
00579 'people_far':'ADD_ASK_COUNTER_LOST'},
00580 remapping={'frame_id_pt_out':'sm_frame_id',
00581 'x_pt_out':'sm_goal_x',
00582 'y_pt_out':'sm_goal_y',
00583 'id_pt_out':'sm_target_id',
00584 'theta_pt_out':'sm_goal_theta'})
00585
00586 smach.StateMachine.add('ADD_ASK_COUNTER_LOST',
00587 Counting_iterations(),
00588 transitions={'abort':'HRI_ABORT',
00589 'wait':'AC_PT_BEHIND'},
00590 remapping={'counter_it':'sm_counter_iterations'})
00591
00592
00593 smach.StateMachine.add('AC_PT_SLOW',
00594 smach_ros.SimpleActionState('people_tracking',
00595 peopleTrackAction,
00596 result_cb=pt_behind2_result_cb,
00597 outcomes=['min_dist','people_front','no_people','people_far','people_war'],
00598 output_keys=['frame_id_pt_out','x_pt_out',
00599 'y_pt_out','id_pt_out','theta_pt_out']),
00600
00601 transitions={'people_front':'AC_PT_SLOW',
00602 'min_dist':'GUIDE_1',
00603 'no_people':'AC_PT_SLOW',
00604 'people_far':'GUIDE_1',
00605 'people_war':'GUIDE_2'},
00606 remapping={'frame_id_pt_out':'sm_frame_id',
00607 'x_pt_out':'sm_goal_x',
00608 'y_pt_out':'sm_goal_y',
00609 'id_pt_out':'sm_target_id',
00610 'theta_pt_out':'sm_goal_theta'})
00611
00612 smach.StateMachine.add('GUIDE_2',
00613 smach_ros.SimpleActionState('guiding',
00614 guideGoalAction,
00615 goal_cb=guide_goal_cb,
00616 result_cb=guide_result_cb,
00617 outcomes=['lost','far','done','slow_down'],
00618 input_keys=['frame_id_in', 'id_pt_in','goal_x_in','goal_y_in','goal_theta_in']),
00619 transitions={'lost':'HRI_LOST',
00620 'far':'HRI_FAR',
00621 'done':'HRI_DONE',
00622 'slow_down':'AC_PT_SLOW'},
00623 remapping={'frame_id_in':'sm_frame_id_map',
00624 'id_pt_in' : 'sm_target_id',
00625 'goal_x_in' : 'sm_goal_x_map',
00626 'goal_y_in' : 'sm_goal_y_map',
00627 'goal_theta_in': 'sm_goal_theta_map' })
00628
00629
00630
00631 sm.userdata.sm_hri_tts_abort = "I cannot see anyone, I'll start again "
00632 sm.userdata.sm_hri_head_abort = ""
00633 smach.StateMachine.add('HRI_ABORT',
00634 smach_ros.SimpleActionState('hri',
00635 sequenceAction,
00636 goal_cb=hri_goal_cb,
00637 input_keys=['tts_in', 'head_in']),
00638 transitions={'succeeded':'HRI_START'},
00639 remapping={'tts_in':'sm_hri_tts_abort',
00640 'head_in':'sm_hri_head_abort'})
00641
00642
00643 smach.StateMachine.add('AC_PT_FAR',
00644 smach_ros.SimpleActionState('people_tracking',
00645 peopleTrackAction,
00646 result_cb=pt_result_cb,
00647 outcomes=['min_dist','max_dist','no_people','people_far'],
00648 output_keys=['frame_id_pt_out','x_pt_out',
00649 'y_pt_out','id_pt_out','theta_pt_out']),
00650
00651 transitions={'max_dist':'COMPUTE_MOVE_BASE_APPROACH',
00652 'min_dist':'HRI_CONTINUE',
00653 'no_people':'AC_PT_FAR',
00654 'people_far':'COMPUTE_MOVE_BASE_APPROACH'},
00655 remapping={'frame_id_pt_out':'sm_frame_id',
00656 'x_pt_out':'sm_goal_x',
00657 'y_pt_out':'sm_goal_y',
00658 'id_pt_out':'sm_target_id',
00659 'theta_pt_out':'sm_goal_theta'})
00660
00661
00662 sm.userdata.sm_hri_tts_continue = "Ok, we can now continue "
00663 sm.userdata.sm_hri_head_continue = ""
00664 smach.StateMachine.add('HRI_CONTINUE',
00665 smach_ros.SimpleActionState('hri',
00666 sequenceAction,
00667 goal_cb=hri_goal_cb,
00668 input_keys=['tts_in', 'head_in']),
00669 transitions={'succeeded':'GUIDE_1'},
00670 remapping={'tts_in':'sm_hri_tts_continue',
00671 'head_in':'sm_hri_head_continue'})
00672
00673
00674
00675 smach.StateMachine.add('COMPUTE_MOVE_BASE_APPROACH',
00676 ComputeGoalToApproach(),
00677 transitions={'person_far':'MOVE_BASE_APPROACH_FAR',
00678 'person_close':'MOVE_BASE_APPROACH_CLOSE'},
00679 remapping={'goal_x_in':'sm_goal_x',
00680 'goal_y_in':'sm_goal_y',
00681 'counter_in':'sm_counter'})
00682
00683
00684 sm.userdata.sm_hri_tts_approach1 = "Please, follow me, you have to walk close to me."
00685 sm.userdata.sm_hri_head_approach1 = "head_random_2.xml"
00686 smach.StateMachine.add('MOVE_BASE_APPROACH_FAR',
00687 hri_mb_cc,
00688 transitions={'succeeded':'AC_PT_POST_GOTO',
00689 'aborted':'HRI_BLOCKED'},
00690 remapping={'cc_hri_tts':'sm_hri_tts_approach1',
00691 'cc_hri_head':'sm_hri_head_approach1',
00692 'cc_frame_id':'sm_frame_id',
00693 'cc_goal_x':'sm_goal_x',
00694 'cc_goal_y':'sm_goal_y',
00695 'cc_goal_theta':'sm_goal_theta'})
00696
00697 smach.StateMachine.add('MOVE_BASE_APPROACH_CLOSE',
00698 hri_rb_cc,
00699 transitions={'succeeded':'AC_PT_POST_GOTO',
00700 'aborted':'HRI_BLOCKED'},
00701 remapping={'cc_hri_tts':'sm_hri_tts_approach1',
00702 'cc_hri_head':'sm_hri_head_approach1',
00703 'cc_frame_id':'sm_frame_id',
00704 'cc_goal_x':'sm_goal_x',
00705 'cc_goal_y':'sm_goal_y',
00706 'cc_goal_theta':'sm_goal_theta'})
00707
00708
00709
00710 smach.StateMachine.add('AC_PT_POST_GOTO',
00711 smach_ros.SimpleActionState('people_tracking',
00712 peopleTrackAction,
00713 result_cb=pt_front_result_cb,
00714 outcomes=['people_back','people_far','no_people','people_close'],
00715 output_keys=['frame_id_pt_out','x_pt_out',
00716 'y_pt_out','id_pt_out','theta_pt_out']),
00717
00718 transitions={'people_back':'AC_PT_POST_GOTO',
00719 'people_close':'HRI_CONTINUE',
00720 'no_people':'AC_PT_POST_GOTO',
00721 'people_far':'COMPUTE_MOVE_BASE_APPROACH_2'},
00722 remapping={'frame_id_pt_out':'sm_frame_id',
00723 'x_pt_out':'sm_goal_x',
00724 'y_pt_out':'sm_goal_y',
00725 'id_pt_out':'sm_target_id',
00726 'theta_pt_out':'sm_goal_theta'})
00727
00728
00729
00730 sm.userdata.sm_hri_tts_approach2 = "If you dont come, I cannot continue."
00731 sm.userdata.sm_hri_head_approach2 = ""
00732 smach.StateMachine.add('MOVE_BASE_APPROACH_CLOSE_2',
00733 hri_rb_cc,
00734 transitions={'succeeded':'AC_PT_POST_GOTO2',
00735 'aborted':'HRI_BLOCKED'},
00736 remapping={'cc_hri_tts':'sm_hri_tts_approach2',
00737 'cc_hri_head':'sm_hri_head_approach2',
00738 'cc_frame_id':'sm_frame_id',
00739 'cc_goal_x':'sm_goal_x',
00740 'cc_goal_y':'sm_goal_y',
00741 'cc_goal_theta':'sm_goal_theta'})
00742
00743 smach.StateMachine.add('MOVE_BASE_APPROACH_FAR_2',
00744 hri_mb_cc,
00745 transitions={'succeeded':'AC_PT_POST_GOTO2',
00746 'aborted':'HRI_BLOCKED'},
00747 remapping={'cc_hri_tts':'sm_hri_tts_approach2',
00748 'cc_hri_head':'sm_hri_head_approach2',
00749 'cc_frame_id':'sm_frame_id',
00750 'cc_goal_x':'sm_goal_x',
00751 'cc_goal_y':'sm_goal_y',
00752 'cc_goal_theta':'sm_goal_theta'})
00753
00754 smach.StateMachine.add('AC_PT_POST_GOTO2',
00755 smach_ros.SimpleActionState('people_tracking',
00756 peopleTrackAction,
00757 result_cb=pt_front_result_cb,
00758 outcomes=['people_back','people_far','no_people','people_close'],
00759 output_keys=['frame_id_pt_out','x_pt_out',
00760 'y_pt_out','id_pt_out','theta_pt_out']),
00761
00762 transitions={'people_back':'AC_PT_POST_GOTO2',
00763 'people_close':'HRI_CONTINUE',
00764 'no_people':'AC_PT_POST_GOTO2',
00765 'people_far':'COMPUTE_MOVE_BASE_APPROACH'},
00766 remapping={'frame_id_pt_out':'sm_frame_id',
00767 'x_pt_out':'sm_goal_x',
00768 'y_pt_out':'sm_goal_y',
00769 'id_pt_out':'sm_target_id',
00770 'theta_pt_out':'sm_goal_theta'})
00771
00772 smach.StateMachine.add('COMPUTE_MOVE_BASE_APPROACH_2',
00773 ComputeGoalToApproach(),
00774 transitions={'person_far':'MOVE_BASE_APPROACH_FAR_2',
00775 'person_close':'MOVE_BASE_APPROACH_CLOSE_2'},
00776 remapping={'goal_x_in':'sm_goal_x',
00777 'goal_y_in':'sm_goal_y',
00778 'counter_in':'sm_counter'})
00779
00780
00781 sm.userdata.sm_hri_tts_abandone = "You are not collaboring, I'll look for someone else "
00782 sm.userdata.sm_hri_head_abandone = ""
00783 smach.StateMachine.add('HRI_ABANDONE',
00784 smach_ros.SimpleActionState('hri',
00785 sequenceAction,
00786 goal_cb=hri_goal_cb,
00787 input_keys=['tts_in', 'head_in']),
00788 transitions={'succeeded':'HRI_START'},
00789 remapping={'tts_in':'sm_hri_tts_abandone',
00790 'head_in':'sm_hri_head_abandone'})
00791
00792
00793
00794
00795
00796
00797 sm.userdata.sm_hri_tts_blocked = "Sergi, Fernando, Anaiis, I cannot move, please help me."
00798 sm.userdata.sm_hri_head_blocked = "head_home.xml"
00799 smach.StateMachine.add('HRI_BLOCKED',
00800 smach_ros.SimpleActionState('hri',
00801 sequenceAction,
00802 goal_cb=hri_goal_cb,
00803 input_keys=['tts_in', 'head_in']),
00804 transitions={'succeeded':'HRI_START'},
00805 remapping={'tts_in':'sm_hri_tts_blocked',
00806 'head_in':'sm_hri_head_blocked'})
00807
00808
00809
00810
00811
00812
00813 sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00814 sis.start()
00815
00816
00817 outcome = sm.execute()
00818
00819
00820 rospy.spin()
00821 sis.stop()
00822
00823
00824 if __name__ == '__main__':
00825 main()
00826