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(0,1, 0)
00049 q = tf.transformations.quaternion_from_euler(0, 0, 2)
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 = 37
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 > 4:
00079 return 'max_dist'
00080 else:
00081 return 'min_dist'
00082
00083
00084
00085 def main():
00086 rospy.init_node('concurrent_hri_move_base')
00087
00088
00089 sm = smach.StateMachine(['succeeded','aborted','preempted'])
00090
00091
00092
00093
00094
00095
00096
00097
00098 def hri_mb_out_cb(outcome_map):
00099 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_MOVE_BASE'] == 'succeeded':
00100 return 'succeeded'
00101 else:
00102 return 'aborted'
00103
00104
00105 def hri_rb_out_cb(outcome_map):
00106 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_ROTATE_BASE'] == 'succeeded':
00107 return 'succeeded'
00108 else:
00109 return 'succeeded'
00110
00111
00112 hri_mb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00113 default_outcome='aborted',
00114 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00115 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00116 child_termination_cb = lambda so: False,
00117 outcome_cb = hri_mb_out_cb)
00118 with hri_mb_cc:
00119 smach.Concurrence.add('CC_HRI',
00120 smach_ros.SimpleActionState('hri',
00121 sequenceAction,
00122 goal_cb=hri_goal_cb,
00123 input_keys=['tts_in','head_in']),
00124 remapping={'tts_in':'cc_hri_tts',
00125 'head_in':'cc_hri_head'})
00126
00127 smach.Concurrence.add('CC_MOVE_BASE',
00128 smach_ros.SimpleActionState('MoveBase',
00129 MoveBaseAction,
00130 goal_cb=move_base_goal_cb,
00131 input_keys=['frame_id_in', 'goal_x_in',
00132 'goal_y_in', 'goal_theta_in']),
00133 remapping={'frame_id_in':'cc_frame_id',
00134 'goal_x_in':'cc_goal_x',
00135 'goal_y_in':'cc_goal_y',
00136 'goal_theta_in':'cc_goal_theta'})
00137
00138
00139 hri_rb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00140 default_outcome='aborted',
00141 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_frame_id',
00142 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00143 child_termination_cb = lambda so: False,
00144 outcome_cb = hri_rb_out_cb)
00145 with hri_rb_cc:
00146 smach.Concurrence.add('CC_HRI',
00147 smach_ros.SimpleActionState('hri',
00148 sequenceAction,
00149 goal_cb=hri_goal_cb,
00150 input_keys=['tts_in','head_in']),
00151 remapping={'tts_in':'cc_hri_tts',
00152 'head_in':'cc_hri_head'})
00153
00154 smach.Concurrence.add('CC_ROTATE_BASE',
00155 smach_ros.SimpleActionState('MoveBase',
00156 MoveBaseAction,
00157 goal_cb=rotate_base_goal_cb,
00158 input_keys=['frame_id_in', 'goal_x_in',
00159 'goal_y_in', 'goal_theta_in']),
00160 remapping={'frame_id_in':'cc_frame_id',
00161 'goal_x_in':'cc_goal_x',
00162 'goal_y_in':'cc_goal_y',
00163 'goal_theta_in':'cc_goal_theta'})
00164
00165
00166
00167
00168 with sm:
00169
00170 sm.userdata.sm_hri_tts_1 = "Hola. estic buscant a algu' am qui parlar"
00171 sm.userdata.sm_hri_head_1 = "head_random_4.xml"
00172 smach.StateMachine.add('HRI_BEFORE_CC',
00173 smach_ros.SimpleActionState('hri',
00174 sequenceAction,
00175 goal_cb=hri_goal_cb,
00176 input_keys=['tts_in', 'head_in']),
00177 transitions={'succeeded':'AC_PT_1'},
00178 remapping={'tts_in':'sm_hri_tts_1',
00179 'head_in':'sm_hri_head_1'})
00180
00181
00182 smach.StateMachine.add('AC_PT_1',
00183 smach_ros.SimpleActionState('people_tracking',
00184 peopleTrackAction,
00185 result_cb=pt_result_cb,
00186 outcomes=['min_dist','max_dist','no_people','people_far'],
00187 output_keys=['frame_id_pt_out','x_pt_out',
00188 'y_pt_out','id_pt_out','theta_pt_out']),
00189 transitions={'max_dist':'CC_HRI_MB_1',
00190 'min_dist':'HRI_NEAR_1',
00191 'no_people':'HRI_FAIL',
00192 'people_far':'CC_HRI_RB_1'},
00193 remapping={'frame_id_pt_out':'sm_frame_id',
00194 'x_pt_out':'sm_goal_x',
00195 'y_pt_out':'sm_goal_y',
00196 'id_pt_out':'sm_target_id',
00197 'theta_pt_out':'sm_goal_theta'})
00198
00199
00200 sm.userdata.sm_hri_tts_2 = "Uy? no veig a ningu'. HO tornare a intentar"
00201 sm.userdata.sm_hri_head_2 = "head_random_4.xml"
00202 smach.StateMachine.add('HRI_FAIL',
00203 smach_ros.SimpleActionState('hri',
00204 sequenceAction,
00205 goal_cb=hri_goal_cb,
00206 input_keys=['tts_in', 'head_in']),
00207 transitions={'succeeded':'HRI_BEFORE_CC'},
00208 remapping={'tts_in':'sm_hri_tts_2',
00209 'head_in':'sm_hri_head_2'})
00210
00211
00212
00213 sm.userdata.sm_hri_tts_approach_1 = "Ja he trobat a una persona. no marxis, Espera que vinc."
00214 sm.userdata.sm_hri_head_approach_1 = "head_greeting.xml"
00215 smach.StateMachine.add('CC_HRI_MB_1',
00216 hri_mb_cc,
00217 transitions={'succeeded':'AC_PT_12',
00218 'aborted':'HRI_BLOCKED'},
00219 remapping={'cc_hri_tts':'sm_hri_tts_approach_1',
00220 'cc_hri_head':'sm_hri_head_approach_1',
00221 'cc_frame_id':'sm_frame_id',
00222 'cc_goal_x':'sm_goal_x',
00223 'cc_goal_y':'sm_goal_y',
00224 'cc_goal_theta':'sm_goal_theta'})
00225
00226
00227 sm.userdata.sm_hri_tts_5 = "Hey, com est'as? Jo soc la tibi"
00228 sm.userdata.sm_hri_head_5 = "head_random_2.xml"
00229 smach.StateMachine.add('HRI_NEAR_1',
00230 smach_ros.SimpleActionState('hri',
00231 sequenceAction,
00232 goal_cb=hri_goal_cb,
00233 input_keys=['tts_in', 'head_in']),
00234 transitions={'succeeded':'AC_PT_2'},
00235 remapping={'tts_in':'sm_hri_tts_5',
00236 'head_in':'sm_hri_head_5'})
00237
00238
00239 sm.userdata.sm_hri_tts_near_2 = "Magradaria explicarte quatre cosetes, sobre aquest esdeveniment"
00240 sm.userdata.sm_hri_head_near_2 = "head_random_3.xml"
00241 smach.StateMachine.add('HRI_NEAR_2',
00242 smach_ros.SimpleActionState('hri',
00243 sequenceAction,
00244 goal_cb=hri_goal_cb,
00245 input_keys=['tts_in', 'head_in']),
00246 transitions={'succeeded':'AC_PT_2'},
00247 remapping={'tts_in':'sm_hri_tts_near_2',
00248 'head_in':'sm_hri_head_near_2'})
00249
00250 sm.userdata.sm_hri_tts_near_3 = "Nomes vull comentar, alguna curiositat sobre el salo del comic"
00251 sm.userdata.sm_hri_head_near_3 = "head_random_4.xml"
00252 smach.StateMachine.add('HRI_NEAR_3',
00253 smach_ros.SimpleActionState('hri',
00254 sequenceAction,
00255 goal_cb=hri_goal_cb,
00256 input_keys=['tts_in', 'head_in']),
00257 transitions={'succeeded':'AC_PT_2'},
00258 remapping={'tts_in':'sm_hri_tts_near_2',
00259 'head_in':'sm_hri_head_near_2'})
00260
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_MB_2',
00271 'min_dist':'HRI_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
00281 sm.userdata.sm_hri_tts_approach_2 = "si et mous no em sentira's, estigues quiet sisplau"
00282 sm.userdata.sm_hri_head_approach_2 = "head_greeting.xml"
00283 smach.StateMachine.add('CC_HRI_MB_2',
00284 hri_mb_cc,
00285 transitions={'succeeded':'AC_PT_4',
00286 'aborted':'HRI_BLOCKED'},
00287 remapping={'cc_hri_tts':'sm_hri_tts_approach_2',
00288 'cc_hri_head':'sm_hri_head_approach_2',
00289 'cc_frame_id':'sm_frame_id',
00290 'cc_goal_x':'sm_goal_x',
00291 'cc_goal_y':'sm_goal_y',
00292 'cc_goal_theta':'sm_goal_theta'})
00293
00294
00295
00296 sm.userdata.sm_hri_tts_meet_1 = "com ja deus saber, estem al salo del comic. aquest any es especial ja que el tema central son els robots. per aquesta rao, tota la familia de liri, hem vingut. Estarem aqui fins diumenge per amanitzar el salo."
00297 sm.userdata.sm_hri_head_meet_1 = "head_random_2.xml"
00298 smach.StateMachine.add('HRI_MEET_1',
00299 smach_ros.SimpleActionState('hri',
00300 sequenceAction,
00301 goal_cb=hri_goal_cb,
00302 input_keys=['tts_in','head_in']),
00303 transitions={'succeeded':'AC_PT_3'},
00304 remapping={'tts_in':'sm_hri_tts_meet_1',
00305 'head_in':'sm_hri_head_meet_1'})
00306
00307
00308 sm.userdata.sm_hri_tts_meet_1_1 = "COm testava dient, abans de que et moguessis, total la familia, de liri hem vingut per estar amb vosaltres"
00309 sm.userdata.sm_hri_head_meet_1_1 = "head_random_2.xml"
00310 smach.StateMachine.add('HRI_MEET_1_1',
00311 smach_ros.SimpleActionState('hri',
00312 sequenceAction,
00313 goal_cb=hri_goal_cb,
00314 input_keys=['tts_in', 'head_in']),
00315 transitions={'succeeded':'AC_PT_7'},
00316 remapping={'tts_in':'sm_hri_tts_meet_1_1',
00317 'head_in':'sm_hri_head_meet_1_1'})
00318
00319
00320
00321
00322 sm.userdata.sm_hri_tts_meet_2 = "Per aqui hi ha el masin guerr seta. El 12 de setembre daquest any, far'a 40 anys. El meu germa' dabo, de gran vol ser com ell. E's un somnniatruites. Nosaltres ja tenim el seu autograf. i tu? ja el tens?"
00323 sm.userdata.sm_hri_head_meet_2 = "head_random_2.xml"
00324 smach.StateMachine.add('HRI_MEET_2',
00325 smach_ros.SimpleActionState('hri',
00326 sequenceAction,
00327 goal_cb=hri_goal_cb,
00328 input_keys=['tts_in', 'head_in']),
00329 transitions={'succeeded':'AC_PT_9'},
00330 remapping={'tts_in':'sm_hri_tts_meet_2',
00331 'head_in':'sm_hri_head_meet_2'})
00332
00333
00334
00335 smach.StateMachine.add('AC_PT_3',
00336 smach_ros.SimpleActionState('people_tracking',
00337 peopleTrackAction,
00338 result_cb=pt_result_cb,
00339 outcomes=['min_dist','max_dist','no_people','people_far'],
00340 output_keys=['frame_id_pt_out','x_pt_out',
00341 'y_pt_out','id_pt_out','theta_pt_out']),
00342 transitions={'max_dist':'CC_HRI_MB_4',
00343 'min_dist':'HRI_MEET_2',
00344 'no_people':'HRI_FAIL',
00345 'people_far':'CC_HRI_RB_1'},
00346 remapping={'frame_id_pt_out':'sm_frame_id',
00347 'x_pt_out':'sm_goal_x',
00348 'y_pt_out':'sm_goal_y',
00349 'id_pt_out':'sm_target_id',
00350 'theta_pt_out':'sm_goal_theta'})
00351
00352
00353
00354 smach.StateMachine.add('AC_PT_4',
00355 smach_ros.SimpleActionState('people_tracking',
00356 peopleTrackAction,
00357 result_cb=pt_result_cb,
00358 outcomes=['min_dist','max_dist','no_people','people_far'],
00359 output_keys=['frame_id_pt_out','x_pt_out',
00360 'y_pt_out','id_pt_out','theta_pt_out']),
00361 transitions={'max_dist':'CC_HRI_MB_3',
00362 'min_dist':'HRI_MEET_1',
00363 'no_people':'HRI_FAIL',
00364 'people_far':'CC_HRI_RB_1'},
00365 remapping={'frame_id_pt_out':'sm_frame_id',
00366 'x_pt_out':'sm_goal_x',
00367 'y_pt_out':'sm_goal_y',
00368 'id_pt_out':'sm_target_id',
00369 'theta_pt_out':'sm_goal_theta'})
00370
00371
00372
00373 smach.StateMachine.add('AC_PT_5',
00374 smach_ros.SimpleActionState('people_tracking',
00375 peopleTrackAction,
00376 result_cb=pt_result_cb,
00377 outcomes=['min_dist','max_dist','no_people','people_far'],
00378 output_keys=['frame_id_pt_out','x_pt_out',
00379 'y_pt_out','id_pt_out','theta_pt_out']),
00380 transitions={'max_dist':'FAREWELL_2',
00381 'min_dist':'HRI_MEET_1_1',
00382 'no_people':'HRI_FAIL',
00383 'people_far':'CC_HRI_RB_1'},
00384 remapping={'frame_id_pt_out':'sm_frame_id',
00385 'x_pt_out':'sm_goal_x',
00386 'y_pt_out':'sm_goal_y',
00387 'id_pt_out':'sm_target_id',
00388 'theta_pt_out':'sm_goal_theta'})
00389
00390
00391 sm.userdata.sm_hri_tts_approach_3 = "Escolta? es el segon cop que et mous, estigues quiet!"
00392 sm.userdata.sm_hri_head_approach_3 = "head_greeting.xml"
00393 smach.StateMachine.add('CC_HRI_MB_3',
00394 hri_mb_cc,
00395 transitions={'succeeded':'AC_PT_6',
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
00405 sm.userdata.sm_hri_tts_approach_4 = "estic parlant amb tu, sisplau, no et moguis, que no em puc concentrar"
00406 sm.userdata.sm_hri_head_approach_4 = "head_greeting.xml"
00407 smach.StateMachine.add('CC_HRI_MB_4',
00408 hri_mb_cc,
00409 transitions={'succeeded':'AC_PT_5',
00410 'aborted':'HRI_BLOCKED'},
00411 remapping={'cc_hri_tts':'sm_hri_tts_approach_4',
00412 'cc_hri_head':'sm_hri_head_approach_4',
00413 'cc_frame_id':'sm_frame_id',
00414 'cc_goal_x':'sm_goal_x',
00415 'cc_goal_y':'sm_goal_y',
00416 'cc_goal_theta':'sm_goal_theta'})
00417
00418
00419
00420 sm.userdata.sm_hri_tts_approach_5 = "Un altre cop? ke estas nervios? sisplau, espera a que acabi dexplicartu toot, si es un momentet"
00421 sm.userdata.sm_hri_head_approach_5 = "head_greeting.xml"
00422 smach.StateMachine.add('CC_HRI_MB_5',
00423 hri_mb_cc,
00424 transitions={'succeeded':'AC_PT_8',
00425 'aborted':'HRI_BLOCKED'},
00426 remapping={'cc_hri_tts':'sm_hri_tts_approach_5',
00427 'cc_hri_head':'sm_hri_head_approach_5',
00428 'cc_frame_id':'sm_frame_id',
00429 'cc_goal_x':'sm_goal_x',
00430 'cc_goal_y':'sm_goal_y',
00431 'cc_goal_theta':'sm_goal_theta'})
00432
00433 sm.userdata.sm_hri_tts_approach_6 = " No marxis, nome's et vull dir adeu"
00434 sm.userdata.sm_hri_head_approach_6 = "head_greeting.xml"
00435 smach.StateMachine.add('CC_HRI_MB_6',
00436 hri_mb_cc,
00437 transitions={'succeeded':'AC_PT_10',
00438 'aborted':'HRI_BLOCKED'},
00439 remapping={'cc_hri_tts':'sm_hri_tts_approach_6',
00440 'cc_hri_head':'sm_hri_head_approach_6',
00441 'cc_frame_id':'sm_frame_id',
00442 'cc_goal_x':'sm_goal_x',
00443 'cc_goal_y':'sm_goal_y',
00444 'cc_goal_theta':'sm_goal_theta'})
00445
00446
00447 sm.userdata.sm_hri_tts_approach_7 = " ultim cop que insisteixo. sisplau no ten vagis"
00448 sm.userdata.sm_hri_head_approach_7 = "head_greeting.xml"
00449 smach.StateMachine.add('CC_HRI_MB_7',
00450 hri_mb_cc,
00451 transitions={'succeeded':'AC_PT_14',
00452 'aborted':'HRI_BLOCKED'},
00453 remapping={'cc_hri_tts':'sm_hri_tts_approach_7',
00454 'cc_hri_head':'sm_hri_head_approach_7',
00455 'cc_frame_id':'sm_frame_id',
00456 'cc_goal_x':'sm_goal_x',
00457 'cc_goal_y':'sm_goal_y',
00458 'cc_goal_theta':'sm_goal_theta'})
00459
00460
00461
00462 smach.StateMachine.add('AC_PT_6',
00463 smach_ros.SimpleActionState('people_tracking',
00464 peopleTrackAction,
00465 result_cb=pt_result_cb,
00466 outcomes=['min_dist','max_dist','no_people','people_far'],
00467 output_keys=['frame_id_pt_out','x_pt_out',
00468 'y_pt_out','id_pt_out','theta_pt_out']),
00469 transitions={'max_dist':'FAREWELL_3',
00470 'min_dist':'HRI_MEET_1',
00471 'no_people':'HRI_FAIL',
00472 'people_far':'CC_HRI_RB_1'},
00473 remapping={'frame_id_pt_out':'sm_frame_id',
00474 'x_pt_out':'sm_goal_x',
00475 'y_pt_out':'sm_goal_y',
00476 'id_pt_out':'sm_target_id',
00477 'theta_pt_out':'sm_goal_theta'})
00478
00479
00480
00481 smach.StateMachine.add('AC_PT_7',
00482 smach_ros.SimpleActionState('people_tracking',
00483 peopleTrackAction,
00484 result_cb=pt_result_cb,
00485 outcomes=['min_dist','max_dist','no_people','people_far'],
00486 output_keys=['frame_id_pt_out','x_pt_out',
00487 'y_pt_out','id_pt_out','theta_pt_out']),
00488 transitions={'max_dist':'CC_HRI_MB_5',
00489 'min_dist':'HRI_MEET_2',
00490 'no_people':'HRI_FAIL',
00491 'people_far':'CC_HRI_RB_1'},
00492 remapping={'frame_id_pt_out':'sm_frame_id',
00493 'x_pt_out':'sm_goal_x',
00494 'y_pt_out':'sm_goal_y',
00495 'id_pt_out':'sm_target_id',
00496 'theta_pt_out':'sm_goal_theta'})
00497
00498 smach.StateMachine.add('AC_PT_8',
00499 smach_ros.SimpleActionState('people_tracking',
00500 peopleTrackAction,
00501 result_cb=pt_result_cb,
00502 outcomes=['min_dist','max_dist','no_people','people_far'],
00503 output_keys=['frame_id_pt_out','x_pt_out',
00504 'y_pt_out','id_pt_out','theta_pt_out']),
00505 transitions={'max_dist':'FAREWELL_2',
00506 'min_dist':'HRI_MEET_2',
00507 'no_people':'HRI_FAIL',
00508 'people_far':'CC_HRI_RB_1'},
00509 remapping={'frame_id_pt_out':'sm_frame_id',
00510 'x_pt_out':'sm_goal_x',
00511 'y_pt_out':'sm_goal_y',
00512 'id_pt_out':'sm_target_id',
00513 'theta_pt_out':'sm_goal_theta'})
00514
00515 smach.StateMachine.add('AC_PT_9',
00516 smach_ros.SimpleActionState('people_tracking',
00517 peopleTrackAction,
00518 result_cb=pt_result_cb,
00519 outcomes=['min_dist','max_dist','no_people','people_far'],
00520 output_keys=['frame_id_pt_out','x_pt_out',
00521 'y_pt_out','id_pt_out','theta_pt_out']),
00522 transitions={'max_dist':'CC_HRI_MB_6',
00523 'min_dist':'FAREWELL',
00524 'no_people':'HRI_FAIL',
00525 'people_far':'CC_HRI_RB_4'},
00526 remapping={'frame_id_pt_out':'sm_frame_id',
00527 'x_pt_out':'sm_goal_x',
00528 'y_pt_out':'sm_goal_y',
00529 'id_pt_out':'sm_target_id',
00530 'theta_pt_out':'sm_goal_theta'})
00531
00532
00533 smach.StateMachine.add('AC_PT_10',
00534 smach_ros.SimpleActionState('people_tracking',
00535 peopleTrackAction,
00536 result_cb=pt_result_cb,
00537 outcomes=['min_dist','max_dist','no_people','people_far'],
00538 output_keys=['frame_id_pt_out','x_pt_out',
00539 'y_pt_out','id_pt_out','theta_pt_out']),
00540 transitions={'max_dist':'FAREWELL',
00541 'min_dist':'FAREWELL',
00542 'no_people':'HRI_FAIL',
00543 'people_far':'CC_HRI_RB_5'},
00544 remapping={'frame_id_pt_out':'sm_frame_id',
00545 'x_pt_out':'sm_goal_x',
00546 'y_pt_out':'sm_goal_y',
00547 'id_pt_out':'sm_target_id',
00548 'theta_pt_out':'sm_goal_theta'})
00549
00550 smach.StateMachine.add('AC_PT_12',
00551 smach_ros.SimpleActionState('people_tracking',
00552 peopleTrackAction,
00553 result_cb=pt_result_cb,
00554 outcomes=['min_dist','max_dist','no_people','people_far'],
00555 output_keys=['frame_id_pt_out','x_pt_out',
00556 'y_pt_out','id_pt_out','theta_pt_out']),
00557 transitions={'max_dist':'CC_HRI_MB_7',
00558 'min_dist':'HRI_NEAR_2',
00559 'no_people':'HRI_FAIL',
00560 'people_far':'CC_HRI_RB_5'},
00561 remapping={'frame_id_pt_out':'sm_frame_id',
00562 'x_pt_out':'sm_goal_x',
00563 'y_pt_out':'sm_goal_y',
00564 'id_pt_out':'sm_target_id',
00565 'theta_pt_out':'sm_goal_theta'})
00566
00567 smach.StateMachine.add('AC_PT_14',
00568 smach_ros.SimpleActionState('people_tracking',
00569 peopleTrackAction,
00570 result_cb=pt_result_cb,
00571 outcomes=['min_dist','max_dist','no_people','people_far'],
00572 output_keys=['frame_id_pt_out','x_pt_out',
00573 'y_pt_out','id_pt_out','theta_pt_out']),
00574 transitions={'max_dist':'FAREWELL_4',
00575 'min_dist':'HRI_NEAR_3',
00576 'no_people':'HRI_FAIL',
00577 'people_far':'CC_HRI_RB_2'},
00578 remapping={'frame_id_pt_out':'sm_frame_id',
00579 'x_pt_out':'sm_goal_x',
00580 'y_pt_out':'sm_goal_y',
00581 'id_pt_out':'sm_target_id',
00582 'theta_pt_out':'sm_goal_theta'})
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601 sm.userdata.sm_hri_tts_people_far = "est'as massa lluny, buscare a una altra persona."
00602 sm.userdata.sm_hri_head_people_far = "head_random_2.xml"
00603 smach.StateMachine.add('CC_HRI_RB_1',
00604 hri_rb_cc,
00605 transitions={'succeeded':'HRI_BEFORE_CC',
00606 'aborted':'HRI_BLOCKED'},
00607 remapping={'cc_hri_tts':'sm_hri_tts_people_far',
00608 'cc_hri_head':'sm_hri_head_people_far',
00609 'cc_frame_id':'sm_frame_id',
00610 'cc_goal_x':'sm_goal_x',
00611 'cc_goal_y':'sm_goal_y',
00612 'cc_goal_theta':'sm_goal_theta'})
00613
00614
00615 sm.userdata.sm_hri_tts_people_far_2 = "Dedueixo que no vols parlar amb mi. BUscare algu altre, que que ho vulgui"
00616 sm.userdata.sm_hri_head_people_far_2 = "head_random_2.xml"
00617 smach.StateMachine.add('CC_HRI_RB_2',
00618 hri_rb_cc,
00619 transitions={'succeeded':'HRI_BEFORE_CC',
00620 'aborted':'HRI_BLOCKED'},
00621 remapping={'cc_hri_tts':'sm_hri_tts_people_far_2',
00622 'cc_hri_head':'sm_hri_head_people_far_2',
00623 'cc_frame_id':'sm_frame_id',
00624 'cc_goal_x':'sm_goal_x',
00625 'cc_goal_y':'sm_goal_y',
00626 'cc_goal_theta':'sm_goal_theta'})
00627
00628
00629 sm.userdata.sm_hri_tts_people_far_3 = "buscare algu altre amb qui parlar"
00630 sm.userdata.sm_hri_head_people_far_3 = "head_random_2.xml"
00631 smach.StateMachine.add('CC_HRI_RB_3',
00632 hri_rb_cc,
00633 transitions={'succeeded':'HRI_BEFORE_CC',
00634 'aborted':'HRI_BLOCKED'},
00635 remapping={'cc_hri_tts':'sm_hri_tts_people_far_3',
00636 'cc_hri_head':'sm_hri_head_people_far_3',
00637 'cc_frame_id':'sm_frame_id',
00638 'cc_goal_x':'sm_goal_x',
00639 'cc_goal_y':'sm_goal_y',
00640 'cc_goal_theta':'sm_goal_theta'})
00641
00642 sm.userdata.sm_hri_tts_people_far_4 = "pro per k'e marxes? nomes em vull acomiadar"
00643 sm.userdata.sm_hri_head_people_far_4 = "head_random_2.xml"
00644 smach.StateMachine.add('CC_HRI_RB_4',
00645 hri_rb_cc,
00646 transitions={'succeeded':'HRI_BEFORE_CC',
00647 'aborted':'HRI_BLOCKED'},
00648 remapping={'cc_hri_tts':'sm_hri_tts_people_far_4',
00649 'cc_hri_head':'sm_hri_head_people_far_4',
00650 'cc_frame_id':'sm_frame_id',
00651 'cc_goal_x':'sm_goal_x',
00652 'cc_goal_y':'sm_goal_y',
00653 'cc_goal_theta':'sm_goal_theta'})
00654
00655 sm.userdata.sm_hri_tts_people_far_5 = "Gracies, per la teva atencio, ha estat un plaer"
00656 sm.userdata.sm_hri_head_people_far_5 = "head_random_2.xml"
00657 smach.StateMachine.add('CC_HRI_RB_5',
00658 hri_rb_cc,
00659 transitions={'succeeded':'HRI_BEFORE_CC',
00660 'aborted':'HRI_BLOCKED'},
00661 remapping={'cc_hri_tts':'sm_hri_tts_people_far_5',
00662 'cc_hri_head':'sm_hri_head_people_far_5',
00663 'cc_frame_id':'sm_frame_id',
00664 'cc_goal_x':'sm_goal_x',
00665 'cc_goal_y':'sm_goal_y',
00666 'cc_goal_theta':'sm_goal_theta'})
00667
00668
00669 sm.userdata.sm_hri_tts_blocked = "Sergi, Fernando, Anaiis, no em puc more, ajudeume"
00670 sm.userdata.sm_hri_head_blocked = "head_farewell.xml"
00671 smach.StateMachine.add('HRI_BLOCKED',
00672 smach_ros.SimpleActionState('hri',
00673 sequenceAction,
00674 goal_cb=hri_goal_cb,
00675 input_keys=['tts_in', 'head_in']),
00676 transitions={'succeeded':'HRI_BEFORE_CC'},
00677 remapping={'tts_in':'sm_hri_tts_blocked',
00678 'head_in':'sm_hri_head_blocked'})
00679
00680
00681 sm.userdata.sm_hri_tts_farewell = "gracies per venir, espero veuret un altre dia"
00682 sm.userdata.sm_hri_head_farewell = "head_farewell.xml"
00683 smach.StateMachine.add('FAREWELL',
00684 smach_ros.SimpleActionState('hri',
00685 sequenceAction,
00686 goal_cb=hri_goal_cb,
00687 input_keys=['tts_in', 'head_in']),
00688 transitions={'succeeded':'CC_HRI_RB_3'},
00689 remapping={'tts_in':'sm_hri_tts_farewell',
00690 'head_in':'sm_hri_head_farewell'})
00691
00692
00693 sm.userdata.sm_hri_tts_farewell_2 = "Vagi b. es una llastima que no hagis volgut escoltarme"
00694 sm.userdata.sm_hri_head_farewell_2 = "head_farewell.xml"
00695 smach.StateMachine.add('FAREWELL_2',
00696 smach_ros.SimpleActionState('hri',
00697 sequenceAction,
00698 goal_cb=hri_goal_cb,
00699 input_keys=['tts_in', 'head_in']),
00700 transitions={'succeeded':'CC_HRI_RB_2'},
00701 remapping={'tts_in':'sm_hri_tts_farewell_2',
00702 'head_in':'sm_hri_head_farewell_2'})
00703
00704
00705
00706 sm.userdata.sm_hri_tts_farewell_3 = "A deu, espero veuret aviat"
00707 sm.userdata.sm_hri_head_farewell_3 = "head_farewell.xml"
00708 smach.StateMachine.add('FAREWELL_3',
00709 smach_ros.SimpleActionState('hri',
00710 sequenceAction,
00711 goal_cb=hri_goal_cb,
00712 input_keys=['tts_in', 'head_in']),
00713 transitions={'succeeded':'HRI_BEFORE_CC'},
00714 remapping={'tts_in':'sm_hri_tts_farewell_3',
00715 'head_in':'sm_hri_head_farewell_3'})
00716
00717
00718 sm.userdata.sm_hri_tts_farewell_4 = "Molt b, si no vols parlar amb mi, buscare algu altre que si q ho fassi"
00719 sm.userdata.sm_hri_head_farewell_4 = "head_farewell.xml"
00720 smach.StateMachine.add('FAREWELL_4',
00721 smach_ros.SimpleActionState('hri',
00722 sequenceAction,
00723 goal_cb=hri_goal_cb,
00724 input_keys=['tts_in', 'head_in']),
00725 transitions={'succeeded':'HRI_BEFORE_CC'},
00726 remapping={'tts_in':'sm_hri_tts_farewell_3',
00727 'head_in':'sm_hri_head_farewell_3'})
00728
00729
00730
00731 sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00732 sis.start()
00733
00734
00735 outcome = sm.execute()
00736
00737
00738 rospy.spin()
00739 sis.stop()
00740
00741
00742 if __name__ == '__main__':
00743 main()
00744