def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup:
(tup[0].frame.p - robot_pose.frame.p).Norm())
cabinet_id = ENTITY_POSES[0][2]
table_id = ENTITY_POSES[0][3]
# Update the world model
robot.ed.update_entity(id="balcony_shelf",
frame_stamped=FrameStamped(
kdl.Frame(kdl.Rotation(),
kdl.Vector(0.0, 3.0,
0.0)),
frame_id="map"))
robot.ed.update_entity(id=cabinet_id,
frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=table_id,
frame_stamped=ENTITY_POSES[0][1])
# Update designators
cabinet.id_ = ENTITY_POSES[0][2]
room.id_ = ENTITY_POSES[0][4]
# Update manipulate machine
manipulate_machine.place_entity_designator.id_ = cabinet_id
manipulate_machine.place_designator._area = ENTITY_POSES[0][5]
manipulate_machine.place_designator.place_location_designator.id = cabinet_id
manipulate_machine.table_designator.id_ = table_id
manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2]
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=['succeeded',
'failed'], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot, STARTING_POINT),
transitions={
"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
"Aborted": "GO_TO_INTERMEDIATE_WAYPOINT",
"Failed": "GO_TO_INTERMEDIATE_WAYPOINT"
})
# There is no transition to Failed in StartChallengeRobust (28 May)
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_1),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_2),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_3),
radius=0.5),
transitions={
'arrived': 'ASK_CONTINUE',
'unreachable': 'ASK_CONTINUE',
'goal_not_defined': 'ASK_CONTINUE'
})
smach.StateMachine.add("ASK_CONTINUE",
states.AskContinue(robot, 30),
transitions={
'continue': 'SAY_CONTINUEING',
'no_response': 'SAY_CONTINUEING'
})
smach.StateMachine.add(
'SAY_CONTINUEING',
states.Say(robot, [
"I heard continue, so I will move to the exit now. See you guys later!"
],
block=False),
transitions={'spoken': 'GO_TO_EXIT'})
# Amigo goes to the exit (waypoint stated in knowledge base)
smach.StateMachine.add('GO_TO_EXIT',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_1),
radius=0.7),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'
})
smach.StateMachine.add('GO_TO_EXIT_2',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_2),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'
})
smach.StateMachine.add('GO_TO_EXIT_3',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_3),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_TARGET',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('RESET_ED_TARGET',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT'})
# Finally amigo will stop and says 'goodbye' to show that he's done.
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "rips")
return sm
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
start_waypoint = ds.EntityByIdDesignator(robot,
id="manipulation_init_pose",
name="start_waypoint")
placed_items = []
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INIT_WM',
'abort': 'Aborted'
})
smach.StateMachine.add("INIT_WM",
InitializeWorldModel(robot),
transitions={'done': 'AWAIT_START'})
# smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR",
# states.Say(robot, ["Hi there, I will now wait until you remove the cup",
# "I'm waiting for you to remove the cup"], block=False),
# transitions={"spoken": "WAIT_FOR_DOOR"})
#
# smach.StateMachine.add("WAIT_FOR_DOOR",
# states.WaitForDoorOpen(robot, timeout=10),
# transitions={"closed": "DOOR_CLOSED",
# "open": "AWAIT_START"})
#
# smach.StateMachine.add("DOOR_CLOSED",
# states.Say(robot, ["I am waiting for you to remove the cup",
# "I'd start, if you remove the cup from my laser"]),
# transitions={"spoken": "WAIT_FOR_DOOR"})
if USE_SLAM:
drive_state = "RESET_ED_SLAM"
else:
drive_state = "NAV_TO_START"
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': drive_state,
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
if USE_SLAM:
# vth = 1.0
# smach.StateMachine.add("NAV_TO_FIT_POSE",
# ForceDrive(robot, 0, 0, vth, 3.14/vth),
# transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("RESET_ED_SLAM",
states.ResetED(robot),
transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("FIT_ENTITY",
FitEntity(robot, CABINET),
transitions={
'succeeded': 'NAV_TO_START',
'failed': 'SAY_FITTING_FAILED'
})
smach.StateMachine.add(
"SAY_FITTING_FAILED",
states.Say(robot, [
"Fitting the {0} failed, I will stop now.".format(CABINET)
],
mood="sad"),
transitions={'spoken': 'Aborted'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"}, cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'FORCE_ROTATE',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("FORCE_ROTATE",
ForceRotate(robot, 0.5, 2.0, 30.0),
transitions={
'done': "NAV_TO_START",
'timedout': "INSPECT_SHELVES"
})
# smach.StateMachine.add("RESET_ED",
# states.ResetED(robot),
# transitions={'done' :'INSPECT_SHELVES'})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, OBJECT_SHELVES),
transitions={
'succeeded': 'EXPORT_PDF',
'nothing_found': 'EXPORT_PDF',
'failed': 'EXPORT_PDF'
})
@smach.cb_interface(outcomes=["exported"])
def export_to_pdf(userdata=None):
global DETECTED_OBJECTS_WITH_PROBS
entities = [e[0] for e in DETECTED_OBJECTS_WITH_PROBS]
# Export images (Only best MAX_NUM_ENTITIES_IN_PDF)
# pdf.entities_to_pdf(robot.ed, entities[:MAX_NUM_ENTITIES_IN_PDF], "tech_united_manipulation_challenge")
return "exported"
smach.StateMachine.add('EXPORT_PDF',
smach.CBState(export_to_pdf),
transitions={'exported': 'RANGE_ITERATOR'})
# Begin setup iterator
range_iterator = smach.Iterator(
outcomes=['succeeded', 'failed'], #Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded'
) #The exhausted argument should be set to the preffered state machine outcome
with range_iterator:
single_item = ManipRecogSingleItem(
robot,
ds.VariableDesignator(placed_items, [Entity],
name="placed_items"))
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(sm, "manipulation")
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
# Apparently, the indices here come from:
# (cabinet_pose, table_pose, cabinet_amcl, grasp_surface, room, default_place_area)
robot_pose = robot.base.get_location()
WORKSPACES.sort(
key=lambda ws: (ws.place_entity_conf.pose_estimate.frame.p
- robot_pose.frame.p).Norm())
closest_workspace = WORKSPACES[0]
rospy.loginfo(
"Closest workspace: grasp from '{grasp}' and place on '{place}'"
.format(
grasp=closest_workspace.grasp_entity_conf.entity_id,
place=closest_workspace.place_entity_conf.entity_id))
cabinet_id = closest_workspace.place_entity_conf.entity_id
table_id = closest_workspace.grasp_entity_conf.entity_id
# Update the world model by fitting the entities to the frame_stamped's given below.
robot.ed.update_entity(id=cabinet_id,
frame_stamped=closest_workspace.
place_entity_conf.pose_estimate)
robot.ed.update_entity(id=table_id,
frame_stamped=closest_workspace.
grasp_entity_conf.pose_estimate)
# Update designators
cabinet.id_ = closest_workspace.place_entity_conf.entity_id
# Update manipulate machine
manipulate_machine.table_designator.id_ = closest_workspace.grasp_entity_conf.entity_id
manipulate_machine.place_entity_designator.id_ = closest_workspace.place_entity_conf.entity_id
manipulate_machine.place_designator._area = closest_workspace.place_entity_conf.manipulation_volumes[
0]
manipulate_machine.place_designator.place_location_designator.id = closest_workspace.place_entity_conf.entity_id
manipulate_machine.cabinet.id_ = closest_workspace.place_entity_conf.entity_id
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=['succeeded',
'failed'], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# -----------------------------------------------------------------
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INSTRUCT_WAIT_FOR_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add(
"INSTRUCT_WAIT_FOR_DOOR",
states.Say(robot, [
"Hi there, I will now wait until you remove the cup",
"I'm waiting for you to remove the cup"
],
block=False),
transitions={"spoken": "WAIT_FOR_DOOR"})
smach.StateMachine.add("WAIT_FOR_DOOR",
states.WaitForDoorOpen(robot, timeout=10),
transitions={
"closed": "DOOR_CLOSED",
"open": "AWAIT_START"
})
smach.StateMachine.add(
"DOOR_CLOSED",
states.Say(robot, [
"I am waiting for you to remove the cup",
"I'd start, if you remove the cup from my laser"
]),
transitions={"spoken": "WAIT_FOR_DOOR"})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': 'LEARN_OPERATOR_FACE',
'no_response': 'AWAIT_START'
})
smach.StateMachine.add('LEARN_OPERATOR_FACE',
LearnOperatorFace(robot),
transitions={
'succeeded':
'WAIT_FOR_OPERATOR_TO_JOIN',
'failed': 'LEARN_OPERATOR_FACE'
})
@smach.cb_interface(outcomes=['done'])
def wait_a_sec(userdata=None):
robot.speech.speak(
"I will wait for 10 seconds for you to join the crowd",
block=True)
start = rospy.Time.now()
stop = rospy.Duration(10) + start
last_spoken = start
while rospy.Time.now() < stop:
if (rospy.Time.now() - last_spoken).to_sec() > 1.0:
robot.speech.speak("%d" %
(stop - rospy.Time.now()).to_sec())
last_spoken = rospy.Time.now()
return 'done'
smach.StateMachine.add('WAIT_FOR_OPERATOR_TO_JOIN',
smach.CBState(wait_a_sec),
transitions={'done': 'FORCE_DRIVE'})
@smach.cb_interface(outcomes=['done'])
def force_drive(userdata=None):
vth = 0.5
th = 3.1415
robot.head.cancel_goal()
robot.base.force_drive(0, 0, vth, th / vth)
return 'done'
smach.StateMachine.add('FORCE_DRIVE',
smach.CBState(force_drive),
transitions={'done': 'DETECT'})
smach.StateMachine.add('DETECT',
Detect(robot),
transitions={
'succeeded': 'END_CHALLENGE',
'failed': 'DETECT'
})
smach.StateMachine.add('END_CHALLENGE',
states.Say(
robot,
"My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "person_recognition")
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'
})
smach.StateMachine.add('GOTO_WAYPOINT_1',
states.NavigateToWaypoint(
robot, challenge_knowledge.starting_point),
transitions={
'done': 'WAIT_TO_BEGIN',
"preempted": 'Aborted',
'error': 'Aborted'
})
smach.StateMachine.add('WAIT_TO_BEGIN',
states.AskContinue(robot, rospy.Duration(60)),
transitions={
'continue': 'BEGIN_MOVEMENT',
'no_response': 'WAIT_TO_BEGIN'
})
go_sm = smach.Concurrence(outcomes=['succes', 'iterate', 'failed'],
default_outcome='iterate',
outcome_map={
'succes': {
'MOVE_ARM': 'succes',
'MOVE_BASE': 'succes',
'GRAB_OBJECT': 'succes'
}
},
child_termination_cb=term_cb)
with go_sm:
smach.Concurrence.add('MOVE_ARM', MoveArm(robot))
smach.Concurrence.add(
'MOVE_BASE',
MoveBase(robot, entity_designator_area_name_map,
entity_lookat_designator))
smach.Concurrence.add('GRAB_OBJECT', GrabObject(robot))
smach.StateMachine.add('GO_SM',
go_sm,
transitions={
'iterate': 'GO_SM',
'succes': 'PO_SM',
'failed': 'Aborted'
})
po_sm = smach.Concurrence(outcomes=['succes', 'iterate', 'failed'],
default_outcome='iterate',
outcome_map={
'succes': {
'MOVE_ARM': 'succes',
'MOVE_BASE': 'succes',
'RELEASE_OBJECT': 'succes'
}
},
child_termination_cb=term_cb)
with po_sm:
smach.Concurrence.add('MOVE_ARM', MoveArm(robot))
smach.Concurrence.add(
'MOVE_BASE',
MoveBase(robot, entity_designator_area_name_map,
entity_lookat_designator))
smach.Concurrence.add('RELEASE_OBJECT', ReleaseObject(robot))
smach.StateMachine.add('PO_SM',
po_sm,
transitions={
'iterate': 'PO_SM',
'succes': 'Done',
'failed': 'Aborted'
})
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'AWAIT_START',
'abort': 'Aborted'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'})
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None):
""" 'Locks' a locking designator """
# For now, don't do anything
return "done"
# Move away the cabinet
robot.ed.update_entity(id="cabinet",
frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(),
kdl.Vector(12.0, 0, 0)),
frame_id="map"))
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm())
# Update the world model
robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1])
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table),
transitions={'done': 'NAV_TO_START'})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(robot,
{cabinet: "in_front_of"},
cabinet),
transitions={'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot),
transitions={'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'})
smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(outcomes=['succeeded', 'failed'], # Outcomes of the iterator state
input_keys=[], output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) # ToDo: add more pdf stuff
smach.Iterator.set_contained_state('SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator,
{'succeeded': 'AT_END',
'failed': 'Aborted'})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
