def __init__(self, minConfidence=90):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['name'],
output_keys=['standard_error', 'face'])
with self:
self.userdata.face = ""
# extract the database that the robot is finding
smach.StateMachine.add('detect_face',
detect_face(minConfidence),
transitions={
'succeeded': 'proces_face',
'aborted': 'aborted',
'preempted': 'preempted'
})
# i filter a little bit
smach.StateMachine.add('proces_face',
proces_face(),
transitions={
'succeeded': 'succeeded',
'aborted': 'time_sleeper',
'preempted': 'preempted'
})
smach.StateMachine.add('time_sleeper',
Sleeper(0.5),
transitions={
'succeeded': 'detect_face',
'aborted': 'detect_face'
})
def main():
rospy.init_node('Conc_time_node')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.sleep_time = 10
STATES = [Sleeper(30)]
STATE_NAMES = ["sleep"]
INPUTS = ["sleep_time"]
smach.StateMachine.add('Conc_time',
ConcurrenceTime(state_name=STATE_NAMES,
states=STATES,
inputs=INPUTS,
timeout=5),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'time_ends': 'say_time_out'
})
# Say TimeOut
smach.StateMachine.add('say_time_out',
text_to_say("The time is finish"),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm.execute()
rospy.spin()
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['pose_current'])
with self:
# Calculate the actual position
smach.StateMachine.add(
'get_actual_pos',
get_current_robot_pose(),
transitions={'succeeded': 'save_robot_position', 'aborted': 'get_actual_pos', 'preempted': 'succeeded'})
# Save position
smach.StateMachine.add(
'save_robot_position',
save_robot_position(),
transitions={'succeeded': 'sleep_state', 'aborted': 'save_robot_position', 'preempted':'preempted'})
# Sleep
smach.StateMachine.add(
'sleep_state',
Sleeper(2),
transitions={'succeeded': 'check_button', 'aborted': 'check_button', 'preempted':'preempted'})
# Check if the button is pressed
smach.StateMachine.add(
'check_button',
emergency_button(),
transitions= {'succeeded':'succeeded', 'aborted':'get_actual_pos', 'preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted', 'end'],
input_keys=[],
output_keys=[])
with self:
self.userdata.pressed = False
self.userdata.press = False
self.userdata.tts_time_before_speaking = 0
self.userdata.tts_text = ""
self.userdata.tts_lang = ""
# Check if the button is pressed
smach.StateMachine.add(
'check_button',
emergency_button(),
transitions= {'succeeded':'check_button_pressed', 'aborted':'check_button', 'preempted':'preempted'})
#Cas 1 - Crec que es perdra tota l'estona si esta premut o no
# transitions= {'succeeded':'check_button_pressed', 'aborted':'aborted', 'preempted':'preempted'})
smach.StateMachine.add(
'check_button_pressed',
check_button_pressed(),
transitions= {'pressed':'say_pressed_button', 'unpressed':'say_not_pressed_button',
'succeeded':'sleep_state', 'preempted':'preempted'})
# Sleep
smach.StateMachine.add(
'sleep_state',
Sleeper(2),
transitions={'succeeded': 'check_button', 'aborted': 'check_button', 'preempted':'preempted'})
# Say button pressed
smach.StateMachine.add(
'say_pressed_button',
text_to_say("Oh you pressed my emergency button, feel free to check me around"),
transitions= {'succeeded':'check_button', 'aborted':'check_button', 'preempted':'preempted'})
# Say button is not pressed
smach.StateMachine.add(
'say_not_pressed_button',
text_to_say("Thank you for unpressing my emergency button. I'll recover my status shortly."),
transitions= {'succeeded':'end', 'aborted':'end', 'preempted':'preempted'})
def main():
"""
Test the timeout and the sleeper
"""
rospy.init_node('basic_functionalities')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.wait_time = None
sm.userdata.sleep_time = None
sm.userdata.standard_error = ''
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['wait_time', 'sleep_time'],
output_keys=['standard_error'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
smach.Concurrence.add('Sleep', Sleeper(5))
smach.Concurrence.add('TimeOut', TimeOut(15))
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'succeeded',
'aborted': 'Concurrence'
})
sm.execute()
def __init__(self, keyword=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['keyword_name'],
output_keys=[])
with self:
smach.StateMachine.add('PrepareData',
prepareData(keyword),
transitions={
'succeeded': 'Activate_keyword',
'aborted': 'aborted'
})
# Call service state
@smach.cb_interface(input_keys=['keyword_name'])
def AsrServerActivateKeyword_cb(userdata, request):
rospy.loginfo("Activating Keyword " + userdata.keyword_name)
requ = ASRSrvRequest()
requ.requests = [ASRSrvRequest.KWSPOTTING]
requ.model.action = ASRLangModelMngmt.ENABLE
requ.model.modelName = copy.deepcopy(userdata.keyword_name)
rospy.logwarn(
"AsrServerActivateKeyword_cb sending keyword: \n" +
str(requ.model.modelName))
return requ
def AsrServerActivateKeyword_response_cb(userdata, response):
rospy.logwarn("response of Activate_keyword is: \n" +
str(response))
return 'succeeded'
smach.StateMachine.add(
'Activate_keyword',
ServiceState('/asr_service',
ASRService,
request_cb=AsrServerActivateKeyword_cb,
response_cb=AsrServerActivateKeyword_response_cb,
input_keys=['keyword_name']),
transitions={
'succeeded': 'Sleeper',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add("Sleeper",
Sleeper(0.1),
transitions={
'succeeded': 'Activate_Asr',
'aborted': 'aborted'
})
# Call service state
@smach.cb_interface(input_keys=['keyword_name'])
def AsrServerRequestActivate_cb(userdata, request):
rospy.loginfo("Activating Asr service")
requ = ASRSrvRequest()
requ.requests = [
ASRSrvRequest.KWSPOTTING, ASRSrvRequest.ACTIVATION
]
requ.activation.action = ASRActivation.ACTIVATE
requ.model.action = ASRLangModelMngmt.ENABLE
requ.model.modelName = copy.deepcopy(userdata.keyword_name)
rospy.logwarn(
"AsrServerRequestActivate_cb activating keyword: " +
str(requ.model.modelName))
return requ
def AsrServerRequestActivate_response_cb(userdata, response):
rospy.logwarn("response of Activate_Asr is: \n" +
str(response))
return 'succeeded'
smach.StateMachine.add(
'Activate_Asr',
ServiceState('/asr_service',
ASRService,
request_cb=AsrServerRequestActivate_cb,
response_cb=AsrServerRequestActivate_response_cb,
input_keys=['keyword_name']),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self, poi_name=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['nav_to_poi_name'],
output_keys=['standard_error'])
with self:
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = None
self.userdata.tts_lang = None
self.userdata.manip_motion_to_play = None
self.userdata.manip_time_to_play = None
self.userdata.skip_planning = False
smach.StateMachine.add('PrepareData',
prepareData(poi_name),
transitions={
'succeeded': 'check_can_pass',
'aborted': 'aborted'
})
# Check door state
smach.StateMachine.add('check_can_pass',
check_door_status(),
transitions={
'succeeded': 'home_position',
'aborted': 'say_open_door',
'door_too_far': 'say_too_far_from_door'
})
# Robot is too far from door
smach.StateMachine.add('say_too_far_from_door',
text_to_say("I'm too far from the door."),
transitions={
'succeeded': 'approach_to_door',
'aborted': 'approach_to_door'
})
# Approach to the door
smach.StateMachine.add('approach_to_door',
nav_to_coord("/base_link"),
transitions={
'succeeded': 'check_can_pass',
'aborted': 'check_can_pass'
})
# Robot ask to open the door
smach.StateMachine.add(
'say_open_door',
text_to_say("Can anyone open the door please?"),
transitions={
'succeeded': 'sleep_state',
'aborted': 'sleep_state'
})
# Sleep time before speak
smach.StateMachine.add('sleep_state',
Sleeper(5),
transitions={
'succeeded': 'check_can_pass',
'aborted': 'check_can_pass'
})
# Home position
smach.StateMachine.add('home_position',
play_motion_sm('home'),
transitions={
'succeeded': 'say_enter_room',
'aborted': 'say_enter_room',
'preempted': 'succeeded'
})
# We don't need to prepare the state, it takes the input_key directly
# Robot announces it is going through the door
smach.StateMachine.add('say_enter_room',
text_to_say("I am going to enter the room"),
transitions={
'succeeded': 'pass_door',
'aborted': 'pass_door'
})
#We pass the door
smach.StateMachine.add('pass_door',
pass_door(),
transitions={
'succeeded': 'enter_room',
'aborted': 'pass_door',
'preempted': 'preempted'
})
# Go to the poi in the other site of the door
smach.StateMachine.add('enter_room',
nav_to_poi(),
transitions={
'succeeded': 'succeeded',
'aborted': 'enter_room',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
self.userdata.tts_time_before_speaking = 0
self.userdata.tts_text = ""
self.userdata.tts_lang = ""
# Indicate that we are ready
smach.StateMachine.add(
'say_ready_inspection',
text_to_say("I'm ready for Robot Inspection test"),
transitions= {'succeeded':'enter_start_door', 'aborted':'enter_start_door', 'preempted':'preempted'})
# Cross start door and go to intermediate point
smach.StateMachine.add(
'enter_start_door',
EnterRoomSM('intermediate'),
transitions={'succeeded': 'robot_presentation', 'aborted': 'aborted',
'preempted': 'preempted'})
# Robot presentation: Little talk + wave gesture
STATES = [text_to_say("Hi everybody! My name is REEM."), play_motion_sm("wave")]
STATE_NAMES = ["say_presentation", "salute_wave"]
outcome_map = {'succeeded': {"say_presentation": 'succeeded', "salute_wave": 'succeeded'}}
smach.StateMachine.add(
"robot_presentation",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'home_position', 'aborted': "robot_presentation"})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm(motion='home', skip_planning=True),
transitions={'succeeded': 'wait_inspection', 'aborted': 'home_position', 'preempted': 'succeeded'})
# Sleep for inspection
smach.StateMachine.add(
'wait_inspection',
Sleeper(5),
transitions={'succeeded': 'say_end_time_inspection', 'aborted': 'say_end_time_inspection', 'preempted': 'succeeded'})
# Indicate that we are ready to go out
smach.StateMachine.add(
'say_end_time_inspection',
text_to_say("I hope you are happy with the inspection. I'll leave the room."),
transitions= {'succeeded':'go_exit', 'aborted':'go_exit', 'preempted':'preempted'})
# Go to the exit
smach.StateMachine.add(
'go_exit',
GoToExit('exit_door'),
transitions= {'succeeded':'set_robot_position', 'aborted':'set_robot_position', 'preempted':'preempted'})
# Indicate that we are ready to go out
# smach.StateMachine.add(
# 'say_save_pos',
# text_to_say("Saving position"),
# transitions= {'succeeded':'set_robot_position', 'aborted':'set_robot_position', 'preempted':'preempted'})
#
# Set position
smach.StateMachine.add(
'set_robot_position',
set_robot_position(),
transitions={'succeeded': 'cross_door_out', 'aborted': 'set_robot_position',
'preempted': 'preempted'})
# Go to the exit door and cross exit door
# If the door is open change EnterRoom for nav_to_poi
smach.StateMachine.add(
'cross_door_out',
nav_to_poi('exit_door'),
transitions={'succeeded': 'say_exit', 'aborted': 'cross_door_out',
'preempted': 'preempted'})
# Indicate that we are ready to go
smach.StateMachine.add(
'say_exit',
text_to_say("I arrived successfully to the exit location, Robot Inspection Test complete."),
transitions= {'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
def __init__(self, minConfidence=90, time_out=0):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['name'],
output_keys=['standard_error', 'face', 'face_frame'])
with self:
self.userdata.face = ""
self.userdata.name = ""
# extract the database that the robot is finding
self.time_init = rospy.Time.now()
smach.StateMachine.add('init_time',
init_time(),
transitions={
'succeeded': 'detect_face',
'aborted': 'aborted'
})
smach.StateMachine.add('detect_face',
detect_face(minConfidence, time_topic=10),
transitions={
'succeeded': 'Process_face',
'aborted': 'check_time',
'preempted': 'preempted'
})
#Change succeeded:proces_face
# i filter a little bit
smach.StateMachine.add('Process_face',
Process_face(),
transitions={
'succeeded': 'Aborting_navigation',
'aborted': 'time_sleeper',
'preempted': 'preempted'
})
smach.StateMachine.add('Aborting_navigation',
CancelNavigation(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('time_sleeper',
Sleeper(0.5),
transitions={
'succeeded': 'check_time',
'aborted': 'detect_face'
})
smach.StateMachine.add('check_time',
check_time(time_out),
transitions={
'succeeded': 'detect_face',
'aborted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name = ''
# Indicate that we are ready
smach.StateMachine.add(
'say_ready_inspection',
text_to_say("I'm ready for Robot Inspection test"),
transitions={
'succeeded': 'enter_start_door',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Cross start door and go to intermediate point
smach.StateMachine.add('enter_start_door',
EnterRoomSM('intermediate'),
transitions={
'succeeded': 'robot_presentation',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Robot presentation: Little talk + wave gesture
STATES = [
text_to_say("Hi everybody! My name is REEM."),
play_motion_sm("wave", 10)
]
STATE_NAMES = ["say_presentation", "salute_wave"]
outcome_map = {
'succeeded': {
"say_presentation": 'succeeded',
"salute_wave": 'succeeded'
}
}
smach.StateMachine.add("robot_presentation",
ConcurrenceRobocup(states=STATES,
state_names=STATE_NAMES,
outcome_map=outcome_map),
transitions={
'succeeded': 'home_position',
'aborted': "aborted"
})
# Home position
smach.StateMachine.add('home_position',
play_motion_sm('home', 10),
transitions={
'succeeded': 'get_actual_pos',
'aborted': 'aborted',
'preempted': 'succeeded'
})
# Calculate the actual position
smach.StateMachine.add('get_actual_pos',
get_current_robot_pose(),
transitions={
'succeeded': 'wait_time',
'aborted': 'aborted',
'preempted': 'succeeded'
})
# Save position
smach.StateMachine.add('save_robot_position',
save_robot_position(),
transitions={
'succeeded': 'wait_time',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Test of robot
smach.StateMachine.add('wait_time',
Sleeper(20),
transitions={
'succeeded': 'end_time_inspection',
'aborted': 'aborted'
})
# Indicate that we are ready
smach.StateMachine.add('end_time_inspection',
text_to_say("Time finished"),
transitions={
'succeeded': 'set_robot_position',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Set position
smach.StateMachine.add('set_robot_position',
set_robot_position(),
transitions={
'succeeded': 'cross_door_out',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to the exit door and cross exit door
# If the door is open change EnterRoom for nav_to_poi
smach.StateMachine.add('cross_door_out',
nav_to_poi('exit_door'),
transitions={
'succeeded': 'succeeded',
'aborted': 'cross_door_out',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
# Robot presentation: Little talk + wave gesture
STATES = [
text_to_say("Hi everybody! My name is REEM."),
play_motion_sm("wave")
]
STATE_NAMES = ["say_presentation", "salute_wave"]
outcome_map = {
'succeeded': {
"say_presentation": 'succeeded',
"salute_wave": 'succeeded'
}
}
smach.StateMachine.add("robot_presentation",
ConcurrenceRobocup(states=STATES,
state_names=STATE_NAMES,
outcome_map=outcome_map),
transitions={
'succeeded': 'object_recognition',
'aborted': "robot_presentation"
})
# Do object_recognition
smach.StateMachine.add('object_recognition',
dummy_recognize(),
transitions={
'succeeded': 'say_grasp_object',
'aborted': 'say_grasp_object',
'preempted': 'preempted'
})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I am hungry. I am going to pick the noodles"),
transitions={
'succeeded': 'grasp_object',
'aborted': 'grasp_object'
})
# Grasp the object
smach.StateMachine.add('grasp_object',
pick_object_sm(),
transitions={
'succeeded': 'say_why_object',
'aborted': 'play_motion_state',
'preempted': 'preempted'
})
# Say release object
smach.StateMachine.add(
'say_why_object',
text_to_say(
"I am a robot, I can't eat this. I am so sad! I am going to leave the noodles in the table",
wait=False),
transitions={
'succeeded': 'release_object',
'aborted': 'release_object'
})
# Release the object
smach.StateMachine.add('release_object',
place_object_sm(),
transitions={
'succeeded': 'robot_finish',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Say hard job + bow
STATES = [
text_to_say(
"Uff, this was a hard job. Thank you very much for your attention"
),
play_motion_sm("bow")
]
STATE_NAMES = ["say_hard_job", "motion_bow"]
outcome_map = {
'succeeded': {
"say_hard_job": 'succeeded',
"motion_bow": 'succeeded'
}
}
smach.StateMachine.add("robot_finish",
ConcurrenceRobocup(states=STATES,
state_names=STATE_NAMES,
outcome_map=outcome_map),
transitions={
'succeeded': 'say_rest',
'aborted': "say_rest"
})
# Say go rest
smach.StateMachine.add('say_rest',
text_to_say(
"I am going to rest for a few seconds",
wait=False),
transitions={
'succeeded': 'sleep_robot',
'aborted': 'sleep_robot'
})
# Sleep
smach.StateMachine.add('sleep_robot',
Sleeper(15),
transitions={
'succeeded': 'robot_presentation',
'preempted': 'robot_presentation',
'aborted': 'robot_presentation'
})
# Home position
smach.StateMachine.add('play_motion_state',
play_motion_sm('home'),
transitions={
'succeeded': 'say_grasp_object',
'preempted': 'say_grasp_object',
'aborted': 'say_grasp_object'
})
def __init__(self, grammar=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['grammar_name'],
output_keys=[])
with self:
smach.StateMachine.add('PrepareData',
prepareData(grammar),
transitions={
'succeeded': 'Activate_Gram',
'aborted': 'aborted'
})
# Call service state
@smach.cb_interface(input_keys=['grammar_name'])
def AsrServerActivateGram_cb(userdata, request):
rospy.loginfo("Activating Grammar " + userdata.grammar_name +
" and Language")
requ = ASRSrvRequest()
requ.requests = [ASRSrvRequest.GRAMMAR, ASRSrvRequest.LANGUAGE]
requ.model.action = ASRLangModelMngmt.ENABLE
requ.model.modelName = copy.deepcopy(userdata.grammar_name)
requ.lang.language = 'en_US'
rospy.logwarn("AsrServerActivateGram_cb sending grammar: \n" +
str(requ.model.modelName))
return requ
def AsrServerActivateGram_response_cb(userdata, response):
rospy.logwarn("response of Activate_Gram is: \n" +
str(response))
return 'succeeded'
smach.StateMachine.add(
'Activate_Gram',
ServiceState('/asr_service',
ASRService,
request_cb=AsrServerActivateGram_cb,
response_cb=AsrServerActivateGram_response_cb,
input_keys=['grammar_name']),
transitions={
'succeeded': 'Sleeper',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add("Sleeper",
Sleeper(0.1),
transitions={
'succeeded': 'Activate_Asr',
'aborted': 'aborted'
})
# Call service state
@smach.cb_interface(input_keys=['grammar_name'])
def AsrServerRequestActivate_cb(userdata, request):
rospy.loginfo("Activating Asr service")
requ = ASRSrvRequest()
requ.requests = [
ASRSrvRequest.GRAMMAR, ASRSrvRequest.ACTIVATION
]
requ.activation.action = ASRActivation.ACTIVATE
requ.model.action = ASRLangModelMngmt.ENABLE
requ.model.modelName = copy.deepcopy(userdata.grammar_name)
rospy.logwarn(
"AsrServerRequestActivate_cb activating grammar: " +
str(requ.model.modelName))
return requ
def AsrServerRequestActivate_response_cb(userdata, response):
rospy.logwarn("response of Activate_Asr is: \n" +
str(response))
return 'succeeded'
smach.StateMachine.add(
'Activate_Asr',
ServiceState('/asr_service',
ASRService,
request_cb=AsrServerRequestActivate_cb,
response_cb=AsrServerRequestActivate_response_cb,
input_keys=['grammar_name']),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})