def getInstance(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preemted'])
with sm:
smach.StateMachine.add('SETUP',
Setup(robot),
transitions={'succeeded': 'INSTRUCTIONS_QR'})
smach.StateMachine.add(
'INSTRUCTIONS_QR',
Speak(robot, "Cuando se le indique levante medallon con q r "),
transitions={'succeeded': 'EXAMPLE1'})
smach.StateMachine.add(
'EXAMPLE1',
QRDetector(robot),
transitions={'succeeded': 'INSTRUCTIONS_ANSWER'})
smach.StateMachine.add(
'INSTRUCTIONS_ANSWER',
Speak(
robot,
"Ahora se probara seleccion por brazo. Levante brazos para responder cuando se indique"
),
transitions={'succeeded': 'EXAMPLE2'})
smach.StateMachine.add('EXAMPLE2',
AnswerSelection(robot),
transitions={'succeeded': 'FINISH'})
smach.StateMachine.add('FINISH',
Speak(robot, "Muchas Gracias"),
transitions={'succeeded': 'succeeded'})
return sm
def build_get_order_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
output_keys=['human'])
ltm.register_state(sm, ["get_order"])
wait_face_sm = WaitForButton(robot)
approach_sm = WaitForButton(robot)
ltm.register_state(approach_sm, ["wait_face"])
ltm.register_state(approach_sm, ["approach_human"])
with sm:
smach.StateMachine.add('WAIT_FACE',
wait_face_sm,
transitions={'succeeded': 'APPROACH_TO_HUMAN'})
smach.StateMachine.add('APPROACH_TO_HUMAN',
approach_sm,
transitions={'succeeded': 'SAY_HELLO'})
smach.StateMachine.add('SAY_HELLO',
Speak(robot,
text="Hello, " + Data.NAME +
" i will prepare a coffee for you."),
transitions={'succeeded': 'RECORD_HUMAN'})
smach.StateMachine.add('RECORD_HUMAN',
RecordHuman(),
transitions={'succeeded': 'succeeded'})
return sm
def getInstance(robot):
sm =smach.StateMachine(outcomes=['succeeded','aborted','preemted'])
with sm:
smach.StateMachine.add('SETUP',Setup(robot),
transitions={
'succeeded':'INSTRUCTIONS'
}
)
smach.StateMachine.add('INSTRUCTIONS',Speak(robot,"Cuando se le indique seleccione alternativa. Levante brazo izquierdo para letra a. Levante brazo derecho para letra b. Levante ambos para letra c. "),
transitions={
'succeeded':'EXAMPLE1'
}
)
smach.StateMachine.add('EXAMPLE1',Example(robot),
transitions={
'succeeded':'EXAMPLE2'
}
)
smach.StateMachine.add('EXAMPLE2',Example(robot),
transitions={
'succeeded':'succeeded'
}
)
return sm
def build_facial_features_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['human'],
output_keys=['human'])
sm.userdata.features = ['gender', 'age']
ltm.register_state(sm, ["facial_analysis"])
with sm:
smach.StateMachine.add('INIT',
Speak(robot,
text="Please, look me in the eyes ..."),
transitions={'succeeded': 'GET_AGE_AND_GENRE'})
age_gender_sm = WaitForButton(robot, 30.0)
ltm.register_state(age_gender_sm,
["age_estimation", "gender_estimation"])
smach.StateMachine.add(
'GET_AGE_AND_GENRE',
age_gender_sm,
transitions={'succeeded': 'RECORD_AGE_AND_GENRE'})
smach.StateMachine.add('RECORD_AGE_AND_GENRE',
RecordHumanAgeAndGenre(),
transitions={'succeeded': 'GET_EMOTION'})
emotion_sm = WaitForButton(robot, 30.0)
ltm.register_state(emotion_sm, ["emotion_estimation"])
smach.StateMachine.add('GET_EMOTION',
emotion_sm,
transitions={'succeeded': 'RECORD_EMOTION'})
smach.StateMachine.add('RECORD_EMOTION',
RecordHumanEmotion(),
transitions={'succeeded': 'succeeded'})
return sm
def execute(self,userdata):
## Aca debe ir el llamado a la funcion del felipe que guarda la info
rospy.loginfo(userdata.actual_question[0]+userdata.recognized_sentence)
Speak(self.robot,"Hola Escuche {}".format(userdata.recognized_sentence))
tts=self.robot.get("tts")
tts.say("Hola Escuche {}".format(userdata.recognized_sentence))
userdata.animal_info[userdata.actual_question[0]]=userdata.recognized_sentence
return "succeeded"
def execute(self, userdata):
Speak(self.robot,
"Oh lo siento no te escuche bien puedes repetir porfavor")
if self.it < self.max_tries:
self.it += 1
return "preempted"
else:
rospy.loginfo("Demasiados intentos")
return "aborted"
def build_introduction_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
output_keys=['human'])
ltm.register_state(sm, ["hri_introduction"])
with sm:
smach.StateMachine.add('SAY_HELLO',
Speak(robot, text="Hello, i am bender ..."),
transitions={'succeeded': 'ASK_NAME'})
smach.StateMachine.add('ASK_NAME',
Speak(robot,
text="Nice to meet you " + Data.NAME),
transitions={'succeeded': 'RECORD_HUMAN'})
smach.StateMachine.add('RECORD_HUMAN',
RecordHuman(),
transitions={'succeeded': 'succeeded'})
return sm
def execute(self,userdata):
self.tts.say("Oh, lo siento no te escuche bien, puedes repetir por favor?")
Speak(self.robot,"Oh, lo siento no te escuche bien puedes repetir por favor?")
self.robot.tts.wait_until_done()
if self.it<self.max_tries:
self.it+=1
return "preempted"
else:
self.tts.say("Oh, lo siento no te escuche bien, pasemos a la siguiente fase?")
rospy.loginfo("Demasiados intentos")
return "aborted"
def getInstance(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
sm.userdata.crowd_location = "kitchen"
sm.userdata.features = ['gender', 'age']
with sm:
smach.StateMachine.add('GREET',
Speak(robot, text="Hi friends!", gestures=True),
transitions={'succeeded': 'LOOKCROWD'})
smach.StateMachine.add('LOOKCROWD',
CodeRunner('rospy.sleep(1)'),
transitions={'succeeded': 'CROWD_INFORMATION'})
smach.StateMachine.add('CROWD_INFORMATION',
CrowdInformationSimple.getInstance(
robot, map_filter=False, waving=WAVING),
transitions={
'succeeded': 'PUBLISH_INFORMATION',
'aborted': 'aborted',
'failed': 'aborted'
})
smach.StateMachine.add('PUBLISH_INFORMATION',
PublishInformation(robot),
transitions={'succeeded': 'FINAL_SPEECH'})
smach.StateMachine.add(
'FINAL_SPEECH',
Speak(robot,
text="thanks!, now i always remember all of you",
gestures=True),
transitions={'succeeded': 'succeeded'})
# initial_data = smach.UserData()
# sm.set_initial_state(['SETUP'],initial_data)#
return sm
def getInstance(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preemted'])
with sm:
smach.StateMachine.add('SETUP',
Setup(robot),
transitions={'succeeded': 'INSTRUCTIONS'})
smach.StateMachine.add(
'INSTRUCTIONS',
Speak(robot, "Cuando se le indique levante paleta con q r "),
transitions={'succeeded': 'EXAMPLE1'})
smach.StateMachine.add('EXAMPLE1',
Example(robot),
transitions={'succeeded': 'EXAMPLE2'})
smach.StateMachine.add('EXAMPLE2',
Example(robot),
transitions={'succeeded': 'succeeded'})
return sm
def getInstance(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preemted'])
with sm:
smach.StateMachine.add('SETUP',
Setup(robot),
transitions={'succeeded': 'SELECTION_EXAMPLE'})
smach.StateMachine.add(
'SELECTION_EXAMPLE',
AnswerSelectionWeb(robot),
transitions={'succeeded': 'SAY_ANSWER'},
)
smach.StateMachine.add(
'SAY_ANSWER',
SaySelection(robot),
transitions={'succeeded': 'SELECTION_EXAMPLE'},
)
smach.StateMachine.add('FINISH',
Speak(robot, "Muchas Gracias"),
transitions={'succeeded': 'succeeded'})
return sm
def getInstance(robot):
start_place = 'start'
RECEPTION_PLACE = 'start'
JOHN_PLACE = 'near_couch'
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
sm.userdata.qr = ""
sm.userdata.page = "http://198.18.0.1:8888/stage1/r1"
sm.userdata.base_page = "http://198.18.0.1:8888/"
sm.userdata.qr_id = ""
sm.userdata.question_count = 0
sm.userdata.s3 = [0, 0, 0, 0, 0]
sm.userdata.actualTeam = 0
with sm:
smach.StateMachine.add('SETUP',
Setup(robot),
transitions={'succeeded': 'PRE_WAIT'})
smach.StateMachine.add('PRE_WAIT',
ShowWebpage(robot,
page="http://198.18.0.1:8888/b"),
transitions={
'succeeded': 'WAIT',
'preempted': 'WAIT'
})
smach.StateMachine.add('WAIT',
WaitTouchScreen(robot),
transitions={'succeeded': 'QR'})
smach.StateMachine.add('QR',
QRDetector(robot),
transitions={
'succeeded': 'CHANGE_URL',
'aborted': 'QR'
})
smach.StateMachine.add('CHANGE_URL',
ChangeURL(robot),
transitions={
'stage1': 'SPEAK_1',
'stage2': 'TUTORIAL_2',
'stage3': 'S3CHECK',
'stage0': 'WRONG'
})
smach.StateMachine.add('WRONG',
ShowWebpage(
robot, page="http://198.18.0.1:8888/wrong"),
transitions={
'succeeded': 'SPEAK_WRONG',
'preempted': 'SPEAK_WRONG'
})
smach.StateMachine.add(
'SPEAK_WRONG',
Speak(
robot,
"Haz encontrado el codigo equivocado, debes volver y encontrar el correcto"
),
transitions={'succeeded': 'PRE_WAIT'})
################################################
#STAGE1#
################################################
smach.StateMachine.add('SPEAK_1',
Speak(
robot,
"Estan en la etapa 1, suerte con el juego"),
transitions={'succeeded': 'WEB_SHOW_1'})
smach.StateMachine.add('WEB_SHOW_1',
ShowWebpage(robot),
transitions={'succeeded': 'HEAR_QUESTIONS_1'})
smach.StateMachine.add('HEAR_QUESTIONS_1',
Questions(robot),
transitions={
'succeeded': 'HEAR_QUESTIONS_1',
'preempted': 'PRE_WAIT',
'return3': 'PRE_WAIT',
'final3': 'PRE_WAIT'
})
################################################
#STAGE2#
################################################
smach.StateMachine.add('TUTORIAL_2',
ShowWebpage(
robot,
page="http://198.18.0.1:8888/tutorial"),
transitions={
'succeeded': 'SPEAK_2',
'preempted': 'SPEAK_2'
})
smach.StateMachine.add(
'SPEAK_2',
Speak(
robot,
"Felicitaciones, estan en la etapa 2. En esta parte para seleccionar la respuesta deben presionar el boton y luego escoger con la pose adecuada"
),
transitions={'succeeded': 'WEB_SHOW_2'})
smach.StateMachine.add('WEB_SHOW_2',
ShowWebpage(robot),
transitions={'succeeded': 'HEAR_QUESTIONS_2'})
smach.StateMachine.add('HEAR_QUESTIONS_2',
Questions(robot),
transitions={
'succeeded': 'HEAR_QUESTIONS_2',
'preempted': 'PRE_WAIT',
'return3': 'PRE_WAIT',
'final3': 'PRE_WAIT'
})
################################################
#STAGE3#
################################################
smach.StateMachine.add('S3CHECK',
Stage3Check(robot),
transitions={
'first': 'SPEAK_3_FIRST',
'after': 'SPEAK_3_AFTER'
})
smach.StateMachine.add(
'SPEAK_3_FIRST',
Speak(
robot,
"UWU, lo lograron, ahora solo queda responder una pregunta mas, la qual sera evaluada por su profesora"
),
transitions={'succeeded': 'WEB_SHOW_3'})
smach.StateMachine.add(
'SPEAK_3_AFTER',
Speak(
robot,
"Espero que hayan pensado muy bien su respuesta. La pregunta aparecera nuevamente en pantalla. Profesora por favor indique con los botones si la pregunta es correcta o deben pensarla de nuevo"
),
transitions={'succeeded': 'WEB_SHOW_3'})
smach.StateMachine.add('WEB_SHOW_3',
ShowWebpage(robot),
transitions={'succeeded': 'HEAR_QUESTIONS_3'})
smach.StateMachine.add('HEAR_QUESTIONS_3',
Questions(robot),
transitions={
'succeeded': 'HEAR_QUESTIONS_3',
'preempted': 'PRE_WAIT',
'return3': 'SPEAK_RETURN',
'final3': 'SPEAK_3_LAST'
})
smach.StateMachine.add(
'SPEAK_3_LAST',
Speak(
robot,
"Felicitaciones a su equipo, ahora deben esperar que todos los equipos terminen para poder saber al ganador"
),
transitions={'succeeded': 'PRE_WAIT'})
smach.StateMachine.add('SPEAK_RETURN',
Speak(robot, "Suerte con la pregunta"),
transitions={'succeeded': 'PRE_WAIT'})
return sm
def getInstance(robot):
sm = smach.StateMachine(
outcomes=['succeeded', 'failed', 'aborted', 'preempted'],
input_keys=['name'])
sm.userdata.object = world_definition.Object('coffee', '', '')
sm.userdata.side = 'l'
sm.userdata.effort = 0.8
#sm.userdata.trayectory_name_pre = ['home','premanip_1','premanip_2']
#sm.userdata.trayectory_name_pos = ['premanip_2','premanip_1','home']
#sm.userdata.trayectory_name_posfail = ['premanip_2']
#arms = {'l':robot.get("l_arm"),'r':robot.get('r_arm')}
sm.userdata.name = 'pringles_gazebo'
ps = PoseStamped()
ps.header.frame_id = 'bender/base_link' #'bender/sensors/rgbd_head_rgb_optical_frame'
ps.header.stamp = rospy.Duration(10.0)
ps.pose.position.x, ps.pose.position.y, ps.pose.position.z = 0.55, 0.25, 0.60 # original (0.90,-0.2,-0.55) (0.57,0.2,-0.55)
p = Pose()
p.position.x, p.position.y, p.position.z = 0.63, 0.25, 0.65
sm.userdata.object.posestamped = ps
sm.userdata.object_pose = ps
sm.userdata.selected_pregrasp = [0.0, 0.15, 0.0, 0.0, 0.0, 0.0]
sm.userdata.selected_grasp = [0.0, 0.15, 0.0, 0.0, 0.0, 0.0]
sm.userdata.pos_grasp = [0.279, 0.054, 0.383, 1.776, 0.061, 0.895]
sm.userdata.home_safe = [0.0, 0.15, 0.0, 0.0, 0.0, 0.0]
sm.userdata.it = -1
sm.userdata.recognized_sentence = ""
with sm:
# smach.StateMachine.add('SETUP', Setup(robot),
# transitions={'succeeded' :'LOOKFRONT'}
# )
# smach.StateMachine.add('LOOKFRONT',LookFront(robot),
# transitions={'succeeded':'FACE'}
# )
# smach.StateMachine.add('FACE', FaceLantern(robot),
# transitions={'succeeded' :'INITIAL_SPEECH'}
# )
smach.StateMachine.add('INITIAL_SPEECH',
Speak(robot, text="I am ready"),
transitions={'succeeded': 'HEAR_START'})
smach.StateMachine.add(
'HEAR_START',
Hear(robot, dictionary='Stage1/StoringGroceries/starttest'),
transitions={
'succeeded': 'CLOSE_GRIPPER',
'preempted': 'HEAR_START'
},
remapping={'recognized_sentence': 'recognized_sentence'})
smach.StateMachine.add('CLOSE_GRIPPER',
basic2.CloseBothGrippers(robot),
transitions={
'succeeded': 'OCTOMAP',
'aborted': 'OCTOMAP'
},
remapping={'effort': 'effort'})
smach.StateMachine.add('OCTOMAP',
octomap.getInstance(robot),
transitions={'succeeded': 'IT_OBJECT'})
smach.StateMachine.add('IT_OBJECT',
Iteration(),
transitions={
'succeeded': 'GET_OBJECT',
'continue': 'failed'
})
smach.StateMachine.add('GET_OBJECT',
get_object.getInstance(robot),
transitions={
'succeeded': 'ARM_DESIGNATOR',
'aborted': 'IT_OBJECT',
'failed': 'IT_OBJECT'
},
remapping={
'object_name': 'name',
'object': 'object'
})
smach.StateMachine.add('ARM_DESIGNATOR',
basic.ArmDesignator(robot),
transitions={
'succeeded': 'GET_GRASPS',
'aborted': 'aborted'
},
remapping={
'object': 'object',
'side': 'side'
})
smach.StateMachine.add('GET_GRASPS',
basic2.GetGrasp(robot),
transitions={
'succeeded': 'MANIPULATION',
'aborted': 'aborted'
},
remapping={
'object_pose': 'object_pose',
'selected_pregrasp': 'selected_pregrasp',
'selected_grasp': 'selected_grasp',
'side': 'side'
})
smach.StateMachine.add('LOOK_POSITION',
LookBestPosition(robot),
transitions={
'succeeded': 'GET_GRASPS',
'aborted': 'aborted'
})
smach.StateMachine.add('MANIPULATION',
manipulate.getInstance(robot),
transitions={
'succeeded': 'RESTART_IT',
'aborted': 'aborted'
},
remapping={
'object': 'object',
'side': 'side',
'selected_pregrasp': 'selected_pregrasp',
'selected_grasp': 'selected_grasp',
'pos_grasp': 'pos_grasp'
})
smach.StateMachine.add('RESTART_IT',
RestartIt(),
transitions={'succeeded': 'succeeded'})
return sm
def execute(self, userdata):
Speak(self.robot, "SETUP JUEGO DEL MAPA")
return "succeeded"
def execute(self, userdata):
Speak(self.robot, "FALTA POR IMPLEMENTAR JUEGO DEL MAPA")
return "succeeded"