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 rotate_base_goal_cb(userdata,goal):
00033 rospy.loginfo('rotate_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/10, userdata.goal_y_in/10, 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 pt_result_cb(userdata, state,result):
00044 rospy.loginfo('result!! num_persons=%d',len(result.ps.peopleSet))
00045 if len(result.ps.peopleSet)<1:
00046 return 'no_people'
00047 else:
00048 i = 0
00049 distance = 100
00050 index = -1
00051 while (i<len(result.ps.peopleSet)):
00052 userdata.frame_id_pt_out = result.ps.header.frame_id
00053 distance_aux = result.ps.peopleSet[i].x*result.ps.peopleSet[i].x+result.ps.peopleSet[i].y*result.ps.peopleSet[i].y
00054 if distance_aux < distance:
00055 distance = distance_aux
00056 userdata.x_pt_out = result.ps.peopleSet[i].x
00057 userdata.y_pt_out = result.ps.peopleSet[i].y
00058 userdata.id_pt_out = result.ps.peopleSet[i].targetId
00059 userdata.theta_pt_out = 0
00060 index = i
00061 i = i+1
00062 else:
00063 i = i+1
00064 if distance > 36:
00065 return 'people_far'
00066 else:
00067 if distance > 1.7*1.7:
00068 return 'max_dist'
00069 else:
00070 return 'min_dist'
00071
00072
00073
00074 def main():
00075 rospy.init_node('concurrent_hri_move_base')
00076
00077
00078 sm = smach.StateMachine(['succeeded','aborted','preempted'])
00079
00080
00081
00082 sm.userdata.sm_goal_x_2 = 2.0
00083 sm.userdata.sm_goal_y_2 = 0.0
00084 sm.userdata.sm_goal_theta_2 = 0.0
00085 sm.userdata.sm_counter = 0
00086
00087
00088
00089 def hri_rb_out_cb(outcome_map):
00090 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_ROTATE_BASE'] == 'succeeded':
00091 return 'succeeded'
00092 else:
00093 return 'aborted'
00094
00095
00096 hri_rb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00097 default_outcome='aborted',
00098 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00099 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00100 child_termination_cb = lambda so: False,
00101 outcome_cb = hri_rb_out_cb)
00102 with hri_rb_cc:
00103 smach.Concurrence.add('CC_HRI',
00104 smach_ros.SimpleActionState('hri',
00105 sequenceAction,
00106 goal_cb=hri_goal_cb,
00107 input_keys=['tts_in','head_in']),
00108 remapping={'tts_in':'cc_hri_tts',
00109 'head_in':'cc_hri_head'})
00110
00111 smach.Concurrence.add('CC_ROTATE_BASE',
00112 smach_ros.SimpleActionState('MoveBase',
00113 MoveBaseAction,
00114 goal_cb=rotate_base_goal_cb,
00115 input_keys=['frame_id_in', 'goal_x_in',
00116 'goal_y_in', 'goal_theta_in']),
00117 remapping={'frame_id_in':'cc_frame_id',
00118 'goal_x_in':'cc_goal_x',
00119 'goal_y_in':'cc_goal_y',
00120 'goal_theta_in':'cc_goal_theta'})
00121
00122
00123
00124
00125 with sm:
00126
00127 sm.userdata.sm_hri_tts_1 ="Hola! estic buscant algu' que em pugui ensenyar a recone`ixer cares."
00128 sm.userdata.sm_hri_head_1 = "head_random_1.xml"
00129 smach.StateMachine.add('HRI_BEFORE_CC',
00130 smach_ros.SimpleActionState('hri',
00131 sequenceAction,
00132 goal_cb=hri_goal_cb,
00133 input_keys=['tts_in', 'head_in']),
00134 transitions={'succeeded':'AC_PT_1'},
00135 remapping={'tts_in':'sm_hri_tts_1',
00136 'head_in':'sm_hri_head_1'})
00137
00138
00139 smach.StateMachine.add('AC_PT_1',
00140 smach_ros.SimpleActionState('people_tracking',
00141 peopleTrackAction,
00142 result_cb=pt_result_cb,
00143 outcomes=['min_dist','max_dist','no_people','people_far'],
00144 output_keys=['frame_id_pt_out','x_pt_out',
00145 'y_pt_out','id_pt_out','theta_pt_out']),
00146 transitions={'max_dist':'RHRI18',
00147 'min_dist':'RHRI2',
00148 'no_people':'HRI_FAIL',
00149 'people_far':'CC_HRI_RB_1'},
00150 remapping={'frame_id_pt_out':'sm_frame_id',
00151 'x_pt_out':'sm_goal_x',
00152 'y_pt_out':'sm_goal_y',
00153 'id_pt_out':'sm_target_id',
00154 'theta_pt_out':'sm_goal_theta'})
00155
00156
00157 sm.userdata.sm_hri_tts_2 = "No veig a ningu' , ho tornare' a intentar"
00158 sm.userdata.sm_hri_head_2 = "head_random_4.xml"
00159 smach.StateMachine.add('HRI_FAIL',
00160 smach_ros.SimpleActionState('hri',
00161 sequenceAction,
00162 goal_cb=hri_goal_cb,
00163 input_keys=['tts_in', 'head_in']),
00164 transitions={'succeeded':'HRI_BEFORE_CC'},
00165 remapping={'tts_in':'sm_hri_tts_2',
00166 'head_in':'sm_hri_head_2'})
00167
00168
00169
00170
00171
00172 sm.userdata.sm_hri_tts_2 = "Hola, com esta's ? estic intentant detectar cares"
00173 sm.userdata.sm_hri_head_2 = "head_random_2.xml"
00174 smach.StateMachine.add('RHRI2',
00175 hri_rb_cc,
00176 transitions={'succeeded':'AC_PT_2',
00177 'aborted':'HRI_BLOCKED'},
00178 remapping={'cc_hri_tts':'sm_hri_tts_2',
00179 'cc_hri_head':'sm_hri_head_2',
00180 'cc_frame_id':'sm_frame_id',
00181 'cc_goal_x':'sm_goal_x',
00182 'cc_goal_y':'sm_goal_y',
00183 'cc_goal_theta':'sm_goal_theta'})
00184
00185
00186 sm.userdata.sm_hri_tts_people_far = "Esta's molt lluny. buscare' alg'u altre."
00187 sm.userdata.sm_hri_head_people_far = "head_random_2.xml"
00188 smach.StateMachine.add('CC_HRI_RB_1',
00189 hri_rb_cc,
00190 transitions={'succeeded':'HRI_BEFORE_CC',
00191 'aborted':'HRI_BLOCKED'},
00192 remapping={'cc_hri_tts':'sm_hri_tts_people_far',
00193 'cc_hri_head':'sm_hri_head_people_far',
00194 'cc_frame_id':'sm_frame_id',
00195 'cc_goal_x':'sm_goal_x_2',
00196 'cc_goal_y':'sm_goal_y_2',
00197 'cc_goal_theta':'sm_goal_theta_2'})
00198
00199
00200 sm.userdata.sm_hri_tts_final = "I see you do not want to talk to me. Good bye."
00201 sm.userdata.sm_hri_head_final = "head_farewell.xml"
00202 smach.StateMachine.add('HRI_FINAL',
00203 smach_ros.SimpleActionState('hri',
00204 sequenceAction,
00205 goal_cb=hri_goal_cb,
00206 input_keys=['tts_in', 'head_in']),
00207 transitions={'succeeded':'CC_HRI_RB_2'},
00208 remapping={'tts_in':'sm_hri_tts_final',
00209 'head_in':'sm_hri_head_final'})
00210
00211
00212 sm.userdata.sm_hri_tts_blocked = "Sergi, Fernando, Anaiis, I cannot move, please help me."
00213 sm.userdata.sm_hri_head_blocked = "head_farewell.xml"
00214 smach.StateMachine.add('HRI_BLOCKED',
00215 smach_ros.SimpleActionState('hri',
00216 sequenceAction,
00217 goal_cb=hri_goal_cb,
00218 input_keys=['tts_in', 'head_in']),
00219 transitions={'succeeded':'HRI_BEFORE_CC'},
00220 remapping={'tts_in':'sm_hri_tts_blocked',
00221 'head_in':'sm_hri_head_blocked'})
00222
00223
00224
00225 smach.StateMachine.add('AC_PT_2',
00226 smach_ros.SimpleActionState('people_tracking',
00227 peopleTrackAction,
00228 result_cb=pt_result_cb,
00229 outcomes=['min_dist','max_dist','no_people','people_far'],
00230 output_keys=['frame_id_pt_out','x_pt_out',
00231 'y_pt_out','id_pt_out','theta_pt_out']),
00232 transitions={'max_dist':'RHRI15',
00233 'min_dist':'RHRI3',
00234 'no_people':'HRI_FAIL',
00235 'people_far':'CC_HRI_RB_1'},
00236 remapping={'frame_id_pt_out':'sm_frame_id',
00237 'x_pt_out':'sm_goal_x',
00238 'y_pt_out':'sm_goal_y',
00239 'id_pt_out':'sm_target_id',
00240 'theta_pt_out':'sm_goal_theta'})
00241
00242
00243 sm.userdata.sm_hri_tts_3 = "Soc la Tibi, em pots ajudar a detectar cares?"
00244 sm.userdata.sm_hri_head_3 = "head_random_2.xml"
00245 smach.StateMachine.add('RHRI3',
00246 hri_rb_cc,
00247 transitions={'succeeded':'AC_PT_3',
00248 'aborted':'HRI_BLOCKED'},
00249 remapping={'cc_hri_tts':'sm_hri_tts_3',
00250 'cc_hri_head':'sm_hri_head_3',
00251 'cc_frame_id':'sm_frame_id',
00252 'cc_goal_x':'sm_goal_x',
00253 'cc_goal_y':'sm_goal_y',
00254 'cc_goal_theta':'sm_goal_theta'})
00255
00256
00257
00258 smach.StateMachine.add('AC_PT_3',
00259 smach_ros.SimpleActionState('people_tracking',
00260 peopleTrackAction,
00261 result_cb=pt_result_cb,
00262 outcomes=['min_dist','max_dist','no_people','people_far'],
00263 output_keys=['frame_id_pt_out','x_pt_out',
00264 'y_pt_out','id_pt_out','theta_pt_out']),
00265 transitions={'max_dist':'RHRI13',
00266 'min_dist':'RHRI4',
00267 'no_people':'HRI_FAIL',
00268 'people_far':'CC_HRI_RB_1'},
00269 remapping={'frame_id_pt_out':'sm_frame_id',
00270 'x_pt_out':'sm_goal_x',
00271 'y_pt_out':'sm_goal_y',
00272 'id_pt_out':'sm_target_id',
00273 'theta_pt_out':'sm_goal_theta'})
00274
00275
00276
00277 sm.userdata.sm_hri_tts_4 = "Nome's t'has de colocar davant meu, i dirme si ho faig b . utilitzant els mandos de la wi"
00278 sm.userdata.sm_hri_head_4 = "head_random_2.xml"
00279 smach.StateMachine.add('RHRI4',
00280 hri_rb_cc,
00281 transitions={'succeeded':'AC_PT_4',
00282 'aborted':'HRI_BLOCKED'},
00283 remapping={'cc_hri_tts':'sm_hri_tts_4',
00284 'cc_hri_head':'sm_hri_head_4',
00285 'cc_frame_id':'sm_frame_id',
00286 'cc_goal_x':'sm_goal_x',
00287 'cc_goal_y':'sm_goal_y',
00288 'cc_goal_theta':'sm_goal_theta'})
00289
00290
00291
00292 smach.StateMachine.add('AC_PT_4',
00293 smach_ros.SimpleActionState('people_tracking',
00294 peopleTrackAction,
00295 result_cb=pt_result_cb,
00296 outcomes=['min_dist','max_dist','no_people','people_far'],
00297 output_keys=['frame_id_pt_out','x_pt_out',
00298 'y_pt_out','id_pt_out','theta_pt_out']),
00299 transitions={'max_dist':'RHRI10',
00300 'min_dist':'RHRI5',
00301 'no_people':'HRI_FAIL',
00302 'people_far':'CC_HRI_RB_1'},
00303 remapping={'frame_id_pt_out':'sm_frame_id',
00304 'x_pt_out':'sm_goal_x',
00305 'y_pt_out':'sm_goal_y',
00306 'id_pt_out':'sm_target_id',
00307 'theta_pt_out':'sm_goal_theta'})
00308
00309
00310 sm.userdata.sm_hri_tts_5 = "demana-li el mando a aquests nois tan macos que hi ha per aqui'"
00311 sm.userdata.sm_hri_head_5 = ""
00312 smach.StateMachine.add('RHRI5',
00313 hri_rb_cc,
00314 transitions={'succeeded':'AC_PT_5',
00315 'aborted':'HRI_BLOCKED'},
00316 remapping={'cc_hri_tts':'sm_hri_tts_5',
00317 'cc_hri_head':'sm_hri_head_5',
00318 'cc_frame_id':'sm_frame_id',
00319 'cc_goal_x':'sm_goal_x',
00320 'cc_goal_y':'sm_goal_y',
00321 'cc_goal_theta':'sm_goal_theta'})
00322
00323
00324 smach.StateMachine.add('AC_PT_5',
00325 smach_ros.SimpleActionState('people_tracking',
00326 peopleTrackAction,
00327 result_cb=pt_result_cb,
00328 outcomes=['min_dist','max_dist','no_people','people_far'],
00329 output_keys=['frame_id_pt_out','x_pt_out',
00330 'y_pt_out','id_pt_out','theta_pt_out']),
00331 transitions={'max_dist':'RHRI7',
00332 'min_dist':'RHRI6',
00333 'no_people':'HRI_FAIL',
00334 'people_far':'CC_HRI_RB_1'},
00335 remapping={'frame_id_pt_out':'sm_frame_id',
00336 'x_pt_out':'sm_goal_x',
00337 'y_pt_out':'sm_goal_y',
00338 'id_pt_out':'sm_target_id',
00339 'theta_pt_out':'sm_goal_theta'})
00340
00341 sm.userdata.sm_hri_tts_6 = "Ara ja podem comensar"
00342 sm.userdata.sm_hri_head_6 = ""
00343 smach.StateMachine.add('RHRI6',
00344 hri_rb_cc,
00345 transitions={'succeeded':'succeeded',
00346 'aborted':'HRI_BLOCKED'},
00347 remapping={'cc_hri_tts':'sm_hri_tts_6',
00348 'cc_hri_head':'sm_hri_head_6',
00349 'cc_frame_id':'sm_frame_id',
00350 'cc_goal_x':'sm_goal_x',
00351 'cc_goal_y':'sm_goal_y',
00352 'cc_goal_theta':'sm_goal_theta'})
00353
00354
00355 sm.userdata.sm_hri_tts_7 = "Ei! pro no marxis, nome's son dos minutets"
00356 sm.userdata.sm_hri_head_7 = ""
00357 smach.StateMachine.add('RHRI7',
00358 hri_rb_cc,
00359 transitions={'succeeded':'AC_PT_7',
00360 'aborted':'HRI_BLOCKED'},
00361 remapping={'cc_hri_tts':'sm_hri_tts_7',
00362 'cc_hri_head':'sm_hri_head_7',
00363 'cc_frame_id':'sm_frame_id',
00364 'cc_goal_x':'sm_goal_x',
00365 'cc_goal_y':'sm_goal_y',
00366 'cc_goal_theta':'sm_goal_theta'})
00367
00368
00369 smach.StateMachine.add('AC_PT_7',
00370 smach_ros.SimpleActionState('people_tracking',
00371 peopleTrackAction,
00372 result_cb=pt_result_cb,
00373 outcomes=['min_dist','max_dist','no_people','people_far'],
00374 output_keys=['frame_id_pt_out','x_pt_out',
00375 'y_pt_out','id_pt_out','theta_pt_out']),
00376 transitions={'max_dist':'RHRI8',
00377 'min_dist':'RHRI9',
00378 'no_people':'HRI_FAIL',
00379 'people_far':'CC_HRI_RB_1'},
00380 remapping={'frame_id_pt_out':'sm_frame_id',
00381 'x_pt_out':'sm_goal_x',
00382 'y_pt_out':'sm_goal_y',
00383 'id_pt_out':'sm_target_id',
00384 'theta_pt_out':'sm_goal_theta'})
00385
00386 sm.userdata.sm_hri_tts_8 = "Gracies per tornar. Ara ja estem llestos."
00387 sm.userdata.sm_hri_head_8 = ""
00388 smach.StateMachine.add('RHRI8',
00389 hri_rb_cc,
00390 transitions={'succeeded':'succeeded',
00391 'aborted':'HRI_BLOCKED'},
00392 remapping={'cc_hri_tts':'sm_hri_tts_8',
00393 'cc_hri_head':'sm_hri_head_8',
00394 'cc_frame_id':'sm_frame_id',
00395 'cc_goal_x':'sm_goal_x',
00396 'cc_goal_y':'sm_goal_y',
00397 'cc_goal_theta':'sm_goal_theta'})
00398
00399
00400 sm.userdata.sm_hri_tts_9 = "Jo pensava que tavia caigut b "
00401 sm.userdata.sm_hri_head_9 = ""
00402 smach.StateMachine.add('RHRI9',
00403 hri_rb_cc,
00404 transitions={'succeeded':'HRI_LOOK_SOMEONE',
00405 'aborted':'HRI_BLOCKED'},
00406 remapping={'cc_hri_tts':'sm_hri_tts_9',
00407 'cc_hri_head':'sm_hri_head_9',
00408 'cc_frame_id':'sm_frame_id',
00409 'cc_goal_x':'sm_goal_x',
00410 'cc_goal_y':'sm_goal_y',
00411 'cc_goal_theta':'sm_goal_theta'})
00412
00413
00414 sm.userdata.sm_hri_tts_ls = " Buscare' algu' me's simpatic que em vulgui ajudar "
00415 sm.userdata.sm_hri_head_ls = ""
00416 smach.StateMachine.add('HRI_LOOK_SOMEONE',
00417 hri_rb_cc,
00418 transitions={'succeeded':'HRI_BEFORE_CC',
00419 'aborted':'HRI_BLOCKED'},
00420 remapping={'cc_hri_tts':'sm_hri_tts_ls',
00421 'cc_hri_head':'sm_hri_head_ls',
00422 'cc_frame_id':'sm_frame_id',
00423 'cc_goal_x':'sm_goal_x',
00424 'cc_goal_y':'sm_goal_y',
00425 'cc_goal_theta':'sm_goal_theta'})
00426
00427
00428 sm.userdata.sm_hri_tts_10 = "Pro perque ten vas? vinga no tinguis por"
00429 sm.userdata.sm_hri_head_10 = ""
00430 smach.StateMachine.add('RHRI10',
00431 hri_rb_cc,
00432 transitions={'succeeded':'AC_PT_8',
00433 'aborted':'HRI_BLOCKED'},
00434 remapping={'cc_hri_tts':'sm_hri_tts_10',
00435 'cc_hri_head':'sm_hri_head_10',
00436 'cc_frame_id':'sm_frame_id',
00437 'cc_goal_x':'sm_goal_x',
00438 'cc_goal_y':'sm_goal_y',
00439 'cc_goal_theta':'sm_goal_theta'})
00440
00441
00442 sm.userdata.sm_hri_tts_11 = "Veig que tu has repensat. perfecte. ja podem comensar."
00443 sm.userdata.sm_hri_head_11 = ""
00444 smach.StateMachine.add('RHRI11',
00445 hri_rb_cc,
00446 transitions={'succeeded':'succeeded',
00447 'aborted':'HRI_BLOCKED'},
00448 remapping={'cc_hri_tts':'sm_hri_tts_11',
00449 'cc_hri_head':'sm_hri_head_11',
00450 'cc_frame_id':'sm_frame_id',
00451 'cc_goal_x':'sm_goal_x',
00452 'cc_goal_y':'sm_goal_y',
00453 'cc_goal_theta':'sm_goal_theta'})
00454
00455
00456 sm.userdata.sm_hri_tts_12 = "Ten vas? "
00457 sm.userdata.sm_hri_head_12 = ""
00458 smach.StateMachine.add('RHRI12',
00459 hri_rb_cc,
00460 transitions={'succeeded':'HRI_LOOK_SOMEONE',
00461 'aborted':'HRI_BLOCKED'},
00462 remapping={'cc_hri_tts':'sm_hri_tts_12',
00463 'cc_hri_head':'sm_hri_head_12',
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
00470
00471 smach.StateMachine.add('AC_PT_8',
00472 smach_ros.SimpleActionState('people_tracking',
00473 peopleTrackAction,
00474 result_cb=pt_result_cb,
00475 outcomes=['min_dist','max_dist','no_people','people_far'],
00476 output_keys=['frame_id_pt_out','x_pt_out',
00477 'y_pt_out','id_pt_out','theta_pt_out']),
00478 transitions={'max_dist':'RHRI11',
00479 'min_dist':'RHRI12',
00480 'no_people':'HRI_FAIL',
00481 'people_far':'CC_HRI_RB_1'},
00482 remapping={'frame_id_pt_out':'sm_frame_id',
00483 'x_pt_out':'sm_goal_x',
00484 'y_pt_out':'sm_goal_y',
00485 'id_pt_out':'sm_target_id',
00486 'theta_pt_out':'sm_goal_theta'})
00487
00488
00489
00490 sm.userdata.sm_hri_tts_13 = "Ei! pro no marxis, nome's son dos minutets"
00491 sm.userdata.sm_hri_head_13 = ""
00492 smach.StateMachine.add('RHRI13',
00493 hri_rb_cc,
00494 transitions={'succeeded':'AC_PT_13',
00495 'aborted':'HRI_BLOCKED'},
00496 remapping={'cc_hri_tts':'sm_hri_tts_13',
00497 'cc_hri_head':'sm_hri_head_13',
00498 'cc_frame_id':'sm_frame_id',
00499 'cc_goal_x':'sm_goal_x',
00500 'cc_goal_y':'sm_goal_y',
00501 'cc_goal_theta':'sm_goal_theta'})
00502
00503
00504
00505
00506 smach.StateMachine.add('AC_PT_13',
00507 smach_ros.SimpleActionState('people_tracking',
00508 peopleTrackAction,
00509 result_cb=pt_result_cb,
00510 outcomes=['min_dist','max_dist','no_people','people_far'],
00511 output_keys=['frame_id_pt_out','x_pt_out',
00512 'y_pt_out','id_pt_out','theta_pt_out']),
00513 transitions={'max_dist':'RHRI4',
00514 'min_dist':'RHRI14',
00515 'no_people':'HRI_FAIL',
00516 'people_far':'CC_HRI_RB_1'},
00517 remapping={'frame_id_pt_out':'sm_frame_id',
00518 'x_pt_out':'sm_goal_x',
00519 'y_pt_out':'sm_goal_y',
00520 'id_pt_out':'sm_target_id',
00521 'theta_pt_out':'sm_goal_theta'})
00522
00523
00524 sm.userdata.sm_hri_tts_14 = "Doncs adeu, ja veig que no em vols ajudar."
00525 sm.userdata.sm_hri_head_14 = ""
00526 smach.StateMachine.add('RHRI14',
00527 hri_rb_cc,
00528 transitions={'succeeded':'HRI_LOOK_SOMEONE',
00529 'aborted':'HRI_BLOCKED'},
00530 remapping={'cc_hri_tts':'sm_hri_tts_14',
00531 'cc_hri_head':'sm_hri_head_14',
00532 'cc_frame_id':'sm_frame_id',
00533 'cc_goal_x':'sm_goal_x',
00534 'cc_goal_y':'sm_goal_y',
00535 'cc_goal_theta':'sm_goal_theta'})
00536
00537
00538 sm.userdata.sm_hri_tts_15 = "Doncs adeu, ja veig que no em vols ajudar."
00539 sm.userdata.sm_hri_head_15 = ""
00540 smach.StateMachine.add('RHRI15',
00541 hri_rb_cc,
00542 transitions={'succeeded':'AC_PT_15',
00543 'aborted':'HRI_BLOCKED'},
00544 remapping={'cc_hri_tts':'sm_hri_tts_15',
00545 'cc_hri_head':'sm_hri_head_15',
00546 'cc_frame_id':'sm_frame_id',
00547 'cc_goal_x':'sm_goal_x',
00548 'cc_goal_y':'sm_goal_y',
00549 'cc_goal_theta':'sm_goal_theta'})
00550
00551
00552 smach.StateMachine.add('AC_PT_15',
00553 smach_ros.SimpleActionState('people_tracking',
00554 peopleTrackAction,
00555 result_cb=pt_result_cb,
00556 outcomes=['min_dist','max_dist','no_people','people_far'],
00557 output_keys=['frame_id_pt_out','x_pt_out',
00558 'y_pt_out','id_pt_out','theta_pt_out']),
00559 transitions={'min_dist':'RHRI16',
00560 'max_dist':'RHRI17',
00561 'no_people':'HRI_FAIL',
00562 'people_far':'CC_HRI_RB_1'},
00563 remapping={'frame_id_pt_out':'sm_frame_id',
00564 'x_pt_out':'sm_goal_x',
00565 'y_pt_out':'sm_goal_y',
00566 'id_pt_out':'sm_target_id',
00567 'theta_pt_out':'sm_goal_theta'})
00568
00569 sm.userdata.sm_hri_tts_16 = "Gracies per venir."
00570 sm.userdata.sm_hri_head_16 = ""
00571 smach.StateMachine.add('RHRI16',
00572 hri_rb_cc,
00573 transitions={'succeeded':'RHRI3',
00574 'aborted':'HRI_BLOCKED'},
00575 remapping={'cc_hri_tts':'sm_hri_tts_16',
00576 'cc_hri_head':'sm_hri_head_16',
00577 'cc_frame_id':'sm_frame_id',
00578 'cc_goal_x':'sm_goal_x',
00579 'cc_goal_y':'sm_goal_y',
00580 'cc_goal_theta':'sm_goal_theta'})
00581
00582 sm.userdata.sm_hri_tts_17 = "Si no vols venir. tu mateix. buscare' algu' altre"
00583 sm.userdata.sm_hri_head_17 = ""
00584 smach.StateMachine.add('RHRI17',
00585 hri_rb_cc,
00586 transitions={'succeeded':'HRI_LOOK_SOMEONE',
00587 'aborted':'HRI_BLOCKED'},
00588 remapping={'cc_hri_tts':'sm_hri_tts_17',
00589 'cc_hri_head':'sm_hri_head_17',
00590 'cc_frame_id':'sm_frame_id',
00591 'cc_goal_x':'sm_goal_x',
00592 'cc_goal_y':'sm_goal_y',
00593 'cc_goal_theta':'sm_goal_theta'})
00594
00595
00596 sm.userdata.sm_hri_tts_18 = "Hola! Perque no tapropes i parlem una estona?"
00597 sm.userdata.sm_hri_head_18 = ""
00598 smach.StateMachine.add('RHRI18',
00599 hri_rb_cc,
00600 transitions={'succeeded':'AC_PT_18',
00601 'aborted':'HRI_BLOCKED'},
00602 remapping={'cc_hri_tts':'sm_hri_tts_18',
00603 'cc_hri_head':'sm_hri_head_18',
00604 'cc_frame_id':'sm_frame_id',
00605 'cc_goal_x':'sm_goal_x',
00606 'cc_goal_y':'sm_goal_y',
00607 'cc_goal_theta':'sm_goal_theta'})
00608
00609
00610
00611 smach.StateMachine.add('AC_PT_18',
00612 smach_ros.SimpleActionState('people_tracking',
00613 peopleTrackAction,
00614 result_cb=pt_result_cb,
00615 outcomes=['min_dist','max_dist','no_people','people_far'],
00616 output_keys=['frame_id_pt_out','x_pt_out',
00617 'y_pt_out','id_pt_out','theta_pt_out']),
00618 transitions={'min_dist':'RHRI2',
00619 'max_dist':'RHRI19',
00620 'no_people':'HRI_FAIL',
00621 'people_far':'CC_HRI_RB_1'},
00622 remapping={'frame_id_pt_out':'sm_frame_id',
00623 'x_pt_out':'sm_goal_x',
00624 'y_pt_out':'sm_goal_y',
00625 'id_pt_out':'sm_target_id',
00626 'theta_pt_out':'sm_goal_theta'})
00627
00628
00629
00630 sm.userdata.sm_hri_tts_19 = "No siguis antipatic, vine una estona!."
00631 sm.userdata.sm_hri_head_19 = ""
00632 smach.StateMachine.add('RHRI19',
00633 hri_rb_cc,
00634 transitions={'succeeded':'AC_PT_19',
00635 'aborted':'HRI_BLOCKED'},
00636 remapping={'cc_hri_tts':'sm_hri_tts_19',
00637 'cc_hri_head':'sm_hri_head_19',
00638 'cc_frame_id':'sm_frame_id',
00639 'cc_goal_x':'sm_goal_x',
00640 'cc_goal_y':'sm_goal_y',
00641 'cc_goal_theta':'sm_goal_theta'})
00642
00643
00644 smach.StateMachine.add('AC_PT_19',
00645 smach_ros.SimpleActionState('people_tracking',
00646 peopleTrackAction,
00647 result_cb=pt_result_cb,
00648 outcomes=['min_dist','max_dist','no_people','people_far'],
00649 output_keys=['frame_id_pt_out','x_pt_out',
00650 'y_pt_out','id_pt_out','theta_pt_out']),
00651 transitions={'min_dist':'RHRI20',
00652 'max_dist':'RHRI21',
00653 'no_people':'HRI_FAIL',
00654 'people_far':'CC_HRI_RB_1'},
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
00663 sm.userdata.sm_hri_tts_20 = "Gracies per aproparte, com estas?."
00664 sm.userdata.sm_hri_head_20 = ""
00665 smach.StateMachine.add('RHRI20',
00666 hri_rb_cc,
00667 transitions={'succeeded':'RHRI3',
00668 'aborted':'HRI_BLOCKED'},
00669 remapping={'cc_hri_tts':'sm_hri_tts_20',
00670 'cc_hri_head':'sm_hri_head_20',
00671 'cc_frame_id':'sm_frame_id',
00672 'cc_goal_x':'sm_goal_x',
00673 'cc_goal_y':'sm_goal_y',
00674 'cc_goal_theta':'sm_goal_theta'})
00675
00676
00677 sm.userdata.sm_hri_tts_21 = "Si no vols venir, tu to perds, buscare algu altre."
00678 sm.userdata.sm_hri_head_21 = ""
00679 smach.StateMachine.add('RHRI21',
00680 hri_rb_cc,
00681 transitions={'succeeded':'HRI_LOOK_SOMEONE',
00682 'aborted':'HRI_BLOCKED'},
00683 remapping={'cc_hri_tts':'sm_hri_tts_21',
00684 'cc_hri_head':'sm_hri_head_21',
00685 'cc_frame_id':'sm_frame_id',
00686 'cc_goal_x':'sm_goal_x',
00687 'cc_goal_y':'sm_goal_y',
00688 'cc_goal_theta':'sm_goal_theta'})
00689
00690
00691
00692
00693 sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00694 sis.start()
00695
00696
00697 outcome = sm.execute()
00698
00699
00700 rospy.spin()
00701 sis.stop()
00702
00703
00704 if __name__ == '__main__':
00705 main()
00706