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] = userdata.left_in
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,userdata.goal_y_in, 0)
00049 q = tf.transformations.quaternion_from_euler(0, 0, 1)
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 = 6.1
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 > 6:
00076 return 'people_far'
00077 else:
00078 if distance > 2:
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 sm.userdata.sm_frame_id = "/tibi/base_link"
00093 sm.userdata.sm_goal_x_1 = 0.0
00094 sm.userdata.sm_goal_y_1 = 1.0
00095 sm.userdata.sm_goal_x_2 = 0
00096 sm.userdata.sm_goal_y_2 = -1.0
00097 sm.userdata.sm_goal_theta = 0.0
00098
00099
00100 def hri_mb_out_cb(outcome_map):
00101 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_MOVE_BASE'] == 'succeeded':
00102 return 'succeeded'
00103 else:
00104 return 'aborted'
00105
00106
00107 hri_mb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00108 default_outcome='aborted',
00109 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_hri_left','cc_frame_id',
00110 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00111 child_termination_cb = lambda so: False,
00112 outcome_cb = hri_mb_out_cb)
00113 with hri_mb_cc:
00114 smach.Concurrence.add('CC_HRI',
00115 smach_ros.SimpleActionState('hri',
00116 sequenceAction,
00117 goal_cb=hri_goal_cb,
00118 input_keys=['tts_in','head_in','left_in']),
00119 remapping={'tts_in':'cc_hri_tts',
00120 'head_in':'cc_hri_head',
00121 'left_in':'cc_hri_left'})
00122
00123 smach.Concurrence.add('CC_MOVE_BASE',
00124 smach_ros.SimpleActionState('MoveBase',
00125 MoveBaseAction,
00126 goal_cb=move_base_goal_cb,
00127 input_keys=['frame_id_in', 'goal_x_in',
00128 'goal_y_in', 'goal_theta_in']),
00129 remapping={'frame_id_in':'cc_frame_id',
00130 'goal_x_in':'cc_goal_x',
00131 'goal_y_in':'cc_goal_y',
00132 'goal_theta_in':'cc_goal_theta'})
00133
00134
00135 def hri_rb_out_cb(outcome_map):
00136 if outcome_map['CC_HRI'] == 'succeeded' and outcome_map['CC_ROTATE_BASE'] == 'succeeded':
00137 return 'succeeded'
00138 else:
00139 return 'aborted'
00140 hri_rb_cc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
00141 default_outcome='aborted',
00142 input_keys=['cc_hri_tts', 'cc_hri_head', 'cc_hri_left','cc_frame_id',
00143 'cc_goal_x', 'cc_goal_y', 'cc_goal_theta'],
00144 child_termination_cb = lambda so: False,
00145 outcome_cb = hri_rb_out_cb)
00146 with hri_rb_cc:
00147 smach.Concurrence.add('CC_HRI',
00148 smach_ros.SimpleActionState('hri',
00149 sequenceAction,
00150 goal_cb=hri_goal_cb,
00151 input_keys=['tts_in','head_in','left_in']),
00152 remapping={'tts_in':'cc_hri_tts',
00153 'head_in':'cc_hri_head',
00154 'left_in':'cc_hri_left'})
00155
00156 smach.Concurrence.add('CC_ROTATE_BASE',
00157 smach_ros.SimpleActionState('MoveBase',
00158 MoveBaseAction,
00159 goal_cb=rotate_base_goal_cb,
00160 input_keys=['frame_id_in', 'goal_x_in',
00161 'goal_y_in', 'goal_theta_in']),
00162 remapping={'frame_id_in':'cc_frame_id',
00163 'goal_x_in':'cc_goal_x',
00164 'goal_y_in':'cc_goal_y',
00165 'goal_theta_in':'cc_goal_theta'})
00166
00167
00168
00169
00170 with sm:
00171
00172 sm.userdata.sm_hri_tts_1 = "Hola a tothom. Benvinguts a ladici'o, n'umero trenta del sal'o del c`omic de Barcelona. Jo s'oc la Tibi. Un robot social. Estic aqui', per explicarvos curiositats sobre el salo'."
00173 sm.userdata.sm_hri_head_1 = "head_random_1.xml"
00174 sm.userdata.sm_hri_left_1 = "left_arm_greeting.xml"
00175 smach.StateMachine.add('HRI_1',
00176 smach_ros.SimpleActionState('hri',
00177 sequenceAction,
00178 goal_cb=hri_goal_cb,
00179 input_keys=['tts_in', 'head_in','left_in']),
00180 transitions={'succeeded':'CC_HRI_RB_1'},
00181 remapping={'tts_in':'sm_hri_tts_1',
00182 'head_in':'sm_hri_head_1',
00183 'left_in':'sm_hri_left_1'})
00184
00185
00186 sm.userdata.sm_hri_tts_2 = "Si necessiteu alguna cosa. no dubteu en preguntarme. encara que. ara que hi penso. Millor parleu amb el Da'bu. e's el meu germa. som mol semblants, pro ell e's blau com el dora'imon."
00187 sm.userdata.sm_hri_head_2 = "head_random_2.xml"
00188 sm.userdata.sm_hri_left_2= "left_arm_random_1.xml"
00189 smach.StateMachine.add('CC_HRI_RB_1',
00190 hri_rb_cc,
00191 transitions={'succeeded':'HRI_2',
00192 'aborted':'HRI_BLOCKED'},
00193 remapping={'cc_hri_tts':'sm_hri_tts_2',
00194 'cc_hri_head':'sm_hri_head_2',
00195 'cc_hri_left':'sm_hri_left_2',
00196 'cc_frame_id':'sm_frame_id',
00197 'cc_goal_x':'sm_goal_x_1',
00198 'cc_goal_y':'sm_goal_y_1',
00199 'cc_goal_theta':'sm_goal_theta'})
00200
00201
00202 sm.userdata.sm_hri_tts_3 = "Per aqui esta' el meu cosi. el Teeo. e's un mini tot terreny. Sempre parla de mappes. e's un aborrit. despre's podeu demanarli, que us fassi un mapa tres dee' del recinte."
00203 sm.userdata.sm_hri_head_3 = "head_random_3.xml"
00204 sm.userdata.sm_hri_left_3 = "left_arm_random_2.xml"
00205 smach.StateMachine.add('HRI_2',
00206 smach_ros.SimpleActionState('hri',
00207 sequenceAction,
00208 goal_cb=hri_goal_cb,
00209 input_keys=['tts_in', 'head_in','left_in']),
00210 transitions={'succeeded':'CC_HRI_RB_2'},
00211 remapping={'tts_in':'sm_hri_tts_3',
00212 'head_in':'sm_hri_head_3',
00213 'left_in':'sm_hri_left_3'})
00214
00215
00216 sm.userdata.sm_hri_tts_4 = " Ah! me n'oblidava. ha vingut l'auron. e's aquell robot aranya amb tantes potes. avui esta hivernant, aixi' que no el desperteu, que s'enfadara' i estara' de mal humor"
00217 sm.userdata.sm_hri_head_4 = "head_random_3.xml"
00218 sm.userdata.sm_hri_left_4 = "left_arm_random_3.xml"
00219 smach.StateMachine.add('CC_HRI_RB_2',
00220 hri_rb_cc,
00221 transitions={'succeeded':'HRI_3',
00222 'aborted':'HRI_BLOCKED'},
00223 remapping={'cc_hri_tts':'sm_hri_tts_4',
00224 'cc_hri_head':'sm_hri_head_4',
00225 'cc_hri_left':'sm_hri_left_4',
00226 'cc_frame_id':'sm_frame_id',
00227 'cc_goal_x':'sm_goal_x_2',
00228 'cc_goal_y':'sm_goal_y_2',
00229 'cc_goal_theta':'sm_goal_theta'})
00230
00231
00232 sm.userdata.sm_hri_tts_5 = "I despre's hi ha la uait mun. nosaltres li diem erpes. e's la nostra mascota. e's la bola blanca. I per ultim tenim els humanoides. Tot i que lestrella es el Daruin, el mimat de la familia. I alla' al davant hi ha el Rim, e's de la compete'ncia, pro a vegades ens trobem per pendre xocolata."
00233 sm.userdata.sm_hri_head_5 = "head_random_3.xml"
00234 sm.userdata.sm_hri_left_5= "left_arm_random_2.xml"
00235 smach.StateMachine.add('HRI_3',
00236 smach_ros.SimpleActionState('hri',
00237 sequenceAction,
00238 goal_cb=hri_goal_cb,
00239 input_keys=['tts_in', 'head_in','left_in']),
00240 transitions={'succeeded':'FAREWELL'},
00241 remapping={'tts_in':'sm_hri_tts_5',
00242 'head_in':'sm_hri_head_5',
00243 'left_in':'sm_hri_left_5'})
00244
00245
00246
00247
00248
00249 sm.userdata.sm_hri_tts_farewell = "Si algu' vol entrar per saber me's coses ser'a benvingut. Espero veure-us aviat. Adeu a tothom"
00250 sm.userdata.sm_hri_head_farewell = "head_farewell.xml"
00251 sm.userdata.sm_hri_left_farewell= "left_arm_farewell.xml"
00252 smach.StateMachine.add('FAREWELL',
00253 smach_ros.SimpleActionState('hri',
00254 sequenceAction,
00255 goal_cb=hri_goal_cb,
00256 input_keys=['tts_in', 'head_in','left_in']),
00257 transitions={'succeeded':'succeeded'},
00258 remapping={'tts_in':'sm_hri_tts_farewell',
00259 'head_in':'sm_hri_head_farewell',
00260 'left_in':'sm_hri_left_farewell'})
00261
00262
00263
00264 sm.userdata.sm_hri_tts_blocked = "Sergi, Fernandoo, Anaiis, no em puc moure, ajudeu-me"
00265 sm.userdata.sm_hri_head_blocked = "head_farewell.xml"
00266 sm.userdata.sm_hri_left_blocked= "left_arm_random_4.xml"
00267 smach.StateMachine.add('HRI_BLOCKED',
00268 smach_ros.SimpleActionState('hri',
00269 sequenceAction,
00270 goal_cb=hri_goal_cb,
00271 input_keys=['tts_in', 'head_in','left_in']),
00272 transitions={'succeeded':'succeeded'},
00273 remapping={'tts_in':'sm_hri_tts_blocked',
00274 'head_in':'sm_hri_head_blocked',
00275 'left_in':'sm_hri_left_blocked'})
00276
00277
00278 sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00279 sis.start()
00280
00281
00282 outcome = sm.execute()
00283
00284
00285 rospy.spin()
00286 sis.stop()
00287
00288
00289 if __name__ == '__main__':
00290 main()
00291