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=['x_goal','y_goal','counter_in'],
00088 output_keys=['x_goal','y_goal','counter_in'])
00089 def execute(self, userdata):
00090 rospy.loginfo('Executing state ComputeSubGoal')
00091 distance = userdata.x_goal*userdata.x_goal+userdata.y_goal*userdata.y_goal
00092 if userdata.counter_in > 5:
00093 userdata.counter_in = 0
00094 return 'person_left'
00095 else:
00096 if distance < 1.7*1.7:
00097 userdata.counter_in = 0
00098 return 'person_close'
00099 else:
00100 userdata.x_goal = userdata.x_goal*1.7/sqrt(distance)
00101 counter_in = counter_in+1
00102 return 'person_far'
00103
00104
00105
00106
00107 def main():
00108 rospy.init_node('concurrent_hri_move_base')
00109
00110
00111 sm = smach.StateMachine(['succeeded','aborted','preempted'])
00112
00113
00114
00115
00116
00117
00118
00119
00120 def hri_mb_out_cb(outcome_map):
00121 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_MOVE_BASE'] == 'succeeded':
00122 return 'succeeded'
00123 else:
00124 return 'aborted'
00125
00126
00127 def hri_rb_out_cb(outcome_map):
00128 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_ROTATE_BASE'] == 'succeeded':
00129 return 'succeeded'
00130 else:
00131 return 'succeeded'
00132
00133
00134 hri_mb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00135 default_outcome='aborted',
00136 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00137 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00138 child_termination_cb = lambda so: False,
00139 outcome_cb = hri_mb_out_cb)
00140 with hri_mb_cc:
00141 smach.Concurrence.add('CC_HRI',
00142 smach_ros.SimpleActionState('hri',
00143 sequenceAction,
00144 goal_cb=hri_goal_cb,
00145 input_keys=['tts_in','head_in']),
00146 remapping={'tts_in':'cc_hri_tts',
00147 'head_in':'cc_hri_head'})
00148
00149 smach.Concurrence.add('CC_MOVE_BASE',
00150 smach_ros.SimpleActionState('MoveBase',
00151 MoveBaseAction,
00152 goal_cb=move_base_goal_cb,
00153 input_keys=['frame_id_in', 'goal_x_in',
00154 'goal_y_in', 'goal_theta_in']),
00155 remapping={'frame_id_in':'cc_frame_id',
00156 'goal_x_in':'cc_goal_x',
00157 'goal_y_in':'cc_goal_y',
00158 'goal_theta_in':'cc_goal_theta'})
00159
00160
00161 hri_rb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00162 default_outcome='aborted',
00163 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00164 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00165 child_termination_cb = lambda so: False,
00166 outcome_cb = hri_rb_out_cb)
00167 with hri_rb_cc:
00168 smach.Concurrence.add('CC_HRI',
00169 smach_ros.SimpleActionState('hri',
00170 sequenceAction,
00171 goal_cb=hri_goal_cb,
00172 input_keys=['tts_in','head_in']),
00173 remapping={'tts_in':'cc_hri_tts',
00174 'head_in':'cc_hri_head'})
00175
00176 smach.Concurrence.add('CC_ROTATE_BASE',
00177 smach_ros.SimpleActionState('MoveBase',
00178 MoveBaseAction,
00179 goal_cb=rotate_base_goal_cb,
00180 input_keys=['frame_id_in', 'goal_x_in',
00181 'goal_y_in', 'goal_theta_in']),
00182 remapping={'frame_id_in':'cc_frame_id',
00183 'goal_x_in':'cc_goal_x',
00184 'goal_y_in':'cc_goal_y',
00185 'goal_theta_in':'cc_goal_theta'})
00186
00187
00188
00189
00190 with sm:
00191
00192 sm.userdata.sm_hri_tts_1 = "Hi, I'm looking someone. who can teach me, to learn something"
00193 sm.userdata.sm_hri_head_1 = "head_random_4.xml"
00194 smach.StateMachine.add('HRI_BEFORE_CC',
00195 smach_ros.SimpleActionState('hri',
00196 sequenceAction,
00197 goal_cb=hri_goal_cb,
00198 input_keys=['tts_in', 'head_in']),
00199 transitions={'succeeded':'AC_PT_1'},
00200 remapping={'tts_in':'sm_hri_tts_1',
00201 'head_in':'sm_hri_head_1'})
00202
00203
00204 smach.StateMachine.add('AC_PT_1',
00205 smach_ros.SimpleActionState('people_tracking',
00206 peopleTrackAction,
00207 result_cb=pt_result_cb,
00208 outcomes=['min_dist','max_dist','no_people','people_far'],
00209 output_keys=['frame_id_pt_out','x_pt_out',
00210 'y_pt_out','id_pt_out','theta_pt_out']),
00211 transitions={'max_dist':'CC_HRI_MB_1',
00212 'min_dist':'CC_HRI_RB_NEAR',
00213 'no_people':'HRI_FAIL',
00214 'people_far':'CC_HRI_RB_1'},
00215 remapping={'frame_id_pt_out':'sm_frame_id',
00216 'x_pt_out':'sm_goal_x',
00217 'y_pt_out':'sm_goal_y',
00218 'id_pt_out':'sm_target_id',
00219 'theta_pt_out':'sm_goal_theta'})
00220
00221
00222 sm.userdata.sm_hri_tts_2 = "There's nobody here, I'll try again'"
00223 sm.userdata.sm_hri_head_2 = "head_random_4.xml"
00224 smach.StateMachine.add('HRI_FAIL',
00225 smach_ros.SimpleActionState('hri',
00226 sequenceAction,
00227 goal_cb=hri_goal_cb,
00228 input_keys=['tts_in', 'head_in']),
00229 transitions={'succeeded':'HRI_BEFORE_CC'},
00230 remapping={'tts_in':'sm_hri_tts_2',
00231 'head_in':'sm_hri_head_2'})
00232
00233
00234
00235 sm.userdata.sm_hri_tts_approach_1 = "I find someone. Please wait for me."
00236 sm.userdata.sm_hri_head_approach_1 = "head_greeting.xml"
00237 smach.StateMachine.add('CC_HRI_MB_1',
00238 hri_mb_cc,
00239 transitions={'succeeded':'succeeded',
00240 'aborted':'HRI_BLOCKED'},
00241 remapping={'cc_hri_tts':'sm_hri_tts_approach_1',
00242 'cc_hri_head':'sm_hri_head_approach_1',
00243 'cc_frame_id':'sm_frame_id',
00244 'cc_goal_x':'sm_goal_x',
00245 'cc_goal_y':'sm_goal_y',
00246 'cc_goal_theta':'sm_goal_theta'})
00247
00248
00249 sm.userdata.sm_hri_tts_near = "Hey, how are you? Im Tibi. I'm trying to learn to detect faces."
00250 sm.userdata.sm_hri_head_near = "head_random_2.xml"
00251 smach.StateMachine.add('CC_HRI_RB_NEAR',
00252 hri_rb_cc,
00253 transitions={'succeeded':'AC_PT_2',
00254 'aborted':'HRI_BLOCKED'},
00255 remapping={'cc_hri_tts':'sm_hri_tts_near',
00256 'cc_hri_head':'sm_hri_head_near',
00257 'cc_frame_id':'sm_frame_id',
00258 'cc_goal_x':'sm_goal_x',
00259 'cc_goal_y':'sm_goal_y',
00260 'cc_goal_theta':'sm_goal_theta'})
00261
00262
00263 smach.StateMachine.add('AC_PT_2',
00264 smach_ros.SimpleActionState('people_tracking',
00265 peopleTrackAction,
00266 result_cb=pt_result_cb,
00267 outcomes=['min_dist','max_dist','no_people','people_far'],
00268 output_keys=['frame_id_pt_out','x_pt_out',
00269 'y_pt_out','id_pt_out','theta_pt_out']),
00270 transitions={'max_dist':'CC_HRI_ROBOT_APPROACHES_3',
00271 'min_dist':'CC_HRI_RB_MEET_1',
00272 'no_people':'HRI_FAIL',
00273 'people_far':'CC_HRI_RB_1'},
00274 remapping={'frame_id_pt_out':'sm_frame_id',
00275 'x_pt_out':'sm_goal_x',
00276 'y_pt_out':'sm_goal_y',
00277 'id_pt_out':'sm_target_id',
00278 'theta_pt_out':'sm_goal_theta'})
00279
00280 sm.userdata.sm_hri_tts_robot_approaches_3 = " Why are you moving? Please wait for me!"
00281 sm.userdata.sm_hri_head_robot_approaches_3 = ""
00282 smach.StateMachine.add('CC_HRI_ROBOT_APPROACHES_3',
00283 hri_rb_cc,
00284 transitions={'succeeded':'AC_PT_9',
00285 'aborted':'HRI_BLOCKED'},
00286 remapping={'cc_hri_tts':'sm_hri_tts_robot_approaches_3',
00287 'cc_hri_head':'sm_hri_head_robot_approaches_3',
00288 'cc_frame_id':'sm_frame_id',
00289 'cc_goal_x':'sm_goal_x',
00290 'cc_goal_y':'sm_goal_y',
00291 'cc_goal_theta':'sm_goal_theta'})
00292
00293
00294 sm.userdata.sm_hri_tts_meet1 = "I need you to stay in front of me, while I start the detection process."
00295 sm.userdata.sm_hri_head_meet1 = "head_random_2.xml"
00296 smach.StateMachine.add('CC_HRI_RB_MEET_1',
00297 hri_rb_cc,
00298 transitions={'succeeded':'AC_PT_3',
00299 'aborted':'HRI_BLOCKED'},
00300 remapping={'cc_hri_tts':'sm_hri_tts_meet1',
00301 'cc_hri_head':'sm_hri_head_meet1',
00302 'cc_frame_id':'sm_frame_id',
00303 'cc_goal_x':'sm_goal_x',
00304 'cc_goal_y':'sm_goal_y',
00305 'cc_goal_theta':'sm_goal_theta'})
00306
00307
00308 smach.StateMachine.add('AC_PT_3',
00309 smach_ros.SimpleActionState('people_tracking',
00310 peopleTrackAction,
00311 result_cb=pt_result_cb,
00312 outcomes=['min_dist','max_dist','no_people','people_far'],
00313 output_keys=['frame_id_pt_out','x_pt_out',
00314 'y_pt_out','id_pt_out','theta_pt_out']),
00315 transitions={'max_dist':'CC_HRI_ROBOT_APPROACHES_1',
00316 'min_dist':'CC_HRI_RB_MEET_2',
00317 'no_people':'HRI_FAIL',
00318 'people_far':'CC_HRI_RB_1'},
00319 remapping={'frame_id_pt_out':'sm_frame_id',
00320 'x_pt_out':'sm_goal_x',
00321 'y_pt_out':'sm_goal_y',
00322 'id_pt_out':'sm_target_id',
00323 'theta_pt_out':'sm_goal_theta'})
00324
00325
00326
00327 sm.userdata.sm_hri_tts_meet2 = "During the process, I'll ask you some questions if I have some doubts, I guess you can help me.'"
00328 sm.userdata.sm_hri_head_meet2 = "head_random_2.xml"
00329 smach.StateMachine.add('CC_HRI_RB_MEET_2',
00330 hri_rb_cc,
00331 transitions={'succeeded':'AC_PT_4',
00332 'aborted':'HRI_BLOCKED'},
00333 remapping={'cc_hri_tts':'sm_hri_tts_meet2',
00334 'cc_hri_head':'sm_hri_head_meet2',
00335 'cc_frame_id':'sm_frame_id',
00336 'cc_goal_x':'sm_goal_x',
00337 'cc_goal_y':'sm_goal_y',
00338 'cc_goal_theta':'sm_goal_theta'})
00339
00340
00341 smach.StateMachine.add('AC_PT_4',
00342 smach_ros.SimpleActionState('people_tracking',
00343 peopleTrackAction,
00344 result_cb=pt_result_cb,
00345 outcomes=['min_dist','max_dist','no_people','people_far'],
00346 output_keys=['frame_id_pt_out','x_pt_out',
00347 'y_pt_out','id_pt_out','theta_pt_out']),
00348 transitions={'max_dist':'CC_HRI_RB_START_PRE_EXP',
00349 'min_dist':'CC_HRI_RB_START_EXP',
00350 'no_people':'HRI_FAIL',
00351 'people_far':'CC_HRI_RB_1'},
00352 remapping={'frame_id_pt_out':'sm_frame_id',
00353 'x_pt_out':'sm_goal_x',
00354 'y_pt_out':'sm_goal_y',
00355 'id_pt_out':'sm_target_id',
00356 'theta_pt_out':'sm_goal_theta'})
00357
00358 sm.userdata.sm_hri_tts_start_exp = "Thanks for your patience. Let's start the demonstration."
00359 sm.userdata.sm_hri_head_start_exp = ""
00360 smach.StateMachine.add('CC_HRI_RB_START_EXP',
00361 hri_rb_cc,
00362 transitions={'succeeded':'succeeded',
00363 'aborted':'HRI_BLOCKED'},
00364 remapping={'cc_hri_tts':'sm_hri_tts_start_exp',
00365 'cc_hri_head':'sm_hri_head_start_exp',
00366 'cc_frame_id':'sm_frame_id',
00367 'cc_goal_x':'sm_goal_x',
00368 'cc_goal_y':'sm_goal_y',
00369 'cc_goal_theta':'sm_goal_theta'})
00370
00371 sm.userdata.sm_hri_tts_pre_start_exp = "Please, don't go. It will be just two minuts."
00372 sm.userdata.sm_hri_head_pre_start_exp = ""
00373 smach.StateMachine.add('CC_HRI_RB_START_PRE_EXP',
00374 hri_rb_cc,
00375 transitions={'succeeded':'AC_PT_5',
00376 'aborted':'HRI_BLOCKED'},
00377 remapping={'cc_hri_tts':'sm_hri_tts_pre_start_exp',
00378 'cc_hri_head':'sm_hri_head_pre_start_exp',
00379 'cc_frame_id':'sm_frame_id',
00380 'cc_goal_x':'sm_goal_x',
00381 'cc_goal_y':'sm_goal_y',
00382 'cc_goal_theta':'sm_goal_theta'})
00383
00384
00385
00386 smach.StateMachine.add('AC_PT_5',
00387 smach_ros.SimpleActionState('people_tracking',
00388 peopleTrackAction,
00389 result_cb=pt_result_cb,
00390 outcomes=['min_dist','max_dist','no_people','people_far'],
00391 output_keys=['frame_id_pt_out','x_pt_out',
00392 'y_pt_out','id_pt_out','theta_pt_out']),
00393 transitions={'max_dist':'COMPUTE_SUB_GOAL_1',
00394 'min_dist':'CC_HRI_RB_START_EXP',
00395 'no_people':'HRI_FAIL',
00396 'people_far':'CC_HRI_RB_1'},
00397 remapping={'frame_id_pt_out':'sm_frame_id',
00398 'x_pt_out':'sm_goal_x',
00399 'y_pt_out':'sm_goal_y',
00400 'id_pt_out':'sm_target_id',
00401 'theta_pt_out':'sm_goal_theta'})
00402
00403 smach.StateMachine.add('COMPUTE_SUB_GOAL_1', ComputeSubGoal(),
00404 transitions={'person_far':'MOVE_BASE_1',
00405 'person_close':'CC_HRI_RB_START_EXP',
00406 'person_left':'FAREWELL'},
00407 remapping={'x_goal':'sm_goal_x',
00408 'y_goal':'sm_goal_y',
00409 'counter_in':'sm_counter'})
00410
00411 smach.StateMachine.add('MOVE_BASE_1',
00412 smach_ros.SimpleActionState('MoveBase',
00413 MoveBaseAction,
00414 goal_cb=move_base_goal_cb,
00415 input_keys=['frame_id_in', 'goal_x_in',
00416 'goal_y_in', 'goal_theta_in']),
00417 transitions={'succeeded':'AC_PT_5'},
00418 remapping={'frame_id_in':'sm_frame_id',
00419 'goal_x_in':'sm_x_goal',
00420 'goal_y_in':'sm_y_goal',
00421 'goal_theta_in':'sm_theta_goal'})
00422
00423
00424
00425
00426 sm.userdata.sm_hri_tts_robot_approaches_1 = " I'm talking to you, if you start moving I cannot concentrate. Please, I just need your attention 2 minuts."
00427 sm.userdata.sm_hri_head_robot_approaches_1 = ""
00428 smach.StateMachine.add('CC_HRI_ROBOT_APPROACHES_1',
00429 hri_rb_cc,
00430 transitions={'succeeded':'AC_PT_6',
00431 'aborted':'HRI_BLOCKED'},
00432 remapping={'cc_hri_tts':'sm_hri_tts_robot_approaches_1',
00433 'cc_hri_head':'sm_hri_head_robot_approaches_1',
00434 'cc_frame_id':'sm_frame_id',
00435 'cc_goal_x':'sm_goal_x',
00436 'cc_goal_y':'sm_goal_y',
00437 'cc_goal_theta':'sm_goal_theta'})
00438
00439
00440
00441
00442 smach.StateMachine.add('AC_PT_6',
00443 smach_ros.SimpleActionState('people_tracking',
00444 peopleTrackAction,
00445 result_cb=pt_result_cb,
00446 outcomes=['min_dist','max_dist','no_people','people_far'],
00447 output_keys=['frame_id_pt_out','x_pt_out',
00448 'y_pt_out','id_pt_out','theta_pt_out']),
00449 transitions={'max_dist':'COMPUTE_SUB_GOAL_2',
00450 'min_dist':'CC_HRI_RB_MEET_1_1',
00451 'no_people':'HRI_FAIL',
00452 'people_far':'CC_HRI_RB_1'},
00453 remapping={'frame_id_pt_out':'sm_frame_id',
00454 'x_pt_out':'sm_goal_x',
00455 'y_pt_out':'sm_goal_y',
00456 'id_pt_out':'sm_target_id',
00457 'theta_pt_out':'sm_goal_theta'})
00458
00459 smach.StateMachine.add('COMPUTE_SUB_GOAL_2', ComputeSubGoal(),
00460 transitions={'person_far':'MOVE_BASE_2',
00461 'person_close':'CC_HRI_RB_MEET_1_1',
00462 'person_left':'FAREWELL'},
00463 remapping={'x_goal':'sm_goal_x',
00464 'y_goal':'sm_goal_y',
00465 'counter_in':'sm_counter'})
00466
00467 smach.StateMachine.add('MOVE_BASE_2',
00468 smach_ros.SimpleActionState('MoveBase',
00469 MoveBaseAction,
00470 goal_cb=move_base_goal_cb,
00471 input_keys=['frame_id_in', 'goal_x_in',
00472 'goal_y_in', 'goal_theta_in']),
00473 transitions={'succeeded':'AC_PT_6'},
00474 remapping={'frame_id_in':'sm_frame_id',
00475 'goal_x_in':'sm_x_goal',
00476 'goal_y_in':'sm_y_goal',
00477 'goal_theta_in':'sm_theta_goal'})
00478
00479
00480 sm.userdata.sm_hri_tts_meet_1_1 = "As I was explaining to you, before you moved. I need you to stay in front of me."
00481 sm.userdata.sm_hri_head_meet_1_1 = "head_random_2.xml"
00482 smach.StateMachine.add('CC_HRI_RB_MEET_1_1',
00483 hri_rb_cc,
00484 transitions={'succeeded':'AC_PT_7',
00485 'aborted':'HRI_BLOCKED'},
00486 remapping={'cc_hri_tts':'sm_hri_tts_meet_1_',
00487 'cc_hri_head':'sm_hri_head_meet_1_1',
00488 'cc_frame_id':'sm_frame_id',
00489 'cc_goal_x':'sm_goal_x',
00490 'cc_goal_y':'sm_goal_y',
00491 'cc_goal_theta':'sm_goal_theta'})
00492
00493
00494
00495
00496 smach.StateMachine.add('AC_PT_7',
00497 smach_ros.SimpleActionState('people_tracking',
00498 peopleTrackAction,
00499 result_cb=pt_result_cb,
00500 outcomes=['min_dist','max_dist','no_people','people_far'],
00501 output_keys=['frame_id_pt_out','x_pt_out',
00502 'y_pt_out','id_pt_out','theta_pt_out']),
00503 transitions={'max_dist':'CC_HRI_ROBOT_APPROACHES_2',
00504 'min_dist':'CC_HRI_RB_MEET_2',
00505 'no_people':'HRI_FAIL',
00506 'people_far':'CC_HRI_RB_1'},
00507 remapping={'frame_id_pt_out':'sm_frame_id',
00508 'x_pt_out':'sm_goal_x',
00509 'y_pt_out':'sm_goal_y',
00510 'id_pt_out':'sm_target_id',
00511 'theta_pt_out':'sm_goal_theta'})
00512
00513 sm.userdata.sm_hri_tts_robot_approaches_2 = "Again? are you nervous? please, wait until I finish, it's just a moment"
00514 sm.userdata.sm_hri_head_robot_approaches_2 = ""
00515 smach.StateMachine.add('CC_HRI_ROBOT_APPROACHES_2',
00516 hri_rb_cc,
00517 transitions={'succeeded':'AC_PT_8',
00518 'aborted':'HRI_BLOCKED'},
00519 remapping={'cc_hri_tts':'sm_hri_tts_robot_approaches_2',
00520 'cc_hri_head':'sm_hri_head_robot_approaches_2',
00521 'cc_frame_id':'sm_frame_id',
00522 'cc_goal_x':'sm_goal_x',
00523 'cc_goal_y':'sm_goal_y',
00524 'cc_goal_theta':'sm_goal_theta'})
00525
00526
00527
00528 smach.StateMachine.add('AC_PT_8',
00529 smach_ros.SimpleActionState('people_tracking',
00530 peopleTrackAction,
00531 result_cb=pt_result_cb,
00532 outcomes=['min_dist','max_dist','no_people','people_far'],
00533 output_keys=['frame_id_pt_out','x_pt_out',
00534 'y_pt_out','id_pt_out','theta_pt_out']),
00535 transitions={'max_dist':'COMPUTE_SUB_GOAL_3',
00536 'min_dist':'CC_HRI_RB_MEET_2',
00537 'no_people':'HRI_FAIL',
00538 'people_far':'CC_HRI_RB_1'},
00539 remapping={'frame_id_pt_out':'sm_frame_id',
00540 'x_pt_out':'sm_goal_x',
00541 'y_pt_out':'sm_goal_y',
00542 'id_pt_out':'sm_target_id',
00543 'theta_pt_out':'sm_goal_theta'})
00544
00545 smach.StateMachine.add('COMPUTE_SUB_GOAL_3', ComputeSubGoal(),
00546 transitions={'person_far':'MOVE_BASE_3',
00547 'person_close':'CC_HRI_RB_MEET_2',
00548 'person_left':'FAREWELL'},
00549 remapping={'x_goal':'sm_goal_x',
00550 'y_goal':'sm_goal_y',
00551 'counter_in':'sm_counter'})
00552
00553 smach.StateMachine.add('MOVE_BASE_3',
00554 smach_ros.SimpleActionState('MoveBase',
00555 MoveBaseAction,
00556 goal_cb=move_base_goal_cb,
00557 input_keys=['frame_id_in', 'goal_x_in',
00558 'goal_y_in', 'goal_theta_in']),
00559 transitions={'succeeded':'AC_PT_8'},
00560 remapping={'frame_id_in':'sm_frame_id',
00561 'goal_x_in':'sm_x_goal',
00562 'goal_y_in':'sm_y_goal',
00563 'goal_theta_in':'sm_theta_goal'})
00564
00565 smach.StateMachine.add('AC_PT_9',
00566 smach_ros.SimpleActionState('people_tracking',
00567 peopleTrackAction,
00568 result_cb=pt_result_cb,
00569 outcomes=['min_dist','max_dist','no_people','people_far'],
00570 output_keys=['frame_id_pt_out','x_pt_out',
00571 'y_pt_out','id_pt_out','theta_pt_out']),
00572 transitions={'max_dist':'COMPUTE_SUB_GOAL_4',
00573 'min_dist':'CC_HRI_RB_MEET_1',
00574 'no_people':'HRI_FAIL',
00575 'people_far':'CC_HRI_RB_1'},
00576 remapping={'frame_id_pt_out':'sm_frame_id',
00577 'x_pt_out':'sm_goal_x',
00578 'y_pt_out':'sm_goal_y',
00579 'id_pt_out':'sm_target_id',
00580 'theta_pt_out':'sm_goal_theta'})
00581
00582 smach.StateMachine.add('COMPUTE_SUB_GOAL_4', ComputeSubGoal(),
00583 transitions={'person_far':'MOVE_BASE_4',
00584 'person_close':'CC_HRI_RB_MEET_1',
00585 'person_left':'FAREWELL'},
00586 remapping={'x_goal':'sm_goal_x',
00587 'y_goal':'sm_goal_y',
00588 'counter_in':'sm_counter'})
00589
00590 smach.StateMachine.add('MOVE_BASE_4',
00591 smach_ros.SimpleActionState('MoveBase',
00592 MoveBaseAction,
00593 goal_cb=move_base_goal_cb,
00594 input_keys=['frame_id_in', 'goal_x_in',
00595 'goal_y_in', 'goal_theta_in']),
00596 transitions={'succeeded':'AC_PT_9'},
00597 remapping={'frame_id_in':'sm_frame_id',
00598 'goal_x_in':'sm_x_goal',
00599 'goal_y_in':'sm_y_goal',
00600 'goal_theta_in':'sm_theta_goal'})
00601
00602
00603
00604
00605
00606
00607
00608 sm.userdata.sm_hri_tts_farewell = "Ok, I'm not stupid, I see you don't want to talk to me. Bye."
00609 sm.userdata.sm_hri_head_farewell = "head_random_2.xml"
00610 smach.StateMachine.add('FAREWELL',
00611 hri_rb_cc,
00612 transitions={'succeeded':'AC_PT_3',
00613 'aborted':'HRI_BLOCKED'},
00614 remapping={'cc_hri_tts':'sm_hri_tts_farewell',
00615 'cc_hri_head':'sm_hri_head_farewell',
00616 'cc_frame_id':'sm_frame_id',
00617 'cc_goal_x':'sm_goal_x',
00618 'cc_goal_y':'sm_goal_y',
00619 'cc_goal_theta':'sm_goal_theta'})
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635 sm.userdata.sm_hri_tts_people_far = "you are so far, I'll look for another person."
00636 sm.userdata.sm_hri_head_people_far = "head_random_2.xml"
00637 smach.StateMachine.add('CC_HRI_RB_1',
00638 hri_rb_cc,
00639 transitions={'succeeded':'HRI_BEFORE_CC',
00640 'aborted':'HRI_BLOCKED'},
00641 remapping={'cc_hri_tts':'sm_hri_tts_people_far',
00642 'cc_hri_head':'sm_hri_head_people_far',
00643 'cc_frame_id':'sm_frame_id',
00644 'cc_goal_x':'sm_goal_x',
00645 'cc_goal_y':'sm_goal_y',
00646 'cc_goal_theta':'sm_goal_theta'})
00647
00648
00649
00650 sm.userdata.sm_hri_tts_blocked = "Sergi, Fernando, Anaiis, I cannot move, please help me."
00651 sm.userdata.sm_hri_head_blocked = "head_farewell.xml"
00652 smach.StateMachine.add('HRI_BLOCKED',
00653 smach_ros.SimpleActionState('hri',
00654 sequenceAction,
00655 goal_cb=hri_goal_cb,
00656 input_keys=['tts_in', 'head_in']),
00657 transitions={'succeeded':'HRI_BEFORE_CC'},
00658 remapping={'tts_in':'sm_hri_tts_blocked',
00659 'head_in':'sm_hri_head_blocked'})
00660
00661
00662
00663
00664
00665 sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00666 sis.start()
00667
00668
00669 outcome = sm.execute()
00670
00671
00672 rospy.spin()
00673 sis.stop()
00674
00675
00676 if __name__ == '__main__':
00677 main()
00678