def call_grasp(obj,world): #TODO #adding grasping
tosay = "I'm going to grasp the " + obj
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_grasp '+obj)
rospy.logwarn(world.item.object_pose)
#############################################################################
if SKILLS :
pose_object_to_grasp = PoseStamped()
pose_object_to_grasp.header = world.item.object_pose.header
pose_object_to_grasp.pose = world.item.object_pose.pose.pose
if (time.time()-TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while(out=='aborted' and tries<3):
sm = pick_object_sm(pose_object_to_grasp) #if not workng, blame chang
out = sm.execute()
tries = tries+1
#grasping here
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def call_grasp(obj, world): #TODO #adding grasping
tosay = "I'm going to grasp the " + obj
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_grasp ' + obj)
rospy.logwarn(world.item.object_pose)
#############################################################################
if SKILLS:
pose_object_to_grasp = PoseStamped()
pose_object_to_grasp.header = world.item.object_pose.header
pose_object_to_grasp.pose = world.item.object_pose.pose.pose
if (time.time() - TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while (out == 'aborted' and tries < 3):
sm = pick_object_sm(
pose_object_to_grasp) #if not workng, blame chang
out = sm.execute()
tries = tries + 1
#grasping here
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def __init__(self):
smach.StateMachine.__init__(self,
outcomes=['succeeded',
'aborted',
'preempted',
'fail_object_detection',
'fail_object_grasping'],
input_keys=['object_name', 'time_out_grasp'],
output_keys=['standard_error', 'object_detected_name'])
with self:
self.userdata.object_detected_name = ''
smach.StateMachine.add('Search_Object',
SearchObjectSM(),
transitions={'succeeded':'Grasp_object',
'aborted':'fail_object_detection',
'preempted':'preempted'})
smach.StateMachine.add('Prepare_Grasp',
prepare_grasp(),
transitions={'succeeded':'Grasp_object',
'aborted':'aborted'})
smach.StateMachine.add('Grasp_object',
pick_object_sm(),
transitions={'succeeded':'succeeded',
'aborted':'fail_object_grasping',
'preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=[
'succeeded', 'aborted', 'preempted', 'fail_object_detection',
'fail_object_grasping'
],
input_keys=['object_name', 'time_out_grasp'],
output_keys=['standard_error', 'object_detected_name'])
with self:
self.userdata.object_detected_name = ''
smach.StateMachine.add('Search_Object',
SearchObjectSM(),
transitions={
'succeeded': 'Grasp_object',
'aborted': 'fail_object_detection',
'preempted': 'preempted'
})
smach.StateMachine.add('Prepare_Grasp',
prepare_grasp(),
transitions={
'succeeded': 'Grasp_object',
'aborted': 'aborted'
})
smach.StateMachine.add('Grasp_object',
pick_object_sm(),
transitions={
'succeeded': 'succeeded',
'aborted': 'fail_object_grasping',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted', 'fail_grasp', 'fail_recognize'],
input_keys=['object_name'],
output_keys=['object_position','object_detected_name'])
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 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': 'fail_recognize',
'preempted': 'preempted'})
# Process the objects recognized
smach.StateMachine.add(
'process_object_recognition',
process_pick_location(),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'fail_recognize',
'preempted': 'preempted'})
# 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': 'aborted'})
# Grasp the object
smach.StateMachine.add(
'grasp_object',
pick_object_sm(),
transitions={'succeeded': 'succeeded', 'aborted': 'home_position',
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm('home',skip_planning=True),
transitions={'succeeded': 'fail_grasp', 'aborted': 'home_position',
'preempted': '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.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,
outcomes=[
'succeeded', 'preempted', 'aborted', 'fail_grasp',
'fail_recognize'
],
input_keys=['object_name'],
output_keys=['object_position', 'object_detected_name'])
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 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': 'fail_recognize',
'preempted': 'preempted'
})
# Process the objects recognized
smach.StateMachine.add('process_object_recognition',
process_pick_location(),
transitions={
'succeeded': 'say_grasp_object',
'aborted': 'fail_recognize',
'preempted': 'preempted'
})
# 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': 'aborted'
})
# Grasp the object
smach.StateMachine.add('grasp_object',
pick_object_sm(),
transitions={
'succeeded': 'succeeded',
'aborted': 'home_position',
'preempted': 'preempted'
})
# Home position
smach.StateMachine.add('home_position',
play_motion_sm('home', skip_planning=True),
transitions={
'succeeded': 'fail_grasp',
'aborted': 'home_position',
'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_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,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[
'object_name', 'name_face', 'list_orders', 'loop_iterations'
],
output_keys=['did_pick', 'name_face'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.try_iterations = 1
self.userdata.did_pick = True
# State that prepare the information for pick - Mock recognition
smach.StateMachine.add('object_recognition',
dummy_recognize(),
transitions={
'succeeded': 'select_object',
'aborted': 'select_object',
'preempted': 'preempted'
})
# Recognize and pick object if found
smach.StateMachine.add('recognize_object_and_pick',
RecObjectAndPick(),
transitions={
'succeeded': 'object_detected',
'fail_grasp': 'Grasp_fail_Ask_Person',
'fail_recognize':
'try_again_recognition'
})
# Check which object I picked and prepare the person name
smach.StateMachine.add('object_detected',
object_detected(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# Recognize Fail Part
# 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': 'Grasp_fail_Ask_Person'
})
#Grasp Mock Part
# Check which object I picked and prepare the person name
smach.StateMachine.add('select_object',
select_object(),
transitions={
'succeeded': 'play_motion_grasp',
'aborted': 'aborted'
})
# 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 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': 'succeeded',
'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'preempted'
})
# We ask for the object
# 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': 'succeeded',
'aborted': 'succeeded',
'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):
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, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name', 'name_face', 'list_orders', 'loop_iterations'],
output_keys=['did_pick','name_face'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.try_iterations = 1
self.userdata.did_pick = True
# State that prepare the information for pick - Mock recognition
smach.StateMachine.add(
'object_recognition',
dummy_recognize(),
transitions={'succeeded': 'select_object', 'aborted': 'select_object',
'preempted': 'preempted'})
# Recognize and pick object if found
smach.StateMachine.add(
'recognize_object_and_pick',
RecObjectAndPick(),
transitions={'succeeded': 'object_detected',
'fail_grasp':'Grasp_fail_Ask_Person',
'fail_recognize': 'try_again_recognition'})
# Check which object I picked and prepare the person name
smach.StateMachine.add(
'object_detected',
object_detected(),
transitions={'succeeded': 'succeeded', 'aborted':'aborted'})
# Recognize Fail Part
# 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':'Grasp_fail_Ask_Person'})
#Grasp Mock Part
# Check which object I picked and prepare the person name
smach.StateMachine.add(
'select_object',
select_object(),
transitions={'succeeded': 'play_motion_grasp', 'aborted':'aborted'})
# 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 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': 'succeeded', 'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'preempted'})
# We ask for the object
# 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':'succeeded', 'aborted':'succeeded', 'preempted':'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.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("Grasping Object"),
transitions={'succeeded': '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'})
# 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'})
# 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'})