def call_bring_to(person_name): #TODO #Adding realese and reread tosay with some responsible person
if person_name == '':
tosay = "I'm leaving this here, sorry but you asked for a person without name"
else:
tosay = person_name + " can you pick this up please?"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_bring_to '+person_name)
#############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while(out=='aborted' and tries<3):
person_object_position = PoseStamped()
person_object_position.header.frame_id = "base_link"
person_object_position.pose.position.x = 0.25
person_object_position.pose.position.z = 1.0
person_object_position.pose.orientation.w = 1.0
sm = place_object_sm(person_object_position)
out = sm.execute()
tries = tries+1
#realese here
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def call_bring_to(
person_name
): #TODO #Adding realese and reread tosay with some responsible person
if person_name == '':
tosay = "I'm leaving this here, sorry but you asked for a person without name"
else:
tosay = person_name + " can you pick this up please?"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_bring_to ' + person_name)
#############################################################################
if SKILLS:
if (time.time() - TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while (out == 'aborted' and tries < 3):
person_object_position = PoseStamped()
person_object_position.header.frame_id = "base_link"
person_object_position.pose.position.x = 0.25
person_object_position.pose.position.z = 1.0
person_object_position.pose.orientation.w = 1.0
sm = place_object_sm(person_object_position)
out = sm.execute()
tries = tries + 1
#realese here
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def call_bring_to_loc(location_name): #TODO #Improve toSay, add realese and, may be add some human recognition to avoid throwing stuff to the ground
if location_name == '':
tosay = "I'm leaving this here"
else:
tosay = "I took this item here as requested. Referee I know you are here, if no one else is going to pick this proably you will want to take it before I throw it to the floor, thanks"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_bring_to_loc '+location_name)
#############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
param_name = "/robocup_params/" + location_name.replace(" ","_") + "_heigh"
loc_object_position = PoseStamped()
loc_object_position.header.frame_id = "base_link"
loc_object_position.pose.position.x = 0.5
if rospy.has_param(param_name):
loc_object_position.pose.position.z = rospy.get_param(param_name)
else:
loc_object_position.pose.position.z = 1.25
loc_object_position.pose.orientation.w = 1.0
out = 'aborted'
tries = 0
while(out=='aborted' and tries<3):
sm = place_object_sm(loc_object_position)
out = sm.execute()
tries = tries+1
#realese here
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_array'])
pose_place = PoseStamped()
pose_place.header.frame_id = '/base_link'
pose_place.pose.position.x = 0.90
pose_place.pose.position.y = 0.0
pose_place.pose.position.z = 1.20
pose_place.pose.orientation.w = 1.0
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 = None
self.userdata.standard_error = 'OK'
self.userdata.object_name = ''
self.userdata.object_index = 0
self.userdata.object_name = None
self.userdata.time_out_grasp = TIME_OUT_GRASP
# Process order
smach.StateMachine.add('decide_next_object',
decide_next_object(),
remapping={"object_loc": "nav_to_poi_name"},
transitions={
'succeeded': 'say_im_going',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Deliver object
smach.StateMachine.add('say_im_going',
text_to_say(),
transitions={
'succeeded': 'go_to_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to poi where the object can stay
smach.StateMachine.add('go_to_object',
nav_to_poi(),
transitions={
'succeeded': 'say_grasp_object',
'aborted': 'go_to_object'
})
# Grasp Object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to recognize and grasp the object"),
transitions={
'succeeded': 'recognize_object_and_pick',
'aborted': 'aborted',
'preempted': 'preempted'
})
# smach.StateMachine.add(
# 'Search_and_grasp_Object',
# object_detection_and_grasping_sm(),
# transitions={'succeeded':'prepare_delivery_goal',
# 'fail_object_detection':'say_object_not_find',
# 'fail_object_grasping':'say_not_grasp',
# 'aborted':'aborted',
# 'preempted':'preempted'},
# remapping = {'object_name':'object_to_grasp'})
# Recognize and pick object if found
# Grasp the object
# Grasp the object
smach.StateMachine.add('grasp_object',
pick_object_sm(),
transitions={
'succeeded': 'succeeded',
'aborted': 'say_not_grasp',
'preempted': 'preempted'
})
smach.StateMachine.add('recognize_object_and_pick',
RecObjectAndPick(),
transitions={
'succeeded': 'prepare_delivery_goal',
'fail_grasp': 'say_not_grasp',
'fail_recognize': 'say_object_not_find'
})
# FAIL Grasp Object
smach.StateMachine.add(
'say_not_grasp',
text_to_say("it was impossible to grasp the Object"),
transitions={
'succeeded': 'decide_next_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# FAIL Recognize Object
smach.StateMachine.add(
'say_object_not_find',
text_to_say("it was not possible to find the Object"),
transitions={
'succeeded': 'decide_next_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('prepare_delivery_goal',
prepare_delivery(),
transitions={
'succeeded': 'say_going_deliver_object',
'aborted': 'say_going_deliver_object',
'preempted': 'preempted'
})
# Deliver object
smach.StateMachine.add(
'say_going_deliver_object',
text_to_say("i am going to deliver the object"),
transitions={
'succeeded': 'go_to_delivery',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to the delivery place
smach.StateMachine.add('go_to_delivery',
nav_to_poi(),
transitions={
'succeeded': 'say_deliver_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Deliver object
smach.StateMachine.add(
'say_deliver_object',
text_to_say("I'm going to deliver the object"),
transitions={
'succeeded': 'deliver_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('deliver_object',
place_object_sm(pose_place),
transitions={
'succeeded': 'check_loop',
'aborted': 'check_loop',
'preempted': 'preempted'
})
# End of loop?
smach.StateMachine.add('check_loop',
checkLoop(),
transitions={
'succeeded': 'decide_next_object',
'aborted': 'aborted',
'preempted': 'preempted',
'end': 'succeeded'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
input_keys=['person_location', 'person_location_coord'])
pose_place = PoseStamped()
pose_place.header.frame_id = '/base_link'
pose_place.pose.position.x = 0.0
pose_place.pose.position.y = 0.0
pose_place.pose.position.z = 1.0
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.object_failed = False
self.userdata.object_name = None
smach.StateMachine.add(
'Ask_Question',
text_to_say(text='What would you like me to bring?'),
transitions={'succeeded':'Listen_Question', 'aborted': 'Ask_Question', 'preempted':'Listen_Question'})
smach.StateMachine.add(
'Listen_Question',
ListenToSM(grammar='robocup/emergency'),
transitions={'succeeded':'Process_Tags', 'aborted':'Ask_Question', 'preempted':'Ask_Question'})
# Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked
# Input keys: actiontag[] 'asr_userSaid_tags'
# Output keys: object
smach.StateMachine.add(
'Process_Tags',
Process_Tags(),
transitions={'succeeded':'Search_Object', 'aborted':'Ask_Question', 'aborted':'Ask_Question'})
smach.StateMachine.add(
'Search_Object',
object_detection_and_grasping_sm(),
transitions={'succeeded':'Say_return_Person',
'fail_object_detection':'Grasp_failed_prepare',
'fail_object_grasping':'Grasp_failed_prepare',
'aborted':'aborted',
'preempted':'preempted'},
remapping = {'object_name':'object_to_grasp'})
smach.StateMachine.add(
'Grasp_failed_prepare',
Fail_Detection_Grasping(),
transitions={'succeeded':'Grasp_fail_Ask_Person'})
# TODO: Saying to where to robot is heading.
# smach.StateMachine.add(
# 'Say_go_Place',
# text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'),
# transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'})
# smach.StateMachine.add(
# 'Go_To_Object_Place',
# nav_to_poi(),
# transitions={'succeeded':'Say_got_to_Kitchen', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'})
# smach.StateMachine.add(
# 'Say_got_to_Kitchen',
# text_to_say('I am in the Kitchen, I am going to grasp fail ask person'),
# transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person'})
#
# self.userdata.time_grasp = 0.0
# smach.StateMachine.add('Grasping_with_timeout',
# grasping_with_timeout(),
# transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'})
smach.StateMachine.add(
'Grasp_fail_Ask_Person',
ask_give_object_grasping(),
transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'})
smach.StateMachine.add(
'Rest_arm',
play_motion_sm('rest_object_right'),
transitions={'succeeded':'Say_return_Person', 'aborted':'Say_return_Person', 'preempted':'Say_return_Person'})
smach.StateMachine.add(
'Say_return_Person',
text_to_say('I am preparing to go back to the person'),
transitions={'succeeded':'Prepare_Go_To_Person', 'aborted':'Prepare_Go_To_Person', 'aborted':'Prepare_Go_To_Person'})
#Go to person
smach.StateMachine.add(
'Prepare_Go_To_Person',
prepare_poi_person_emergency(),
transitions={'succeeded':'Go_To_Person', 'aborted':'Go_To_Person', 'preempted':'Go_To_Person'})
#TODO: POI For Person in Emergency -- From SearchPeople SM -
smach.StateMachine.add(
'Go_To_Person',
nav_to_coord('/map'),
transitions={'succeeded':'Say_Give_Object', 'aborted':'Say_Give_Object', 'preempted':'Say_Give_Object'})
smach.StateMachine.add(
'Say_Give_Object',
text_to_say('I am going to give you the Object you asked.'),
transitions={'succeeded':'Select_next_state_grasping',
'aborted':'Select_next_state_grasping',
'preempted':'Select_next_state_grasping'})
smach.StateMachine.add(
'Select_next_state_grasping',
Select_Grasp_Next_State(),
transitions={'state_failed':'Give_object_arm', 'state_succeeded':'Give_object_both'})
smach.StateMachine.add(
'Give_object_both',
place_object_sm(pose_place),
transitions={'succeeded':'Say_Rescue_stay',
'aborted':'Say_Rescue_stay',
'preempted':'preempted'})
smach.StateMachine.add(
'Give_object_arm',
play_motion_sm('give_object_right'),
transitions={'succeeded':'Give_Object', 'aborted':'Give_Object', 'preempted':'Give_Object'})
#Give the grabbed object to the person
smach.StateMachine.add(
'Give_Object',
move_hands_form(hand_pose_name='pre_grasp', hand_side='right'),
transitions={'succeeded':'Give_Object_2', 'aborted':'Give_Object', 'preempted':'Give_Object'})
smach.StateMachine.add(
'Give_Object_2',
move_hands_form(hand_pose_name='full_open', hand_side='right'),
transitions={'succeeded':'Say_Rescue_stay', 'aborted':'Give_Object_2', 'preempted':'Give_Object_2'})
smach.StateMachine.add(
'Say_Rescue_stay',
text_to_say('Please Stay here I am going to call for the Ambulance'),
transitions={'succeeded':'succeeded', 'aborted':'aborted', 'aborted':'preempted'})
def call_bring_to_loc(
location_name
): #TODO #Improve toSay, add realese and, may be add some human recognition to avoid throwing stuff to the ground
# if location_name == '':
tosay = "I'm leaving this here"
# else:
# tosay = "I took this item here as requested. Referee I know you are here, if no one else is going to pick this proably you will want to take it before I throw it to the floor, thanks"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_bring_to_loc ' + location_name)
#############################################################################
if SKILLS:
if (time.time() - TIME_INIT) > 270:
return "succeeded"
#param_place_high_name = "/robocup_params/" + location_name.replace(" ","_") + "_heigh"
param_place_high_name = "/robocup_params/place/" + location_name.replace(
" ", "_")
loc_object_position = PoseStamped() #PoseWithCovarianceStamped()
loc_object_position.header.frame_id = "base_link"
loc_object_position.pose.position.z = 1.25
loc_object_position.pose.position.x = 0.25
loc_object_position.pose.orientation.w = 1.0
current_position = world.get_current_position()
rospy.logwarn(current_position)
rospy.logwarn(ROOMS)
if current_position == "kitchen":
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/bar")[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/bar")[2]
call_go_to("bar", world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "living room":
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/dinner_table")[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/dinner_table")[2]
call_go_to("dinner table", world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "hallway":
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/hallway_table")[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/hallway_table")[2]
call_go_to("hallway table", world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "bedroom":
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/bed")[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/bed")[2]
call_go_to("bed", world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position.replace("_", " ") in TABLES:
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/" +
current_position.replace("_", " "))[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/" +
current_position.replace("_", " "))[2]
sm = place_object_sm(loc_object_position)
out = sm.execute()
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
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_lang = ''
self.userdata.tts_wait_before_speak = ''
self.userdata.tts_text = ''
# Say start Pick and Place
smach.StateMachine.add(
'say_start_pick_place',
text_to_say("I'm going to start Pick and Place test"),
transitions={'succeeded': 'say_start_obj_recognition', 'aborted': 'say_start_obj_recognition'})
# Say Going to location
# smach.StateMachine.add(
# 'say_go_location',
# text_to_say("I'm going to the location where I have to recognize some objects",wait=False),
# transitions={'succeeded': 'prepare_location', 'aborted': 'prepare_location'})
#
# # Prepare the poi for nav_to_poi
# smach.StateMachine.add(
# 'prepare_location',
# prepare_location(),
# transitions={'succeeded': 'go_location', 'aborted': 'prepare_location',
# 'preempted': 'preempted'})
#
# # Go to the location
# smach.StateMachine.add(
# 'go_location',
# nav_to_poi(),
# transitions={'succeeded': 'say_start_obj_recognition', 'aborted': 'say_go_location',
# 'preempted': 'preempted'})
# Say start object recognition
smach.StateMachine.add(
'say_start_obj_recognition',
text_to_say("I'm going to start the Object recognition"),
transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'})
# Do object_recognition
smach.StateMachine.add(
'object_recognition',
dummy_recognize(),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_release_obj',
'preempted': 'preempted'})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to grasp the object"),
transitions={'succeeded': 'grasp_object', 'aborted': 'grasp_object'})
# Grasp the object
smach.StateMachine.add(
'grasp_object',
pick_object_sm(),
transitions={'succeeded': 'say_go_second_location', 'aborted': 'play_motion_grasp',
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'play_motion_grasp',
play_motion_sm('home'),
transitions={'succeeded': 'say_grasp_object', 'preempted':'say_grasp_object',
'aborted':'play_motion_grasp'})
# Say go to second location
smach.StateMachine.add(
'say_go_second_location',
text_to_say("I'm going to the location where I should release the object"),
transitions={'succeeded': 'go_second_location', 'aborted': 'go_second_location'})
# Go the location - We need to go to the place to object category, so we assume that the
# object recognition will init the poi to the object must to go
smach.StateMachine.add(
'go_second_location',
nav_to_poi(),
transitions={'succeeded': 'say_release_obj', 'aborted': 'say_go_second_location',
'preempted': 'preempted'})
# Say release object
smach.StateMachine.add(
'say_release_obj',
text_to_say("I'm going to release the object"),
transitions={'succeeded': 'pregrasp_state', 'aborted': 'pregrasp_state'})
# Pre-grasp position
smach.StateMachine.add(
'pregrasp_state',
play_motion_sm('pre_grasp', skip_planning=True),
transitions={'succeeded': 'play_motion_state', 'preempted':'play_motion_state',
'aborted':'pregrasp_state'})
# Release the object
smach.StateMachine.add(
'release_object',
place_object_sm(),
transitions={'succeeded': 'play_motion_state', 'aborted': 'say_release_obj',
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home'),
transitions={'succeeded': 'say_end_pick_place',
'preempted':'say_end_pick_place',
'aborted':'say_end_pick_place'})
# Say end Pick and Place
smach.StateMachine.add(
'say_end_pick_place',
text_to_say("I finished the Pick and Place test"),
transitions={'succeeded': 'succeeded', '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!
# 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 call_bring_to_loc(location_name): #TODO #Improve toSay, add realese and, may be add some human recognition to avoid throwing stuff to the ground
# if location_name == '':
tosay = "I'm leaving this here"
# else:
# tosay = "I took this item here as requested. Referee I know you are here, if no one else is going to pick this proably you will want to take it before I throw it to the floor, thanks"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_bring_to_loc '+location_name)
#############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
#param_place_high_name = "/robocup_params/" + location_name.replace(" ","_") + "_heigh"
param_place_high_name = "/robocup_params/place/" + location_name.replace(" ","_")
loc_object_position = PoseStamped() #PoseWithCovarianceStamped()
loc_object_position.header.frame_id = "base_link"
loc_object_position.pose.position.z = 1.25
loc_object_position.pose.position.x = 0.25
loc_object_position.pose.orientation.w = 1.0
current_position = world.get_current_position()
rospy.logwarn(current_position)
rospy.logwarn(ROOMS)
if current_position == "kitchen":
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/bar")[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/bar")[2]
call_go_to("bar",world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "living room":
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/dinner_table")[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/dinner_table")[2]
call_go_to("dinner table",world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "hallway":
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/hallway_table")[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/hallway_table")[2]
call_go_to("hallway table",world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "bedroom":
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/bed")[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/bed")[2]
call_go_to("bed",world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position.replace("_"," ") in TABLES:
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/"+current_position.replace("_"," "))[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/"+current_position.replace("_"," "))[2]
sm = place_object_sm(loc_object_position)
out = sm.execute()
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def __init__(self):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
input_keys=['person_location', 'person_location_coord'])
pose_place = PoseStamped()
pose_place.header.frame_id = '/base_link'
pose_place.pose.position.x = 0.0
pose_place.pose.position.y = 0.0
pose_place.pose.position.z = 1.0
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.object_failed = False
self.userdata.object_name = None
smach.StateMachine.add(
'Ask_Question',
text_to_say(text='What would you like me to bring?'),
transitions={
'succeeded': 'Listen_Question',
'aborted': 'Ask_Question',
'preempted': 'Listen_Question'
})
smach.StateMachine.add('Listen_Question',
ListenToSM(grammar='robocup/emergency'),
transitions={
'succeeded': 'Process_Tags',
'aborted': 'Ask_Question',
'preempted': 'Ask_Question'
})
# Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked
# Input keys: actiontag[] 'asr_userSaid_tags'
# Output keys: object
smach.StateMachine.add('Process_Tags',
Process_Tags(),
transitions={
'succeeded': 'Search_Object',
'aborted': 'Ask_Question',
'aborted': 'Ask_Question'
})
smach.StateMachine.add(
'Search_Object',
object_detection_and_grasping_sm(),
transitions={
'succeeded': 'Say_return_Person',
'fail_object_detection': 'Grasp_failed_prepare',
'fail_object_grasping': 'Grasp_failed_prepare',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'object_name': 'object_to_grasp'})
smach.StateMachine.add(
'Grasp_failed_prepare',
Fail_Detection_Grasping(),
transitions={'succeeded': 'Grasp_fail_Ask_Person'})
# TODO: Saying to where to robot is heading.
# smach.StateMachine.add(
# 'Say_go_Place',
# text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'),
# transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'})
# smach.StateMachine.add(
# 'Go_To_Object_Place',
# nav_to_poi(),
# transitions={'succeeded':'Say_got_to_Kitchen', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'})
# smach.StateMachine.add(
# 'Say_got_to_Kitchen',
# text_to_say('I am in the Kitchen, I am going to grasp fail ask person'),
# transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person'})
#
# self.userdata.time_grasp = 0.0
# smach.StateMachine.add('Grasping_with_timeout',
# grasping_with_timeout(),
# transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'})
smach.StateMachine.add('Grasp_fail_Ask_Person',
ask_give_object_grasping(),
transitions={
'succeeded': 'Rest_arm',
'aborted': 'Rest_arm',
'preempted': 'Rest_arm'
})
smach.StateMachine.add('Rest_arm',
play_motion_sm('rest_object_right'),
transitions={
'succeeded': 'Say_return_Person',
'aborted': 'Say_return_Person',
'preempted': 'Say_return_Person'
})
smach.StateMachine.add(
'Say_return_Person',
text_to_say('I am preparing to go back to the person'),
transitions={
'succeeded': 'Prepare_Go_To_Person',
'aborted': 'Prepare_Go_To_Person',
'aborted': 'Prepare_Go_To_Person'
})
#Go to person
smach.StateMachine.add('Prepare_Go_To_Person',
prepare_poi_person_emergency(),
transitions={
'succeeded': 'Go_To_Person',
'aborted': 'Go_To_Person',
'preempted': 'Go_To_Person'
})
#TODO: POI For Person in Emergency -- From SearchPeople SM -
smach.StateMachine.add('Go_To_Person',
nav_to_coord('/map'),
transitions={
'succeeded': 'Say_Give_Object',
'aborted': 'Say_Give_Object',
'preempted': 'Say_Give_Object'
})
smach.StateMachine.add(
'Say_Give_Object',
text_to_say('I am going to give you the Object you asked.'),
transitions={
'succeeded': 'Select_next_state_grasping',
'aborted': 'Select_next_state_grasping',
'preempted': 'Select_next_state_grasping'
})
smach.StateMachine.add('Select_next_state_grasping',
Select_Grasp_Next_State(),
transitions={
'state_failed': 'Give_object_arm',
'state_succeeded': 'Give_object_both'
})
smach.StateMachine.add('Give_object_both',
place_object_sm(pose_place),
transitions={
'succeeded': 'Say_Rescue_stay',
'aborted': 'Say_Rescue_stay',
'preempted': 'preempted'
})
smach.StateMachine.add('Give_object_arm',
play_motion_sm('give_object_right'),
transitions={
'succeeded': 'Give_Object',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
#Give the grabbed object to the person
smach.StateMachine.add('Give_Object',
move_hands_form(hand_pose_name='pre_grasp',
hand_side='right'),
transitions={
'succeeded': 'Give_Object_2',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
smach.StateMachine.add('Give_Object_2',
move_hands_form(hand_pose_name='full_open',
hand_side='right'),
transitions={
'succeeded': 'Say_Rescue_stay',
'aborted': 'Give_Object_2',
'preempted': 'Give_Object_2'
})
smach.StateMachine.add(
'Say_Rescue_stay',
text_to_say(
'Please Stay here I am going to call for the Ambulance'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'aborted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_array'])
pose_place = PoseStamped()
pose_place.header.frame_id = '/base_link'
pose_place.pose.position.x = 0.0
pose_place.pose.position.y = 0.0
pose_place.pose.position.z = 1.0
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=None
self.userdata.standard_error='OK'
self.userdata.object_name = ''
self.userdata.object_index = 0
self.userdata.time_out_grasp=TIME_OUT_GRASP
# Process order
smach.StateMachine.add(
'decide_next_object',
decide_next_object(),
remapping={"object_loc": "nav_to_poi_name"},
transitions={'succeeded': 'say_im_going', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deliver object
smach.StateMachine.add(
'say_im_going',
text_to_say(),
transitions={'succeeded': 'go_to_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to poi where the object can stay
smach.StateMachine.add(
'go_to_object',
nav_to_poi(),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'go_to_object'})
# Grasp Object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("Grasping Object"),
transitions={'succeeded': 'Search_and_grasp_Object', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'Search_and_grasp_Object',
object_detection_and_grasping_sm(),
transitions={'succeeded':'prepare_delivery_goal',
'fail_object_detection':'say_object_not_find',
'fail_object_grasping':'say_not_grasp',
'aborted':'aborted',
'preempted':'preempted'},
remapping = {'object_name':'object_to_grasp'})
# Go to the delivery place
# Grasp Object
smach.StateMachine.add(
'say_not_grasp',
text_to_say("it was impossible to grasp the Object"),
transitions={'succeeded': 'decide_next_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Grasp Object
smach.StateMachine.add(
'say_object_not_find',
text_to_say("it was not possible to find the Object"),
transitions={'succeeded': 'decide_next_object', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepare_delivery_goal',
prepare_delivery(),
transitions={'succeeded': 'say_going_deliver_object', 'aborted': 'say_going_deliver_object',
'preempted': 'preempted'})
# Deliver object
smach.StateMachine.add(
'say_going_deliver_object',
text_to_say("i am going to deliver the object"),
transitions={'succeeded': 'go_to_delivery', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the delivery place
smach.StateMachine.add(
'go_to_delivery',
nav_to_poi(),
transitions={'succeeded': 'say_deliver_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deliver object
smach.StateMachine.add(
'say_deliver_object',
text_to_say("Delivering Object"),
transitions={'succeeded': 'deliver_object', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'deliver_object',
place_object_sm(pose_place),
transitions={'succeeded':'check_loop',
'aborted':'check_loop',
'preempted':'preempted'})
# End of loop?
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'decide_next_object', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'succeeded'})
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):
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_lang = ''
self.userdata.tts_wait_before_speak = ''
self.userdata.tts_text = ''
self.userdata.loop_iterations = 1
self.userdata.did_unk = False
self.userdata.object_name = ['Pringles', 'Barritas']
# Say start Pick and Place
smach.StateMachine.add(
'say_start_pick_place',
text_to_say("I'm going to start Pick and Place test"),
transitions={'succeeded': 'say_go_location', 'aborted': 'say_go_location'})
# Say Going to location
smach.StateMachine.add(
'say_go_location',
text_to_say("I'm going to the location where I have to recognize some objects", wait=False),
transitions={'succeeded': 'prepare_location', 'aborted': 'prepare_location'})
# Prepare the poi for nav_to_poi
smach.StateMachine.add(
'prepare_location',
prepare_location(),
transitions={'succeeded': 'go_location', 'aborted': 'prepare_location',
'preempted': 'preempted'})
# Go to the location
smach.StateMachine.add(
'go_location',
nav_to_poi(),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'say_go_location',
'preempted': 'preempted'})
# recognize and pick object if found
smach.StateMachine.add(
'recognize_object_and_pick',
RecObjectAndPick(),
transitions={'succeeded': 'Process_Place_location',
'fail_grasp':'Process_Place_location',
'fail_recognize': 'try_again_recognition'})
# Prepare the place location
smach.StateMachine.add(
'Process_Place_location',
process_place_location(),
transitions={'succeeded':'say_go_second_location',
'aborted':'aborted'})
# Say start object recognition
# smach.StateMachine.add(
# 'say_start_obj_recognition',
# text_to_say("I'm going to start the Object recognition process.", wait=False),
# transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'})
#
# # Do object_recognition
# smach.StateMachine.add(
# 'object_recognition',
# recognize_object(),
# transitions={'succeeded': 'process_object_recognition', 'aborted': 'try_again_recognition',
# 'preempted': 'preempted'})
#
# # Process the objects recognized
# smach.StateMachine.add(
# 'process_object_recognition',
# process_place_location(),
# transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_grasp_object',
# 'preempted': 'preempted'})
# We don't recognized the object
smach.StateMachine.add(
'try_again_recognition',
checkLoop(),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'recognize_object_and_pick',
'preempted': 'preempted', 'end':'say_fail_recognize'})
# Say fail recognize objects
smach.StateMachine.add(
'say_fail_recognize',
text_to_say("I'm not able to recognized any object. I'm going to search for anything"),
transitions={'succeeded': 'prepare_unk_object', 'aborted': 'prepare_unk_object'})
# Prepare goal to pick any object
smach.StateMachine.add(
'prepare_unk_object',
prepare_unk_object(),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_grasp_object'})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to grasp the object", wait=False),
transitions={'succeeded': 'grasp_object', 'aborted': 'grasp_object'})
# Grasp the object
smach.StateMachine.add(
'grasp_object',
pick_object_sm(),
transitions={'succeeded': 'say_go_second_location', 'aborted': 'home_position', #TODO: Change aborted to try again
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm('home'),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'home_position', #TODO: Change aborted to try again
'preempted': 'preempted'})
# Say go to second location
smach.StateMachine.add(
'say_go_second_location',
text_to_say("I'm going to the location where I should release the object", wait=False),
transitions={'succeeded': 'go_second_location', 'aborted': 'go_second_location'})
# Go the location - We need to go to the place to object category, so we assume that the
# object recognition will init the poi to the object must to go
smach.StateMachine.add(
'go_second_location',
nav_to_poi(),
transitions={'succeeded': 'say_release_obj', 'aborted': 'say_go_second_location',
'preempted': 'preempted'})
# Say release object
smach.StateMachine.add(
'say_release_obj',
text_to_say("I'm going to release the object", wait=True),
transitions={'succeeded': 'release_object', 'aborted': 'release_object'})
# Check if we pick the know or the unk object
smach.StateMachine.add(
'check_place_give',
check_object(),
transitions={'succeeded':'release_object', 'aborted':'pregrasp_state', 'preempted':'check_place_give'})
# Release the object
smach.StateMachine.add(
'release_object',
place_object_sm(),
transitions={'succeeded': 'play_motion_state', 'aborted': 'say_release_obj',
'preempted': 'preempted'})
# Pre-grasp position
smach.StateMachine.add(
'pregrasp_state',
play_motion_sm('pre_grasp', skip_planning=True),
transitions={'succeeded': 'play_motion_state', 'preempted':'play_motion_state',
'aborted':'pregrasp_state'})
# Home position
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home'),
transitions={'succeeded': 'say_end_pick_place', 'preempted':'say_end_pick_place',
'aborted':'say_end_pick_place'})
# Say end Pick and Place
smach.StateMachine.add(
'say_end_pick_place',
text_to_say("I finished the Pick and Place test"),
transitions={'succeeded': 'succeeded', '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.loop_iterations = 0
self.userdata.try_iterations = 1
self.userdata.gesture_name = ''
self.userdata.object_name = ""
self.userdata.manip_time_to_play = 4
self.userdata.did_pick = True
self.userdata.grammar_name = GRAMMAR_NAME
smach.StateMachine.add(
'init_cocktail',
text_to_say("Ready for cocktail party"),
transitions={'succeeded': 'learning_person', 'aborted': 'wait_for_door'})
# We wait for open door and go inside
smach.StateMachine.add(
'wait_for_door',
EnterRoomSM("party_room"),
transitions={'succeeded': 'say_search_wave', 'aborted': 'aborted', 'preempted': 'preempted'})
# Say Wave recognize
smach.StateMachine.add(
'say_search_wave',
text_to_say("I'm searching for people waving at me", wait=False),
transitions={'succeeded': 'wave_recognition', 'aborted': 'wave_recognition'})
# Gesture recognition -> Is anyone waving?
smach.StateMachine.add(
'wave_recognition',
WaveDetection(),
transitions={'succeeded': 'say_wave_recognize', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Say Wave recognize
smach.StateMachine.add(
'say_wave_recognize',
text_to_say("Someone waved to me. I will go there", wait=False),
transitions={'succeeded': 'prepare_coord_wave', 'aborted': 'prepare_coord_wave'})
# Prepare the goal to the person that is waving
# TODO: it goes a little far to the person...
smach.StateMachine.add(
'prepare_coord_wave',
prepare_coord_wave(),
transitions={'succeeded': 'go_to_person_wave', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the person -> we assume that gesture will return the position
smach.StateMachine.add(
'go_to_person_wave',
nav_to_coord('/base_link'),
transitions={'succeeded': 'learning_person', 'aborted': 'go_to_person_wave',
'preempted': 'preempted'})
# Ask for person if it can see anyone
smach.StateMachine.add(
'ask_for_person',
text_to_say("I can't see anyone. Can anyone come to me, please?"),
transitions={'succeeded': 'wait_for_person', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Wait for person
smach.StateMachine.add(
'wait_for_person',
detect_face(),
transitions={'succeeded': 'learning_person', 'aborted': 'ask_for_person'})
# Learn Person -> Ask name + Face Recognition
# TODO: Set database
smach.StateMachine.add(
'learning_person',
SaveFaceSM(),
transitions={'succeeded': 'ask_order', 'aborted': 'learning_person',
'preempted': 'preempted'})
# Ask for order
smach.StateMachine.add(
'ask_order',
AskQuestionSM("What would you like to order?", GRAMMAR_NAME),
transitions={'succeeded': 'process_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Process the answer
smach.StateMachine.add(
'process_order',
process_order(),
transitions={'succeeded': 'say_got_it', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Say what he ask
smach.StateMachine.add(
'say_got_it',
text_to_say("I got it!"),
transitions={'succeeded': 'go_to_storage', 'aborted': 'go_to_storage',
'preempted': 'preempted'})
# Go to the storage_room
smach.StateMachine.add(
'go_to_storage',
nav_to_poi("storage_room"),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'go_to_storage',
'preempted': 'preempted'})
# Recognize and pick object if found
smach.StateMachine.add(
'recognize_object_and_pick',
RecObjectAndPick(),
transitions={'succeeded': 'go_to_party',
'fail_grasp':'go_to_party',
'fail_recognize': 'try_again_recognition'})
# Say start object recognition
# smach.StateMachine.add(
# 'say_start_obj_recognition',
# text_to_say("I'm going to start the Object recognition"),
# transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'})
#
# # Do object_recognition
# smach.StateMachine.add(
# 'object_recognition',
# dummy_recognize(),
# transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_release_obj',
# 'preempted': 'preempted'})
#
# # Say grasp object
# smach.StateMachine.add(
# 'say_grasp_order',
# text_to_say("I'm going to grasp it"),
# transitions={'succeeded': 'grasp_food_order', 'aborted': 'Grasp_fail_Ask_Person',
# 'preempted': 'preempted'})
#
# # Grasp Object
# smach.StateMachine.add(
# 'grasp_food_order',
# pick_object_sm(),
# transitions={'succeeded': 'go_to_party', 'aborted': 'Grasp_fail_Ask_Person',
# 'preempted': 'preempted'})
# We don't recognized the object
smach.StateMachine.add(
'try_again_recognition',
checkRecognition(),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'recognize_object_and_pick',
'preempted': 'preempted', 'end':'say_fail_recognize'})
# Say fail recognize objects
smach.StateMachine.add(
'say_fail_recognize',
text_to_say("I'm not able to recognized any object."),
transitions={'succeeded': 'Grasp_fail_Ask_Person', 'aborted': 'Grasp_fail_Ask_Person'})
# Ask for grasp object
smach.StateMachine.add(
'Grasp_fail_Ask_Person',
ask_give_object_grasping(),
remapping={'object_to_grasp':'object_name'},
transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'})
smach.StateMachine.add(
'Rest_arm',
play_motion_sm('rest_object_right'),
transitions={'succeeded':'change_did_pick', 'aborted':'change_did_pick', 'preempted':'change_did_pick'})
smach.StateMachine.add(
'change_did_pick',
change_did_pick(),
transitions={'succeeded':'go_to_party', 'aborted':'go_to_party', 'preempted':'go_to_party'})
# Go to the party room
smach.StateMachine.add(
'go_to_party',
nav_to_poi('party_room'),
transitions={'succeeded': 'say_search_person', 'aborted': 'go_to_party',
'preempted': 'preempted'})
# Say search for person
smach.StateMachine.add(
'say_search_person',
text_to_say("I'm going to search the person who ordered me"),
transitions={'succeeded': 'search_for_person', 'aborted': 'search_for_person',
'preempted': 'preempted'})
# Search for person -> He could change his position
smach.StateMachine.add(
'search_for_person',
recognize_face_concurrent(),
transitions={'succeeded': 'say_found_person', 'aborted': 'prepare_ask_for_person_back',
'preempted': 'preempted'})
# Say found the person
smach.StateMachine.add(
'say_found_person',
text_to_say("I found you!"),
transitions={'succeeded': 'prepare_coord_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Prepare the goal to the person that ask for the order
smach.StateMachine.add(
'prepare_coord_order',
prepare_coord_order(),
transitions={'succeeded': 'go_to_person_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to person
smach.StateMachine.add(
'go_to_person_order',
nav_to_coord('/base_link'),
transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted',
'preempted': 'preempted'})
# Ask for person if it can see anyone
smach.StateMachine.add(
'prepare_ask_for_person_back',
prepare_ask_person_back(),
transitions={'succeeded': 'ask_for_person_back', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'ask_for_person_back',
text_to_say(),
transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted', 'preempted': 'preempted'})
# Deliver Drink
smach.StateMachine.add(
'deliver_drink',
text_to_say("I'm going to deliver the drink"),
transitions={'succeeded': 'Give_Object', 'aborted': 'Give_Object',
'preempted': 'preempted'})
# Check if we pick or ask the drink
smach.StateMachine.add(
'check_place_give',
check_place_give(),
transitions={'succeeded':'Place_Object', 'aborted':'Give_Object', 'preempted':'check_place_give'})
# Place if we pick the drink
smach.StateMachine.add(
'Place_Object',
place_object_sm(),
transitions={'succeeded':'check_loop', 'aborted':'Place_Object', 'preempted':'Place_Object'})
# Give if we ask for drink
smach.StateMachine.add(
'Give_Object',
give_object(),
transitions={'succeeded':'check_loop', 'aborted':'Give_Object', 'preempted':'Give_Object'})
# End of loop?
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'wave_recognition', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'leaving_arena'})
# Say leaving the arena
smach.StateMachine.add(
'say_leaving_arena',
text_to_say("I finished the cocktail party, I'm leaving the arena"),
transitions={'succeeded': 'Give_Object', 'aborted': 'Give_Object',
'preempted': 'preempted'})
# Leaving the arena
smach.StateMachine.add(
'leaving_arena',
nav_to_poi('leave_arena'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
