def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done','Aborted']) self.userdata.crowd_data = { "males": 3, "men": 2, "females": 5, "women": 3, "children": 3, "boys": 1, "girls": 2, "adults": 5, "elders": 1, "crowd_size": 8 } with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={'initialized': 'RIDDLE_GAME', 'abort': 'Aborted'}) smach.StateMachine.add("RIDDLE_GAME", HearAndAnswerQuestions( robot, grammar=knowledge.grammar, knowledge=common_knowledge, num_questions=3, ), transitions={'done': 'Done'}, remapping={'crowd_data':'crowd_data'})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted']) runs = ds.Designator([0, 1]) run = ds.VariableDesignator(resolve_type=int) with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SET_INITIAL_POSE', 'abort': 'aborted' }) smach.StateMachine.add('SET_INITIAL_POSE', states.SetInitialPose( robot, challenge_knowledge.starting_point), transitions={ 'done': 'HANDLE_GUEST_1', "preempted": 'aborted', 'error': 'HANDLE_GUEST_1' }) smach.StateMachine.add('HANDLE_GUEST_1', HandleSingleGuest(robot, assume_john=True), transitions={ 'succeeded': 'HANDLE_GUEST_2', 'aborted': 'HANDLE_GUEST_2' }) smach.StateMachine.add('HANDLE_GUEST_2', HandleSingleGuest(robot, assume_john=False), transitions={ 'succeeded': 'SAY_DONE', 'aborted': 'SAY_DONE' }) smach.StateMachine.add( 'SAY_DONE', states.Say(robot, ["That's all folks, my job is done, bye bye!"], block=False), transitions={'spoken': 'GO_BACK'}) smach.StateMachine.add( 'GO_BACK', states.NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_door['id']), challenge_knowledge.waypoint_door['radius']), transitions={ 'arrived': 'succeeded', 'unreachable': 'succeeded', 'goal_not_defined': 'succeeded' })
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: # Start challenge via StartChallengeRobust smach.StateMachine.add("START_CHALLENGE_ROBUST", states.Initialize(robot), transitions={ 'initialized': "SAY_1", "abort": "Aborted" }) smach.StateMachine.add('SAY_1', states.Say(robot, "Please ask me question one"), transitions={'spoken': 'QUESTION_1'}) smach.StateMachine.add('QUESTION_1', HearQuestion(robot), transitions={'answered': 'SAY_2'}) smach.StateMachine.add('SAY_2', states.Say(robot, "Please ask me question two"), transitions={'spoken': 'QUESTION_2'}) smach.StateMachine.add('QUESTION_2', HearQuestion(robot), transitions={'answered': 'SAY_3'}) smach.StateMachine.add('SAY_3', states.Say(robot, "Please ask me question three"), transitions={'spoken': 'QUESTION_3'}) smach.StateMachine.add('QUESTION_3', HearQuestion(robot), transitions={'answered': 'SAY_4'}) smach.StateMachine.add('SAY_4', states.Say(robot, "Please ask me question four"), transitions={'spoken': 'QUESTION_4'}) smach.StateMachine.add('QUESTION_4', HearQuestion(robot), transitions={'answered': 'SAY_5'}) smach.StateMachine.add('SAY_5', states.Say(robot, "Please ask me question five"), transitions={'spoken': 'QUESTION_5'}) smach.StateMachine.add('QUESTION_5', HearQuestion(robot), transitions={'answered': 'AT_END'}) smach.StateMachine.add('AT_END', states.Say(robot, "That was all folks!"), transitions={'spoken': 'Done'}) return sm
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done','Aborted']) with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={'initialized': 'DETECT_CROWD', 'abort': 'Aborted'}) smach.StateMachine.add("DETECT_CROWD", DetectCrowd(robot), transitions={'succeeded': 'Done', 'failed': 'Aborted'})
def __init__(self, robot): """ Final challenge of RWC 2019 Sydney :param robot: (Robot) api object """ smach.StateMachine.__init__(self, outcomes=["done"]) with self: # smach.StateMachine.add("START_ROBUST", # states.StartChallengeRobust(robot, "initial_pose"), # transitions={"Done": "LASER_POINTING", # "Aborted": "LASER_POINTING", # "Failed": "LASER_POINTING"}) smach.StateMachine.add("INIT", states.Initialize(robot), transitions={ "initialized": "LASER_POINTING", "abort": "LASER_POINTING" }) smach.StateMachine.add("LASER_POINTING", DriveAndSwordFight(robot), transitions={"done": "FIND_PEOPLE"}) smach.StateMachine.add("FIND_PEOPLE", FindPeople(robot), transitions={"done": "GET_ORDERS"}) smach.StateMachine.add("GET_ORDERS", GetOrders(robot), transitions={"done": "GET_DRINKS"}) smach.StateMachine.add("GET_DRINKS", GetDrinks(robot=robot), transitions={ "done": "done", "failed": "done" }) smach.StateMachine.add("SHOW_PEOPLE_DETECTION", LightSaber(robot), transitions={"done": "done"})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) self.userdata.crowd_data = { "males": 1, "men": 2, "females": 3, "women": 4, "children": 5, "boys": 6, "girls": 7, "adults": 8, "elders": 9, "crowd_size": 10 } with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'HEAR_QUESTION', 'abort': 'Aborted' }) smach.StateMachine.add("HEAR_QUESTION", HearQuestion(robot), transitions={ 'answered': 'HEAR_QUESTION_2', 'not_answered': 'HEAR_QUESTION_2' }, remapping={'crowd_data': 'crowd_data'}) smach.StateMachine.add("HEAR_QUESTION_2", HearQuestion(robot), transitions={ 'answered': 'Done', 'not_answered': 'Done' })
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 __init__(self, robot): # type: (Robot) -> str """ Initialization method :param robot: robot api object """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) # Designators bar_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.bar_id, name='bar_des') room_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.room_id, name='room_des') objects_list_des = ds.VariableDesignator(resolve_type=[ClassificationResult], name='objects_list_des') unav_drink_des = ds.VariableDesignator(resolve_type=str, name='unav_drink_str_des') hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm') with self: smach.StateMachine.add("INITIALIZE", states.Initialize(robot=robot), transitions={"initialized": "INITIAL_POSE", "abort": "aborted"}) smach.StateMachine.add("INITIAL_POSE", states.SetInitialPose(robot, challenge_knowledge.starting_point), transitions={"done": "INSPECT_BAR", "preempted": "aborted", "error": "INSPECT_BAR"}) # Inspect bar and store the list of available drinks smach.StateMachine.add("INSPECT_BAR", states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of", objectIDsDes=objects_list_des), transitions={"done": "INSPECT_FALLBACK", #TODO: Change to CHECK_INSPECT_RESULT after RWC2019 "failed": "INSPECT_FALLBACK"}) smach.StateMachine.add("CHECK_INSPECT_RESULT", CheckInspect(objects_list_des, [ClassificationResult]), transitions={"true": "IDENTIFY_UNAVAILABLE_DRINK", "false": "INSPECT_FALLBACK"}) smach.StateMachine.add("IDENTIFY_UNAVAILABLE_DRINK", IdentifyUnavailableDrinkFromRecognitions(objects=common_knowledge.objects, classification_list_designator=objects_list_des, unavailable_drink_designator=unav_drink_des.writeable, max_unavailable_drinks=challenge_knowledge.MAX_UNAVAILABLE_DRINKS), transitions={"done": "NAVIGATE_TO_ROOM", "failed": "INSPECT_FALLBACK"}) # Inspect fallback - ask the bartender which drink is unavailable and store the unavailable drink smach.StateMachine.add("INSPECT_FALLBACK", AskAvailability(robot=robot, unavailable_drink_designator=unav_drink_des.writeable, objects=common_knowledge.objects), transitions={"succeeded": "RESET_ROBOT", "failed": "RESET_ROBOT"}) smach.StateMachine.add("RESET_ROBOT", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "NAVIGATE_TO_ROOM", 'failed': "NAVIGATE_TO_ROOM"}) # Navigate to the predefined room smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "SAY_HI", "unreachable": "SAY_HI", "goal_not_defined": "aborted"}) smach.StateMachine.add("SAY_HI", states.Say(robot, "Hi, I am {}. I'll be your waiter today".format(robot.robot_name)), transitions={"spoken": "SERVE_DRINK_1"}) # Explicitly add a new state for each drink, i.e., don't use a range iterator to make sure a new state # is constructed every time for idx in range(1, challenge_knowledge.NR_DRINKS + 1): next_state = "SERVE_DRINK_{}".format(idx + 1) if idx < challenge_knowledge.NR_DRINKS else "SAY_DONE" smach.StateMachine.add("SERVE_DRINK_{}".format(idx), ServeOneDrink(robot=robot, bar_designator=bar_designator, room_id=challenge_knowledge.room_id, room_designator=room_designator, objects_list_des=objects_list_des, unav_drink_des=unav_drink_des, name_options=common_knowledge.names, objects=common_knowledge.objects), transitions={"succeeded": next_state, "failed": next_state, "aborted": next_state}) smach.StateMachine.add("SAY_DONE", states.Say(robot, "My job here is done. Enjoy your day and see you next time"), transitions={"spoken": "succeeded"})
def __init__(self, robot): """ Constructor :param robot: robot object """ smach.StateMachine.__init__(self, outcomes=['STOP']) start_pose = robot.base.get_location() start_x = start_pose.frame.p.x() start_y = start_pose.frame.p.y() start_rz = start_pose.frame.M.GetRPY()[2] kitchen_id = "kitchen" kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator( robot=robot, id=kitchen_id) customer_id = 'current_customer' customer_designator = states.util.designators.VariableDesignator( resolve_type=Entity, name=customer_id) orders = [] with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SAY_WAVING', 'abort': 'STOP' }) smach.StateMachine.add( 'SAY_WAVING', states.Say(robot, "Mr. Barman, please make sure that the people wave " "slowly and put their arm up high. Like is shown " "on my screen", block=True), transitions={'spoken': 'SHOW_IMAGE'}) smach.StateMachine.add( 'SHOW_IMAGE', states.ShowImageState( robot, "~/ros/kinetic/system/src/challenge_restaurant/" "images/waving.jpg", seconds=10), transitions={ 'succeeded': 'STORE_KITCHEN', 'failed': 'STORE_KITCHEN' }) smach.StateMachine.add('STORE_KITCHEN', StoreWaypoint(robot=robot, location_id=kitchen_id), transitions={'done': 'WAIT_FOR_CUSTOMER'}) # smach.StateMachine.add('WAIT_FOR_CUSTOMER', # WaitForCustomer(robot, caller_id, kitchen_designator), # transitions={'succeeded': 'SAY_I_HAVE_SEEN', # 'aborted': 'STOP'}) # Implement new find state to detect nearest waving person smach.StateMachine.add( 'WAIT_FOR_CUSTOMER', states.FindFirstPerson(robot, customer_designator.writeable, properties={'tags': ['LWave', 'RWave']}, strict=False, nearest=True, speak=True, look_range=(-np.pi / 4, np.pi / 4), look_steps=4, search_timeout=600), # 10 minutes transitions={ 'found': 'SAY_I_HAVE_SEEN', 'failed': 'WAIT_FOR_CUSTOMER' }) # No Asking # smach.StateMachine.add('SAY_I_HAVE_SEEN', # states.Say(robot, 'I have seen a waving person, I will take the order, I will be there shortly! Coming your way my amigo!'), # transitions={"spoken": 'NAVIGATE_TO_CUSTOMER'}) # End No Asking # Asking for confirmation smach.StateMachine.add( 'SAY_I_HAVE_SEEN', states.Say( robot, 'I have seen a waving person, should I take the order? ' 'Please say "{0} take the order" or "{0} wait"'.format( robot.robot_name)), transitions={"spoken": 'WAIT_FOR_START'}) smach.StateMachine.add('WAIT_FOR_START', AskTakeTheOrder(robot), transitions={ 'yes': 'SAY_NAVIGATE_TO_CUSTOMER', 'wait': 'WAIT_FOR_CUSTOMER', 'timeout': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add( 'SAY_NAVIGATE_TO_CUSTOMER', states.Say( robot, "I am at your service, I will be there shortly! Coming your way my amigo!", block=True), transitions={'spoken': 'NAVIGATE_TO_CUSTOMER'}) # End Asking for confirmation smach.StateMachine.add( 'NAVIGATE_TO_CUSTOMER', states.NavigateToObserve(robot=robot, entity_designator=customer_designator, radius=0.8), transitions={ 'arrived': 'TAKE_ORDER', 'unreachable': 'SAY_NAVIGATE_TO_CUSTOMER_FALLBACK', 'goal_not_defined': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add('SAY_NAVIGATE_TO_CUSTOMER_FALLBACK', states.Say(robot, "Help, lets try it another way"), transitions={'spoken': 'TURN_AROUND'}) smach.StateMachine.add( 'TURN_AROUND', states.Turn(robot, radians=2 * math.pi), transitions={'turned': 'NAVIGATE_TO_CUSTOMER_FALLBACK'}) smach.StateMachine.add('NAVIGATE_TO_CUSTOMER_FALLBACK', states.NavigateToObserve( robot=robot, entity_designator=customer_designator, radius=1.1), transitions={ 'arrived': 'TAKE_ORDER', 'unreachable': 'RETURN_TO_START', 'goal_not_defined': 'RETURN_TO_START' }) smach.StateMachine.add('TAKE_ORDER', TakeOrder( robot=robot, entity_designator=customer_designator, orders=orders), transitions={ 'succeeded': 'NAVIGATE_TO_KITCHEN', 'failed': 'RETURN_TO_START' }) smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint( robot=robot, waypoint_designator=kitchen_designator, radius=0.15), transitions={ 'arrived': 'RECITE_ORDER', 'unreachable': 'SAY_NAVIGATE_TO_KITCHEN_FALLBACK', 'goal_not_defined': 'SAY_NAVIGATE_TO_KITCHEN_FALLBACK' }) smach.StateMachine.add( 'SAY_NAVIGATE_TO_KITCHEN_FALLBACK', states.Say(robot, "Help, how do I get there?", block=False), transitions={'spoken': 'TURN_AROUND_KITCHEN_FALLBACK'}) smach.StateMachine.add( 'TURN_AROUND_KITCHEN_FALLBACK', states.Turn(robot, radians=math.pi), transitions={'turned': 'NAVIGATE_TO_KITCHEN_FALLBACK'}) smach.StateMachine.add('NAVIGATE_TO_KITCHEN_FALLBACK', states.NavigateToWaypoint( robot=robot, waypoint_designator=kitchen_designator, radius=0.20), transitions={ 'arrived': 'RECITE_ORDER', 'unreachable': 'SAY_NAVIGATE_TO_KITCHEN_FALLBACK', 'goal_not_defined': 'SAY_NAVIGATE_TO_KITCHEN_FALLBACK' }) smach.StateMachine.add('RECITE_ORDER', ReciteOrders(robot=robot, orders=orders), transitions={'spoken': 'CLEAR_ORDER'}) smach.StateMachine.add( 'CLEAR_ORDER', ClearOrders(orders=orders), transitions={'succeeded': 'SAY_CANNOT_GRASP'}) smach.StateMachine.add('SAY_CANNOT_GRASP', states.Say( robot, "I am unable to grasp my own order, " "could you please put it in my basket"), transitions={'spoken': 'WAIT_FOR_OBJECTS'}) smach.StateMachine.add('WAIT_FOR_OBJECTS', states.WaitTime(robot=robot, waittime=10.0), transitions={ 'waited': 'BRING_OBJECTS', 'preempted': 'STOP' }) smach.StateMachine.add( 'BRING_OBJECTS', states.NavigateToObserve(robot=robot, entity_designator=customer_designator, radius=1.1), transitions={ 'arrived': 'SAY_OBJECTS', 'unreachable': 'SAY_BRING_OBJECTS_FALLBACK', 'goal_not_defined': 'RETURN_TO_START' }) smach.StateMachine.add( 'SAY_BRING_OBJECTS_FALLBACK', states.Say(robot, "Help, how do I get there?"), transitions={'spoken': 'TURN_AROUND_BRING_OBJECTS_FALLBACK'}) smach.StateMachine.add( 'TURN_AROUND_BRING_OBJECTS_FALLBACK', states.Turn(robot, radians=2 * math.pi), transitions={'turned': 'BRING_OBJECTS_FALLBACK'}) smach.StateMachine.add('BRING_OBJECTS_FALLBACK', states.NavigateToObserve( robot=robot, entity_designator=customer_designator, radius=1.1), transitions={ 'arrived': 'SAY_OBJECTS', 'unreachable': 'SAY_OBJECTS', 'goal_not_defined': 'RETURN_TO_START' }) smach.StateMachine.add( 'SAY_OBJECTS', states.Say( robot, "Hi there handsome, here are your objects, " "please take them from my basket"), transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'}) smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS', states.WaitTime(robot=robot, waittime=10.0), transitions={ 'waited': 'RETURN_TO_START', 'preempted': 'STOP' }) smach.StateMachine.add('RETURN_TO_START', states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3), transitions={ 'arrived': 'WAIT_FOR_CUSTOMER', 'unreachable': 'SAY_RETURN_TO_START_FALLBACK', 'goal_not_defined': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add( 'SAY_RETURN_TO_START_FALLBACK', states.Say(robot, "Help, how do I get back?"), transitions={'spoken': 'RETURN_TO_START_TURN_AROUND'}) smach.StateMachine.add( 'RETURN_TO_START_TURN_AROUND', states.Turn(robot, radians=math.pi), transitions={'turned': 'RETURN_TO_START_FALLBACK'}) smach.StateMachine.add('RETURN_TO_START_FALLBACK', states.NavigateToObserve( robot=robot, entity_designator=customer_designator, radius=0.7), transitions={ 'arrived': 'WAIT_FOR_CUSTOMER', 'unreachable': 'WAIT_FOR_CUSTOMER', 'goal_not_defined': 'WAIT_FOR_CUSTOMER' })
def setup_statemachine(robot): #retract old facts robot.reasoner.query(Compound("retractall", Compound("challenge", "X"))) robot.reasoner.query(Compound("retractall", Compound("goal", "U","V","W", "X", "Y", "Z"))) robot.reasoner.query(Compound("retractall", Compound("explored", "X"))) robot.reasoner.query(Compound("retractall", Compound("state", "X", "Y"))) robot.reasoner.query(Compound("retractall", Compound("current_exploration_target", "X"))) robot.reasoner.query(Compound("retractall", Compound("current_object", "X"))) robot.reasoner.query(Compound("retractall", Compound("visited", "X"))) robot.reasoner.query(Compound("retractall", Compound("unreachable", "X"))) robot.reasoner.query(Compound("retractall", Compound("disposed", "X"))) robot.reasoner.query(Compound("retractall", Compound("point_roi_tried", "X"))) robot.reasoner.query(Compound("retractall", Compound("tasks_done", "X"))) robot.reasoner.query(Compound("retractall", Compound("tasks_max", "X"))) robot.reasoner.query(Compound("retractall", Compound("tasks_failed", "X"))) #Load database robot.reasoner.query(Compound("load_database","tue_knowledge",'prolog/locations.pl')) robot.reasoner.query(Compound("load_database","tue_knowledge",'prolog/egpsr.pl')) #Assert the current challenge. robot.reasoner.query(Compound("assertz",Compound("challenge", "egpsr"))) robot.reasoner.query(Compound("assertz",Compound("tasks_done", "0.0"))) robot.reasoner.query(Compound("assertz",Compound("tasks_max", "20.0"))) # Define how many tasks you want to perform sm = smach.StateMachine(outcomes=['Done','Aborted']) with sm: ###################################################### ##################### INITIALIZE ##################### ###################################################### smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized':'INTRODUCE_SHORT', ###### IN CASE NEXT STATE IS NOT "GO_TO_DOOR" SOMETHING IS SKIPPED 'abort':'Aborted'}) ###################################################### #################### DEMO Version #################### ###################################################### smach.StateMachine.add("INTRODUCE_SHORT", states.Say(robot,"Hi! I will just wait here and wonder if I can do something for you", block=True), transitions={'spoken':'ASK_ACTION'}) smach.StateMachine.add("DEMO_SENTENCE", states.Say(robot,"Although I know what to do, for now I am just showing that I am able to understand you!", block=True), transitions={'spoken':'ASK_ACTION'}) smach.StateMachine.add("ASK_ACTION", Ask_action(robot), transitions={'done':'DEMO_SENTENCE', 'no_action':'ASK_ACTION'}) return sm
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['done', 'aborted']) with sm: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'STORE_ROBOCUP_ARENA', 'abort': 'aborted' }) smach.StateMachine.add('STORE_ROBOCUP_ARENA', StoreRobocupArena(robot), transitions={'done': 'HEAD_STRAIGHT'}) smach.StateMachine.add('HEAD_STRAIGHT', HeadStraight(robot), transitions={'done': 'SAY_INTRO'}) smach.StateMachine.add('SAY_INTRO', states.Say( robot, "Hi, Guide me out of the arena please.", look_at_standing_person=True), transitions={'spoken': 'FOLLOW_INITIAL'}) smach.StateMachine.add('FOLLOW_INITIAL', states.FollowOperator(robot, operator_timeout=30, replan=True), transitions={ 'stopped': 'WAIT_FOR_OPERATOR_COMMAND', 'lost_operator': 'WAIT_FOR_OPERATOR_COMMAND', 'no_operator': 'FOLLOW_INITIAL' }) smach.StateMachine.add('FOLLOW', states.FollowOperator(robot, operator_timeout=30, ask_follow=False, learn_face=False, replan=True), transitions={ 'stopped': 'WAIT_FOR_OPERATOR_COMMAND', 'lost_operator': 'SAY_GUIDE', 'no_operator': 'SAY_GUIDE' }) smach.StateMachine.add('WAIT_FOR_OPERATOR_COMMAND', WaitForOperatorCommand(robot), transitions={ 'follow': 'FOLLOW', 'command': 'SAY_GUIDE' }) smach.StateMachine.add( 'SAY_GUIDE', states.Say(robot, "I will guide you back to the robocup arena!", look_at_standing_person=True), transitions={'spoken': 'GUIDE_TO_ROBOCUP_ARENA'}) smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id="robocup_arena"), radius=knowledge.back_radius), transitions={ 'arrived': 'SAY_BACK', 'unreachable': 'GUIDE_TO_ROBOCUP_ARENA_BACKUP', 'goal_not_defined': 'GUIDE_TO_ROBOCUP_ARENA_BACKUP' }) smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id="robocup_arena"), radius=knowledge.back_radius + 0.1), transitions={ 'arrived': 'SAY_BACK', 'unreachable': 'GUIDE_TO_ROBOCUP_ARENA', 'goal_not_defined': 'GUIDE_TO_ROBOCUP_ARENA' }) smach.StateMachine.add('SAY_BACK', states.Say(robot, "We are back in the robocup arena!", look_at_standing_person=True), transitions={'spoken': 'done'}) return sm
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 __init__(self, robot): """ Constructor :param robot: robot object """ smach.StateMachine.__init__(self, outcomes=['Done', 'WAIT_FOR_CUSTOMER']) kitchen_id = "kitchen" kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator( robot=robot, id=kitchen_id) caller_id = "customer" caller_designator = states.util.designators.ed_designators.EdEntityDesignator( robot=robot, id=caller_id) orders = {} with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'STORE_KITCHEN', 'abort': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add('STORE_KITCHEN', StoreWaypoint(robot=robot, location_id=kitchen_id), transitions={'done': 'WAIT_FOR_CUSTOMER'}) smach.StateMachine.add('WAIT_FOR_CUSTOMER', WaitForClickedCustomer(robot, caller_id), transitions={ 'succeeded': 'NAVIGATE_TO_CUSTOMER', 'failed': 'WAIT_FOR_CUSTOMER', 'aborted': 'WAIT_FOR_CUSTOMER', 'rejected': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add('NAVIGATE_TO_CUSTOMER', states.NavigateToObserve( robot=robot, entity_designator=caller_designator, radius=0.7), transitions={ 'arrived': 'TAKE_ORDER', 'unreachable': 'TAKE_ORDER', 'goal_not_defined': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add('TAKE_ORDER', TakeOrder(robot=robot, location=caller_id, orders=orders), transitions={ 'succeeded': 'NAVIGATE_TO_KITCHEN', 'failed': 'WAIT_FOR_CUSTOMER', 'misunderstood': 'TAKE_ORDER' }) smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint( robot=robot, waypoint_designator=kitchen_designator, radius=0.15), transitions={ 'arrived': 'RECITE_ORDER', 'unreachable': 'RECITE_ORDER', 'goal_not_defined': 'RECITE_ORDER' }) smach.StateMachine.add('RECITE_ORDER', ReciteOrders(robot=robot, orders=orders), transitions={'spoken': 'SAY_CANNOT_GRASP'}) smach.StateMachine.add('SAY_CANNOT_GRASP', states.Say( robot, "I am unable to grasp my own order," "could you please put it in my basket"), transitions={'spoken': 'WAIT_FOR_OBJECTS'}) smach.StateMachine.add('WAIT_FOR_OBJECTS', states.WaitTime(robot=robot, waittime=5.0), transitions={ 'waited': 'BRING_OBJECTS', 'preempted': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add('BRING_OBJECTS', states.NavigateToObserve( robot=robot, entity_designator=caller_designator, radius=0.7), transitions={ 'arrived': 'SAY_OBJECTS', 'unreachable': 'SAY_OBJECTS', 'goal_not_defined': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add( 'SAY_OBJECTS', states.Say( robot, "Dear mister, here are your objects, " "please take them from my basket"), transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'}) smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS', states.WaitTime(robot=robot, waittime=5.0), transitions={ 'waited': 'RETURN_TO_KITCHEN', 'preempted': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add('RETURN_TO_KITCHEN', states.NavigateToWaypoint( robot=robot, waypoint_designator=kitchen_designator, radius=0.15), transitions={ 'arrived': 'WAIT_FOR_CUSTOMER', 'unreachable': 'WAIT_FOR_CUSTOMER', 'goal_not_defined': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add('SAY_DONE', states.Say(robot, "That's it for today, I'm done"), transitions={'spoken': 'Done'})
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) robot.ed.reset() with sm: smach.StateMachine.add("INITIALIZE", robot_smach_states.Initialize(robot), transitions={ "initialized": "SAY_WAITING_FOR_TRIGGER", "abort": "Aborted" }) # Start challenge via StartChallengeRobust, skipped atm smach.StateMachine.add("START_CHALLENGE_ROBUST", robot_smach_states.StartChallengeRobust( robot, challenge_knowledge.starting_point, door=False), transitions={ "Done": "SAY_WAITING_FOR_TRIGGER", "Failed": "Aborted", "Aborted": "Aborted" }) smach.StateMachine.add( 'SAY_WAITING_FOR_TRIGGER', robot_smach_states.Say(robot, [ "Trigger me if you need me!", "Waiting for trigger", "Waiting for you to call me!" ], block=False), transitions={"spoken": "WAIT_FOR_TRIGGER"}) smach.StateMachine.add('WAIT_FOR_TRIGGER', robot_smach_states.WaitForTrigger( robot, ["gpsr"], "/amigo/trigger"), transitions={ "gpsr": "VERIFY", "preempted": "VERIFY" }) smach.StateMachine.add('VERIFY', VerifyWorldModelInfo(robot), transitions={ "done": "SAY_START_CHALLENGE", "failed": "SAY_KNOWLEDGE_NOT_COMPLETE" }) smach.StateMachine.add( 'SAY_KNOWLEDGE_NOT_COMPLETE', robot_smach_states.Say(robot, [ "My knowledge of the world is not complete!", "Please give me some more information!" ], block=False), transitions={"spoken": "SAY_WAITING_FOR_TRIGGER"}) smach.StateMachine.add( 'SAY_START_CHALLENGE', robot_smach_states.Say(robot, [ "Starting R5COP Cooperative cleaning demonstrator", "What a mess here, let's clean this room!", "Let's see if I can find some garbage here", "All I want to do is clean this mess up!" ], block=False), transitions={"spoken": "INSPECT_0"}) for i, place in enumerate(challenge_knowledge.inspection_places): next_i = i + 1 if i + 1 < len( challenge_knowledge.inspection_places) else 0 smach.StateMachine.add( "INSPECT_%d" % i, CleanInspect(robot, place["entity_id"], place["room_id"], place["navigate_area"], place["segment_areas"], challenge_knowledge.known_types), transitions={"done": "INSPECT_%d" % next_i}) return sm
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"]) hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult) information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator( hmi_result_des, "semantics", resolve_type=unicode), str, resolve_type=str) information_point_designator = ds.EdEntityDesignator( robot, id_designator=information_point_id_designator) with self: single_item = InformMachine(robot) if START_ROBUST: smach.StateMachine.add("START_CHALLENGE", states.StartChallengeRobust( robot, INITIAL_POSE_ID), transitions={ "Done": "ASK_WHERE_TO_GO", "Aborted": "Aborted", "Failed": "Aborted" }) smach.StateMachine.add( "ASK_WHERE_TO_GO", states.Say( robot, "Near which furniture object should I go to start guiding people?" ), transitions={"spoken": "WAIT_WHERE_TO_GO"}) smach.StateMachine.add( "WAIT_WHERE_TO_GO", states.HearOptionsExtra( robot=robot, spec_designator=ds.Designator( initial_value=START_GRAMMAR), speech_result_designator=hmi_result_des.writeable), transitions={ "heard": "ASK_CONFIRMATION", "no_result": "ASK_WHERE_TO_GO" }) # ToDo: add fallbacks #option: STORE_STARTING_POSE smach.StateMachine.add( "ASK_CONFIRMATION", states.Say(robot, [ "I hear that you would like me to start the tours at" " the {place}, is this correct?" ], place=information_point_id_designator, block=True), transitions={"spoken": "CONFIRM_LOCATION"}) smach.StateMachine.add("CONFIRM_LOCATION", states.HearOptions( robot=robot, options=["yes", "no"]), transitions={ "yes": "MOVE_OUT_OF_MY_WAY", "no": "ASK_WHERE_TO_GO", "no_result": "ASK_WHERE_TO_GO" }) smach.StateMachine.add( "MOVE_OUT_OF_MY_WAY", states.Say(robot, "Please move your ass so I can get going!"), transitions={"spoken": "TC_MOVE_TIME"}) smach.StateMachine.add("TC_MOVE_TIME", states.WaitTime(robot=robot, waittime=3), transitions={ "waited": "NAV_TO_START", "preempted": "Aborted" }) smach.StateMachine.add( "NAV_TO_START", states.NavigateToSymbolic( robot=robot, entity_designator_area_name_map={ information_point_designator: "in_front_of" }, entity_lookat_designator=information_point_designator), transitions={ "arrived": "TURN_AROUND", "unreachable": "WAIT_NAV_BACKUP", "goal_not_defined": "Aborted" }) # If this happens: never mind smach.StateMachine.add("WAIT_NAV_BACKUP", states.WaitTime(robot, 3.0), transitions={ "waited": "NAV_TO_START_BACKUP", "preempted": "Aborted" }) smach.StateMachine.add( "NAV_TO_START_BACKUP", states.NavigateToSymbolic( robot=robot, entity_designator_area_name_map={ information_point_designator: "near" }, entity_lookat_designator=information_point_designator), transitions={ "arrived": "TURN_AROUND", "unreachable": "SAY_CANNOT_REACH_WAYPOINT", # Current pose backup "goal_not_defined": "Aborted" }) # If this happens: never mind @smach.cb_interface(outcomes=["done"]) def _turn_around(userdata=None): """ Turns the robot approximately 180 degrees around """ v_th = 0.5 robot.base.force_drive(vx=0.0, vy=0.0, vth=v_th, timeout=math.pi / v_th) return "done" smach.StateMachine.add( "TURN_AROUND", smach.CBState(_turn_around), transitions={"done": "STORE_STARTING_POSE"}) smach.StateMachine.add( "SAY_CANNOT_REACH_WAYPOINT", states.Say( robot, "I am not able to reach the starting point." "I'll use this as starting point"), transitions={"spoken": "STORE_STARTING_POSE"}) else: smach.StateMachine.add("INITIALIZE", states.Initialize(robot), transitions={ "initialized": "STORE_STARTING_POSE", "abort": "Aborted" }) ## This is purely for a back up scenario until the range iterator @smach.cb_interface(outcomes=["succeeded"]) def store_pose(userdata=None): base_loc = robot.base.get_location() base_pose = base_loc.frame location_id = INFORMATION_POINT_ID robot.ed.update_entity(id=location_id, frame_stamped=FrameStamped( base_pose, "/map"), type="waypoint") return "succeeded" smach.StateMachine.add("STORE_STARTING_POSE", smach.CBState(store_pose), transitions={"succeeded": "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(1000), 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"})
def __init__(self, robot): """ Constructor :param robot: robot object """ smach.StateMachine.__init__(self, outcomes=['STOP']) start_pose = robot.base.get_location() start_x = start_pose.frame.p.x() start_y = start_pose.frame.p.y() start_rz = start_pose.frame.M.GetRPY()[2] kitchen_id = "kitchen" kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator( robot=robot, id=kitchen_id) caller_id = "customer" caller_designator = states.util.designators.ed_designators.EdEntityDesignator( robot=robot, id=caller_id) orders = {} with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'STORE_KITCHEN', 'abort': 'STOP' }) smach.StateMachine.add('STORE_KITCHEN', StoreWaypoint(robot=robot, location_id=kitchen_id), transitions={'done': 'WAIT_FOR_CUSTOMER'}) smach.StateMachine.add('WAIT_FOR_CUSTOMER', WaitForCustomer(robot, caller_id, kitchen_designator), transitions={ 'succeeded': 'NAVIGATE_TO_CUSTOMER', 'aborted': 'STOP', 'rejected': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add( 'NAVIGATE_TO_CUSTOMER', states.NavigateToObserve(robot=robot, entity_designator=caller_designator, radius=0.85), transitions={ 'arrived': 'TAKE_ORDER', 'unreachable': 'SAY_NAVIGATE_TO_CUSTOMER_FALLBACK', 'goal_not_defined': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add('SAY_NAVIGATE_TO_CUSTOMER_FALLBACK', states.Say(robot, "Help, how do I get there?"), transitions={'spoken': 'TURN_AROUND'}) smach.StateMachine.add( 'TURN_AROUND', states.Turn(robot, radians=2 * math.pi), transitions={'turned': 'NAVIGATE_TO_CUSTOMER_FALLBACK'}) smach.StateMachine.add('NAVIGATE_TO_CUSTOMER_FALLBACK', states.NavigateToObserve( robot=robot, entity_designator=caller_designator, radius=0.85), transitions={ 'arrived': 'TAKE_ORDER', 'unreachable': 'RETURN_TO_START', 'goal_not_defined': 'RETURN_TO_START' }) smach.StateMachine.add('TAKE_ORDER', TakeOrder(robot=robot, location=caller_id, orders=orders), transitions={ 'succeeded': 'NAVIGATE_TO_KITCHEN', 'failed': 'RETURN_TO_START' }) smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint( robot=robot, waypoint_designator=kitchen_designator, radius=0.15), transitions={ 'arrived': 'RECITE_ORDER', 'unreachable': 'SAY_NAVIGATE_TO_KITCHEN_FALLBACK', 'goal_not_defined': 'SAY_NAVIGATE_TO_KITCHEN_FALLBACK' }) smach.StateMachine.add( 'SAY_NAVIGATE_TO_KITCHEN_FALLBACK', states.Say(robot, "Help, how do I get there?"), transitions={'spoken': 'TURN_AROUND_KITCHEN_FALLBACK'}) smach.StateMachine.add( 'TURN_AROUND_KITCHEN_FALLBACK', states.Turn(robot, radians=math.pi), transitions={'turned': 'NAVIGATE_TO_KITCHEN_FALLBACK'}) smach.StateMachine.add('NAVIGATE_TO_KITCHEN_FALLBACK', states.NavigateToWaypoint( robot=robot, waypoint_designator=kitchen_designator, radius=0.20), transitions={ 'arrived': 'RECITE_ORDER', 'unreachable': 'RECITE_ORDER', 'goal_not_defined': 'RECITE_ORDER' }) smach.StateMachine.add('RECITE_ORDER', ReciteOrders(robot=robot, orders=orders), transitions={'spoken': 'SAY_CANNOT_GRASP'}) smach.StateMachine.add('SAY_CANNOT_GRASP', states.Say( robot, "I am unable to grasp my own order," "could you please put it in my basket"), transitions={'spoken': 'WAIT_FOR_OBJECTS'}) smach.StateMachine.add('WAIT_FOR_OBJECTS', states.WaitTime(robot=robot, waittime=5.0), transitions={ 'waited': 'BRING_OBJECTS', 'preempted': 'STOP' }) smach.StateMachine.add( 'BRING_OBJECTS', states.NavigateToObserve(robot=robot, entity_designator=caller_designator, radius=0.85), transitions={ 'arrived': 'SAY_OBJECTS', 'unreachable': 'SAY_BRING_OBJECTS_FALLBACK', 'goal_not_defined': 'RETURN_TO_START' }) smach.StateMachine.add( 'SAY_BRING_OBJECTS_FALLBACK', states.Say(robot, "Help, how do I get there?"), transitions={'spoken': 'TURN_AROUND_BRING_OBJECTS_FALLBACK'}) smach.StateMachine.add( 'TURN_AROUND_BRING_OBJECTS_FALLBACK', states.Turn(robot, radians=2 * math.pi), transitions={'turned': 'BRING_OBJECTS_FALLBACK'}) smach.StateMachine.add('BRING_OBJECTS_FALLBACK', states.NavigateToObserve( robot=robot, entity_designator=caller_designator, radius=0.85), transitions={ 'arrived': 'SAY_OBJECTS', 'unreachable': 'SAY_OBJECTS', 'goal_not_defined': 'RETURN_TO_START' }) smach.StateMachine.add( 'SAY_OBJECTS', states.Say( robot, "Dear mister, here are your objects, " "please take them from my basket"), transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'}) smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS', states.WaitTime(robot=robot, waittime=5.0), transitions={ 'waited': 'RETURN_TO_START', 'preempted': 'STOP' }) smach.StateMachine.add('RETURN_TO_START', states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3), transitions={ 'arrived': 'WAIT_FOR_CUSTOMER', 'unreachable': 'SAY_RETURN_TO_START_FALLBACK', 'goal_not_defined': 'WAIT_FOR_CUSTOMER' }) smach.StateMachine.add( 'SAY_RETURN_TO_START_FALLBACK', states.Say(robot, "Help, how do I get back?"), transitions={'spoken': 'RETURN_TO_START_TURN_AROUND'}) smach.StateMachine.add( 'RETURN_TO_START_TURN_AROUND', states.Turn(robot, radians=math.pi), transitions={'turned': 'RETURN_TO_START_FALLBACK'}) smach.StateMachine.add('RETURN_TO_START_FALLBACK', states.NavigateToObserve( robot=robot, entity_designator=caller_designator, radius=0.7), transitions={ 'arrived': 'WAIT_FOR_CUSTOMER', 'unreachable': 'WAIT_FOR_CUSTOMER', 'goal_not_defined': 'WAIT_FOR_CUSTOMER' })
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) with self: #Part I: Set a table smach.StateMachine.add( 'ENTER_ROOM', # Enter the room states.StartChallengeRobust(robot, knowledge.initial_pose), transitions={ 'Done': 'ANNOUNCEMENT', 'Aborted': 'Aborted', 'Failed': 'Aborted' }) smach.StateMachine.add( 'ANNOUNCEMENT', states.Say( robot, "Let's see if my master has a task for me! Moving to the meeting point.", block=True), transitions={'spoken': 'NAVIGATE_TO_WAYPOINT_I'}) smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_I', states.NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot=robot, id=knowledge.starting_pose), radius=0.3), transitions={ 'arrived': 'FETCH_COMMAND_I', 'unreachable': 'FETCH_COMMAND_I', 'goal_not_defined': 'FETCH_COMMAND_I' }) smach.StateMachine.add( 'FETCH_COMMAND_I', # Hear "set the table" HearFetchCommand(robot, 15.0), transitions={'heard': 'ASK_FOR_MEAL'}) smach.StateMachine.add('ASK_FOR_MEAL', states.Say(robot, "What should I serve, master?", block=True), transitions={'spoken': 'SET_THE_TABLE'}) smach.StateMachine.add( 'SET_THE_TABLE', # Take order and Set the table (bring the objects to the table) states.Initialize(robot), transitions={ 'initialized': 'SERVE_MEAL', 'abort': 'Aborted' }, remapping={'meal': 'meal'}) smach.StateMachine.add( 'SERVE_MEAL', # Serve the meal (for example: pour milk into the bowl) states.Initialize(robot), transitions={ 'initialized': 'CORRECT_OBJECT_POSITIONS', 'abort': 'Aborted' }) smach.StateMachine.add( 'CORRECT_OBJECT_POSITIONS', # Inspect table and correct the moved objects states.Initialize(robot), transitions={ 'initialized': 'ANNOUNCE_TASK_COMPLETION', 'abort': 'Aborted' }) smach.StateMachine.add( 'ANNOUNCE_TASK_COMPLETION', states.Say( robot, "The table is set! Moving to the meeting point for the next task.", block=True), transitions={'spoken': 'NAVIGATE_TO_WAYPOINT_II'}) #Part II: Clean the table smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_II', states.NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot=robot, id=knowledge.starting_pose), radius=0.3), transitions={ 'arrived': 'FETCH_COMMAND_II', 'unreachable': 'FETCH_COMMAND_II', 'goal_not_defined': 'FETCH_COMMAND_II' }) smach.StateMachine.add( 'FETCH_COMMAND_II', # Hear "clear up the table" HearFetchCommand(robot, 15.0), transitions={'heard': 'CLEAR_UP'}) smach.StateMachine.add( 'CLEAR_UP', # Clear up the table (bring the objects to their default location) states.Initialize(robot), transitions={ 'initialized': 'CLEAN_THE_TABLE', 'abort': 'Aborted' }) smach.StateMachine.add( 'CLEAN_THE_TABLE', # Inspect for spots and spills and clean them states.Initialize(robot), transitions={ 'initialized': 'END_CHALLENGE', 'abort': 'Aborted' }) # End smach.StateMachine.add('END_CHALLENGE', states.Say(robot, "I am finally free!"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "set_a_table")
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # Create designators grasp_designator1 = ds.EdEntityDesignator(robot, type="temp") grasp_designator2 = ds.EdEntityDesignator(robot, type="temp") grasp_designator3 = ds.EdEntityDesignator(robot, type="temp") start_pose = robot.base.get_location() start_x = start_pose.frame.p.x() start_y = start_pose.frame.p.y() start_rz = start_pose.frame.M.GetRPY()[2] with self: # Part I: Set a table smach.StateMachine.add('ENTER_ROOM', # Enter the room states.Initialize(robot), transitions={'initialized': 'ANNOUNCEMENT', 'abort': 'Aborted'}) smach.StateMachine.add('ANNOUNCEMENT', states.Say(robot, "Let's see if my master has a task for me! ", block=True), transitions={'spoken': 'FETCH_COMMAND_I'}) smach.StateMachine.add('FETCH_COMMAND_I', # Hear "set the table" HearFetchCommand(robot, 15.0, "set"), transitions={'done': 'ASK_FOR_MEAL'}) smach.StateMachine.add('ASK_FOR_MEAL', states.Say(robot, "What should I serve, master?", block=True), transitions={'spoken': 'GET_ORDER'}) smach.StateMachine.add('GET_ORDER', GetBreakfastOrder(robot, knowledge.options, grasp_designator1, grasp_designator2, grasp_designator3, timeout=15.0), transitions={'done': 'SET_THE_TABLE'}) smach.StateMachine.add('SET_THE_TABLE', # Take order and Set the table (bring the objects to the table) ManipulateMachine(robot=robot, grasp_designator1=grasp_designator1, grasp_designator2=grasp_designator2, grasp_designator3=grasp_designator3, grasp_furniture_id1=knowledge.cupboard, grasp_furniture_id3=knowledge.cupboard, place_furniture_id=knowledge.table), transitions={'succeeded': 'ANNOUNCE_TASK_COMPLETION', 'failed': 'RETURN_TO_START_2'}) smach.StateMachine.add('ANNOUNCE_TASK_COMPLETION', states.Say(robot, "The table is set! Moving to the meeting point for the next task.", block=False), transitions={'spoken': 'RETURN_TO_START_2'}) # Part II: Clean the table smach.StateMachine.add('RETURN_TO_START_2', states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3), transitions={'arrived': 'FETCH_COMMAND_II', 'unreachable': 'FETCH_COMMAND_II', 'goal_not_defined': 'FETCH_COMMAND_II'}) smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table" HearFetchCommand(robot, 15.0, "clear"), transitions={'done': 'CLEAR_UP'}) smach.StateMachine.add('CLEAR_UP', # Clear the table ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.table, place_furniture_id1=knowledge.cupboard, place_furniture_id3=knowledge.cupboard), transitions={'succeeded': 'END_CHALLENGE', 'failed': 'END_CHALLENGE'}) # End smach.StateMachine.add('END_CHALLENGE', states.Say(robot, "I am done here"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "set_a_table")
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # - - - - - - - - - - - - - - - - - - - Callback States - - - - - - - - - - - - - - - - - - - @smach.cb_interface(outcomes=['spoken']) def sayHelloCB(userdata=None): printOk("sayHelloCB") robot.speech.speak("Hello " + personNameDes.resolve() + "!", block=False) return 'spoken' @smach.cb_interface(outcomes=['done']) def setOutcomeStatusCB(userdata=None, resultDes=None, succeeded=None): printOk("setOutcomeStatusCB") if True == succeeded: printOk("Container finished successfully") resultDes.write(0) else: printWarning("Container finished unsuccessfully") resultDes.write(1) return 'done' # def my_cb(ud, x=0, y, z): # ud.xyz = ud.q + x + y + z # return 'foo' # ... # with sm: # ... # StateMachine.add('MY_CB', CBState(my_cb, # cb_args=[10], # cb_kwargs={'z':2,'y':3}), # {'foo':'OTHER_STATE'}) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'INIT_WM', 'abort': 'Aborted' }) smach.StateMachine.add( "INIT_WM", states.InitializeWorldModel(robot), transitions={'done': 'SELECT_NEXT_CONTAINER'}) smach.StateMachine.add('SELECT_NEXT_CONTAINER', test_states.SelectNextContainer( robot, containerResultDes), transitions={ 'go_to_enter_room': 'ENTER_ROOM_CONTAINER', 'go_to_wait_person': 'WAIT_PERSON_CONTAINER', 'go_to_pick_up': 'PICK_UP_CONTAINER', 'go_to_recognize_people': 'RECOGNIZE_PEOPLE_CONTAINER', 'go_to_search_people': 'SEARCH_PEOPLE_CONTAINER' }) # 'go_to_end_challenge':'END_CHALLENGE' smach.StateMachine.add( 'SET_OUTCOME_STATUS_SUCCEEDED', smach.CBState(setOutcomeStatusCB, cb_kwargs={ 'resultDes': containerResultDes.writeable, 'succeeded': True }), transitions={'done': 'SELECT_NEXT_CONTAINER'}) smach.StateMachine.add( 'SET_OUTCOME_STATUS_FAILED', smach.CBState(setOutcomeStatusCB, cb_kwargs={ 'resultDes': containerResultDes.writeable, 'succeeded': False }), transitions={'done': 'SELECT_NEXT_CONTAINER'}) # Navigation test smach.StateMachine.add('ENTER_ROOM_CONTAINER', EnterRoomContainer(robot), transitions={ 'container_success': 'SET_OUTCOME_STATUS_SUCCEEDED', 'container_failed': 'SET_OUTCOME_STATUS_FAILED' }) # Human Interaction Test smach.StateMachine.add('WAIT_PERSON_CONTAINER', WaitPersonContainer(robot), transitions={ 'container_success': 'LEARN_NAME_CONTAINER', 'container_failed': 'WAIT_PERSON_CONTAINER' }) # Speech Test smach.StateMachine.add('LEARN_NAME_CONTAINER', LearnNameContainer(robot, personNameDes), transitions={ 'container_failed': 'SET_OUTCOME_STATUS_FAILED', 'container_success': 'SET_OUTCOME_STATUS_SUCCEEDED' }) # Face Learning Test smach.StateMachine.add('LEARN_FACE_CONTAINER', LearnFaceContainer(robot, personNameDes), transitions={ 'container_success': 'SET_OUTCOME_STATUS_SUCCEEDED', 'container_failed': 'SET_OUTCOME_STATUS_FAILED' }) # Face Recognition Test smach.StateMachine.add('RECOGNIZE_PEOPLE_CONTAINER', RecognizePeopleContainer(robot), transitions={ 'container_success': 'SET_OUTCOME_STATUS_SUCCEEDED', 'container_failed': 'SET_OUTCOME_STATUS_FAILED' }) # Manipulation Test smach.StateMachine.add('PICK_UP_CONTAINER', PickUpContainer(robot, objectsIDsDes), transitions={ 'container_success': 'SET_OUTCOME_STATUS_SUCCEEDED', 'container_failed': 'SET_OUTCOME_STATUS_FAILED' }) # Face Recogniton Test smach.StateMachine.add('SEARCH_PEOPLE_CONTAINER', SearchPeopleContainer(robot, personNameDes), transitions={ 'container_success': 'SET_OUTCOME_STATUS_SUCCEEDED', 'container_failed': 'SET_OUTCOME_STATUS_FAILED', }) smach.StateMachine.add('END_CHALLENGE', states.Say( robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) msg = "\n".join([ "==============================================", "== CHALLENGE HELP ME CARRY ==", "==============================================" ]) rospy.loginfo("\n" + msg) self.target_destination = ds.EntityByIdDesignator( robot, id=challenge_knowledge.default_place) self.car_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_car['id']) self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, arm_properties={ "required_goals": [ "handover_to_human", "reset", challenge_knowledge.driving_bag_pose, challenge_knowledge.drop_bag_pose ], "required_gripper_types": [GripperTypes.GRASPING] }, name="empty_arm_designator") # With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked. # For this challenge, unlocking is not needed. self.bag_arm_designator = self.empty_arm_designator.lockable() self.bag_arm_designator.lock() self.place_position = ds.LockingDesignator(EmptySpotDesignator( robot, self.target_destination, arm_designator=self.bag_arm_designator, name="placement", area=challenge_knowledge.default_area), name="place_position") # We don't actually grab something, so there is no need for an actual thing to grab self.current_item = ds.VariableDesignator(Entity( "dummy", "dummy", "/{}/base_link".format(robot.robot_name), kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [], datetime.datetime.now()), name="current_item") with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SET_INITIAL_POSE', 'abort': 'Aborted' }) smach.StateMachine.add('SET_INITIAL_POSE', states.SetInitialPose( robot, challenge_knowledge.starting_point), transitions={ 'done': 'FOLLOW_OPERATOR', "preempted": 'Aborted', 'error': 'FOLLOW_OPERATOR' }) # Follow the operator until (s)he states that you have arrived at the "car". smach.StateMachine.add('FOLLOW_OPERATOR', states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True), transitions={ 'stopped': 'ASK_FOR_TASK', 'lost_operator': 'ASK_FOR_TASK', 'no_operator': 'ASK_FOR_TASK' }) smach.StateMachine.add('ASK_FOR_TASK', states.Say(robot, ["Are we at the car already?"], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_TASK'}) smach.StateMachine.add( 'WAIT_FOR_TASK', hmc_states.WaitForOperatorCommand( robot, possible_commands=challenge_knowledge.commands.keys(), commands_as_outcomes=True), transitions={ 'no': 'FOLLOW_OPERATOR', 'yes': 'REMEMBER_CAR_LOCATION', 'abort': 'Aborted' }) smach.StateMachine.add('REMEMBER_CAR_LOCATION', hmc_states.StoreCarWaypoint(robot), transitions={ 'success': 'ASK_FOR_DESTINATION', 'abort': 'Aborted' }) smach.StateMachine.add( 'ASK_FOR_DESTINATION', states.Say(robot, ["Where should I bring the groceries?"], block=True, look_at_standing_person=True), transitions={'spoken': 'RECEIVE_DESTINATION'}) smach.StateMachine.add( 'RECEIVE_DESTINATION', hmc_states.WaitForOperatorCommand( robot, possible_commands=challenge_knowledge.destinations, commands_as_userdata=True, target=self.target_destination), transitions={ 'success': 'GRAB_ITEM', 'abort': 'Aborted' }) # Grab the item (bag) the operator hands to the robot, when they are at the "car". smach.StateMachine.add( 'GRAB_ITEM', states.HandoverFromHuman( robot, self.bag_arm_designator, "current_item", self.current_item, arm_configuration=challenge_knowledge.carrying_bag_pose), transitions={ 'succeeded': 'ARM_DRIVING_POSE', 'timeout': 'BACKUP_CLOSE_GRIPPER', # For now in simulation timeout is considered a success. 'failed': 'BACKUP_CLOSE_GRIPPER' }) smach.StateMachine.add('BACKUP_CLOSE_GRIPPER', states.SetGripper( robot, self.bag_arm_designator, gripperstate=GripperState.CLOSE), transitions={ 'succeeded': 'ARM_DRIVING_POSE', 'failed': 'ARM_DRIVING_POSE' }) smach.StateMachine.add('ARM_DRIVING_POSE', states.ArmToJointConfig( robot, self.bag_arm_designator, challenge_knowledge.driving_bag_pose), transitions={ 'succeeded': 'SAY_GOING_TO_ROOM', 'failed': 'SAY_GOING_TO_ROOM' }) smach.StateMachine.add( 'SAY_GOING_TO_ROOM', states.Say(robot, [ "Let me bring in your groceries", "Helping you carry stuff", "I'm going back inside" ], block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_DESTINATION'}) smach.StateMachine.add( 'GOTO_DESTINATION', states.NavigateToWaypoint( robot, self.target_destination, challenge_knowledge.default_target_radius), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'GOTO_DESTINATION_BACKUP', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'GOTO_DESTINATION_BACKUP', states.NavigateToWaypoint( robot, self.target_destination, challenge_knowledge.backup_target_radius), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) # Put the item (bag) down when the robot has arrived at the "drop-off" location (house). smach.StateMachine.add('PUTDOWN_ITEM', hmc_states.DropBagOnGround( robot, self.bag_arm_designator, challenge_knowledge.drop_bag_pose), transitions={'done': 'ASKING_FOR_HELP'}) smach.StateMachine.add( 'ASKING_FOR_HELP', # TODO: look and then face new operator states.Say( robot, "Please follow me and help me carry groceries into the house", block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_CAR'}) smach.StateMachine.add( 'GOTO_CAR', states.NavigateToWaypoint( robot, self.car_waypoint, challenge_knowledge.waypoint_car['radius']), # TODO: detect closed door transitions={ 'unreachable': 'OPEN_DOOR', 'arrived': 'AT_END', 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'OPEN_DOOR', # TODO: implement functionality states.Say(robot, "Please open the door for me"), transitions={'spoken': 'GOTO_CAR'}) smach.StateMachine.add( 'AT_END', states.Say(robot, [ "We arrived at the car, goodbye", "You have reached your destination, goodbye", "The car is right here, see you later!" ], block=True, look_at_standing_person=True), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "help_me_carry")
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 setup_statemachine(robot): place_name = ds.EntityByIdDesignator(robot, id=challenge_knowledge.default_place, name="place_name") place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot, place_name, name="placement", area=challenge_knowledge.default_area), name="place_position") empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms, robot.rightArm, name="empty_arm_designator") # With the empty_arm_designator locked, it will ALWAYS resolve to the same arm (first resolve is cached), unless it is unlocked # For this challenge, unlocking is not needed bag_arm_designator = empty_arm_designator.lockable() bag_arm_designator.lock() # We don't actually grab something, so there is no need for an actual thing to grab current_item = ds.VariableDesignator(Entity("dummy", "dummy", "/{}/base_link".format(robot.robot_name), kdl_conversions.kdlFrameFromXYZRPY(0.6, 0, 0.5), None, {}, [], datetime.datetime.now()), name="current_item") 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('SET_INITIAL_POSE', states.SetInitialPose(robot, challenge_knowledge.starting_point), transitions={'done': 'FOLLOW_OPERATOR', "preempted": 'Aborted', 'error': 'FOLLOW_OPERATOR'}) # TODO: learn operator state needs to be added before follow # smach.StateMachine.add('WAIT_TO_FOLLOW', # WaitForOperatorCommand(robot, possible_commands=['follow', 'follow me']), # transitions={'success': 'FOLLOW_OPERATOR', # 'abort': 'Aborted'}) smach.StateMachine.add('ASK_FOLLOW_OR_REMEMBER', states.Say(robot, ["Are we at the car or should I follow you?"], block=True), transitions={'spoken': 'WAIT_TO_FOLLOW_OR_REMEMBER'}) smach.StateMachine.add('WAIT_TO_FOLLOW_OR_REMEMBER', WaitForOperatorCommand(robot, possible_commands=[ "follow", 'follow me', "here is the car", "stop following", "stop following me", ], commands_as_outcomes=True), transitions={'follow': 'FOLLOW_OPERATOR', 'follow me': 'FOLLOW_OPERATOR', 'here is the car': 'REMEMBER_CAR_LOCATION', 'stop following': 'REMEMBER_CAR_LOCATION', 'stop following me': 'REMEMBER_CAR_LOCATION', 'abort': 'Aborted'}) # Follow the operator until (s)he states that you have arrived at the "car". smach.StateMachine.add('FOLLOW_OPERATOR', states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True), transitions={'stopped': 'ASK_FOLLOW_OR_REMEMBER', 'lost_operator': 'ASK_FOLLOW_OR_REMEMBER', 'no_operator': 'ASK_FOLLOW_OR_REMEMBER'}) smach.StateMachine.add('REMEMBER_CAR_LOCATION', StoreCarWaypoint(robot), transitions={'success': 'ASK_DESTINATION', 'abort': 'Aborted'}) smach.StateMachine.add('ASK_DESTINATION', states.Say(robot, ["Where should I bring the groceries?"], block=True), transitions={'spoken': 'WAIT_FOR_DESTINATION'}) smach.StateMachine.add('WAIT_FOR_DESTINATION', WaitForOperatorCommand(robot, possible_commands=challenge_knowledge.waypoints.keys(), commands_as_userdata=True), transitions={'success': 'GRAB_ITEM', 'abort': 'Aborted'}) # Grab the item (bag) the operator hands to the robot, when they are at the "car". smach.StateMachine.add('GRAB_ITEM', GrabItem(robot, bag_arm_designator, current_item), transitions={'succeeded': 'ARM_DRIVING_POSE', 'timeout': 'BACKUP_CLOSE_GRIPPER', # For now in simulation timeout is considered a succes. 'failed': 'BACKUP_CLOSE_GRIPPER'}, remapping={'target_room_in': 'command_recognized', 'target_room_out': 'target_room'}) smach.StateMachine.add('BACKUP_CLOSE_GRIPPER', states.SetGripper(robot, bag_arm_designator, gripperstate=GripperState.CLOSE), transitions={'succeeded': 'ARM_DRIVING_POSE', 'failed': 'ARM_DRIVING_POSE'}) smach.StateMachine.add('ARM_DRIVING_POSE', states.ArmToJointConfig(robot, bag_arm_designator, 'driving_bag_pose'), transitions={'succeeded': 'SAY_GOING_TO_ROOM', 'failed': 'SAY_GOING_TO_ROOM'}) smach.StateMachine.add('SAY_GOING_TO_ROOM', states.Say(robot, ["Let me bring in your groceries", "Helping you carry stuff", "I'm going back inside"], block=True), transitions={'spoken': 'GOTO_DESTINATION'}) smach.StateMachine.add('GOTO_DESTINATION', NavigateToRoom(robot), transitions={'arrived': 'PUTDOWN_ITEM', 'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted'}) # Put the item (bag) down when the robot has arrived at the "drop-off" location (house). smach.StateMachine.add('PUTDOWN_ITEM', DropBagOnGround(robot, bag_arm_designator), transitions={'succeeded': 'ASKING_FOR_HELP', 'failed': 'ASKING_FOR_HELP'}) smach.StateMachine.add('ASKING_FOR_HELP', #TODO: look and then face new operator states.Say(robot, "Please follow me and help me carry groceries into the house"), transitions={'spoken': 'GOTO_CAR'}) #transitions={'success': 'GOTO_CAR', # 'abort': 'Aborted'}) smach.StateMachine.add('GOTO_CAR', states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=challenge_knowledge.waypoint_car['id']), challenge_knowledge.waypoint_car['radius']), # TODO: detect closed door transitions={'unreachable': 'OPEN_DOOR', 'arrived': 'AT_END', 'goal_not_defined': 'Aborted'}) smach.StateMachine.add('OPEN_DOOR', #TODO: implement functionality states.Say(robot, "Please open the door for me"), transitions={'spoken': 'GOTO_CAR'}) #transitions={'success': 'GOTO_CAR', # 'abort': 'Aborted'}) smach.StateMachine.add('AT_END', states.Say(robot, ["We arrived at the car, goodbye", "You have reached your destination, goodbye", "The car is right here, see you later!"], block=True), transitions={'spoken': 'Done'}) ds.analyse_designators(sm, "help_me_carry") 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 setup_statemachine(robot): load_waypoints(robot) operator_id = VariableDesignator(resolve_type=str) sm = smach.StateMachine(outcomes=['done', 'aborted']) with sm: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={'initialized': 'STORE_KITCHEN', 'abort': 'aborted'} ) smach.StateMachine.add('STORE_KITCHEN', StoreKitchen(robot), transitions={'done': 'HEAD_STRAIGHT'} ) smach.StateMachine.add('HEAD_STRAIGHT', HeadStraight(robot), transitions={'done': 'SAY_INTRO'} ) smach.StateMachine.add('SAY_INTRO', states.Say(robot, "Hi, Show me your restaurant please."), transitions={'spoken': 'FOLLOW_INITIAL'} ) smach.StateMachine.add('FOLLOW_INITIAL', states.FollowOperator(robot, operator_timeout=30, operator_id_des=operator_id ), transitions={'stopped': 'STORE', 'lost_operator': 'FOLLOW_INITIAL', 'no_operator': 'FOLLOW_INITIAL'} ) smach.StateMachine.add('FOLLOW', states.FollowOperator(robot, operator_timeout=30, ask_follow=False, operator_id_des=operator_id ), transitions={'stopped': 'STORE', 'lost_operator': 'FOLLOW_INITIAL', 'no_operator': 'FOLLOW_INITIAL'} ) smach.StateMachine.add('STORE', StoreWaypoint(robot), transitions={'done': 'CHECK_KNOWLEDGE', 'continue': 'FOLLOW'} ) smach.StateMachine.add('CHECK_KNOWLEDGE', CheckKnowledge(robot), transitions={'yes': 'SAY_FOLLOW_TO_KITCHEN', 'no':'FOLLOW'} ) smach.StateMachine.add('SAY_FOLLOW_TO_KITCHEN', states.Say(robot, "Please bring me back to the kitchen!" ), transitions={'spoken': 'FOLLOW_TO_KITCHEN'} ) smach.StateMachine.add('FOLLOW_TO_KITCHEN', states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=False, operator_id_des=operator_id ), transitions={'stopped': 'CHECK_IN_KITCHEN', 'lost_operator': 'SAY_GOTO_KITCHEN', 'no_operator': 'SAY_GOTO_KITCHEN'} ) smach.StateMachine.add('SAY_GOTO_KITCHEN', states.Say(robot, "You know what? I will go back to the kitchen on my own!", block=False ), transitions={'spoken': 'GOTO_KITCHEN'}) smach.StateMachine.add('GOTO_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen") ), transitions={'arrived': 'SAY_IN_KITCHEN', 'unreachable': 'SAY_I_DONT_KNOW_HOW', 'goal_not_defined': 'SAY_I_DONT_KNOW_HOW'} ) smach.StateMachine.add('SAY_I_DONT_KNOW_HOW', states.Say(robot, "Oops, I don't know the way back.", block=True ), transitions={'spoken': 'GOTO_KITCHEN'}) # smach.StateMachine.add('FOLLOW_TO_KITCHEN', # states.FollowOperator(robot, # operator_timeout=30, # ask_follow=False # ), # transitions={'stopped': 'CHECK_IN_KITCHEN', # 'lost_operator': 'FOLLOW_TO_KITCHEN_INITIAL', # 'no_operator': 'FOLLOW_TO_KITCHEN_INITIAL'} # ) smach.StateMachine.add('CHECK_IN_KITCHEN', CheckInKitchen(robot), transitions={'not_in_kitchen': 'FOLLOW_TO_KITCHEN', 'in_kitchen':'SAY_IN_KITCHEN'} ) smach.StateMachine.add('SAY_IN_KITCHEN', states.Say(robot, "We are in the kitchen again!" ), transitions={'spoken': 'SAY_WHICH_ORDER'} ) # Where to take the order from? smach.StateMachine.add('SAY_WHICH_ORDER', states.Say(robot, "From which table should I take the first order?"), transitions={ 'spoken' :'HEAR_WHICH_ORDER'}) smach.StateMachine.add('HEAR_WHICH_ORDER', HearWhichTable(robot), transitions={ 'no_result' :'SAY_WHICH_ORDER', 'one' : 'FIRST_SAY_TAKE_ORDER_FROM_TABLE_1', 'two': 'FIRST_SAY_TAKE_ORDER_FROM_TABLE_2', 'three' : "FIRST_SAY_TAKE_ORDER_FROM_TABLE_3"}) # ############## first table ############## for i, name in tables.iteritems(): next_i = i+1 if next_i > 3: next_i = 1 smach.StateMachine.add('FIRST_SAY_TAKE_ORDER_FROM_TABLE_%d'%i, states.Say(robot, "Okay, I will take an order from table %d"%i, block=False), transitions={ 'spoken' :'FIRST_NAVIGATE_TO_WAYPOINT_TABLE_%d'%i}) smach.StateMachine.add('FIRST_NAVIGATE_TO_WAYPOINT_TABLE_%d'%i, states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=name), radius = WAYPOINT_RADIUS), transitions={'arrived': 'FIRST_ASK_ORDER_TABLE_%d'%i, 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i, 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i}) smach.StateMachine.add('FIRST_ASK_ORDER_TABLE_%d'%i, AskOrder(robot, name), transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i, 'orders_done' : 'SAY_ORDERS_DONE'}) # ############## Loop over the reset of the tables until we have a beverage and a combo ############## smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_1', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="one"), radius = WAYPOINT_RADIUS), transitions={'arrived': 'SAY_IF_ORDER_TABLE_1', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_2', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_2'}) smach.StateMachine.add('SAY_IF_ORDER_TABLE_1', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]), transitions={ 'spoken' :'HEAD_DOWN_TABLE_1'}) smach.StateMachine.add('HEAD_DOWN_TABLE_1', LookAtPersonSitting(robot), transitions={ 'done' :'HEAR_IF_ORDER_TABLE_1'}) smach.StateMachine.add('HEAR_IF_ORDER_TABLE_1', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False), transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_2', 'yes':'ASK_ORDER_TABLE_1','no':'NAVIGATE_TO_WAYPOINT_TABLE_2'}) smach.StateMachine.add('ASK_ORDER_TABLE_1', AskOrder(robot, "one"), transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_2', 'orders_done' : 'SAY_ORDERS_DONE'}) smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="two"), radius = WAYPOINT_RADIUS), transitions={'arrived': 'SAY_IF_ORDER_TABLE_2', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_3', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_3'}) smach.StateMachine.add('SAY_IF_ORDER_TABLE_2', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]), transitions={ 'spoken' :'HEAD_DOWN_TABLE_2'}) smach.StateMachine.add('HEAD_DOWN_TABLE_2', LookAtPersonSitting(robot), transitions={ 'done' :'HEAR_IF_ORDER_TABLE_2'}) smach.StateMachine.add('HEAR_IF_ORDER_TABLE_2', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False), transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_3', 'yes':'ASK_ORDER_TABLE_2','no':'NAVIGATE_TO_WAYPOINT_TABLE_3'}) smach.StateMachine.add('ASK_ORDER_TABLE_2', AskOrder(robot, "two"), transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_3', 'orders_done' : 'SAY_ORDERS_DONE'}) smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_3', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="three"), radius = WAYPOINT_RADIUS), transitions={'arrived': 'SAY_IF_ORDER_TABLE_3', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_1', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_1'}) smach.StateMachine.add('SAY_IF_ORDER_TABLE_3', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]), transitions={ 'spoken' :'HEAD_DOWN_TABLE_3'}) smach.StateMachine.add('HEAD_DOWN_TABLE_3', LookAtPersonSitting(robot), transitions={ 'done' :'HEAR_IF_ORDER_TABLE_3'}) smach.StateMachine.add('HEAR_IF_ORDER_TABLE_3', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False), transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_1', 'yes':'ASK_ORDER_TABLE_3','no':'NAVIGATE_TO_WAYPOINT_TABLE_1'}) smach.StateMachine.add('ASK_ORDER_TABLE_3', AskOrder(robot, "three"), transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_1', 'orders_done' : 'SAY_ORDERS_DONE'}) smach.StateMachine.add('SAY_ORDERS_DONE', states.Say(robot, "I received enough orders for now, going back to the kitchen!", block=False), transitions={ 'spoken' :'NAVIGATE_BACK_TO_THE_KITCHEN'}) smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS), transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP', 'goal_not_defined':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP'}) smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.2), transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2', 'goal_not_defined':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2'}) smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.4), transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'SPEAK_ORDERS', 'goal_not_defined':'SPEAK_ORDERS'}) smach.StateMachine.add('SPEAK_ORDERS', SpeakOrders(robot), transitions={ 'spoken' :'STORE_BEVERAGE_SIDE'}) smach.StateMachine.add('STORE_BEVERAGE_SIDE', StoreBeverageSide(robot), transitions={ 'done' : 'NAVIGATE_TO_BEVERAGES'}) smach.StateMachine.add('NAVIGATE_TO_BEVERAGES', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="beverages"), radius = WAYPOINT_RADIUS), transitions={'arrived': 'SPEAK_I_SEE_THE_BEVERAGES', 'unreachable':'NAVIGATE_TO_BEVERAGES_BACKUP', 'goal_not_defined':'NAVIGATE_TO_BEVERAGES_BACKUP'}) smach.StateMachine.add('NAVIGATE_TO_BEVERAGES_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="beverages"), radius = WAYPOINT_RADIUS+0.1), transitions={'arrived': 'SPEAK_I_SEE_THE_BEVERAGES', 'unreachable':'STORE_BEVERAGE_SIDE', 'goal_not_defined':'STORE_BEVERAGE_SIDE'}) smach.StateMachine.add('SPEAK_I_SEE_THE_BEVERAGES', states.Say(robot, "The beverages are in front of me", block=False), transitions={ 'spoken' :'DELIVER_BEVERAGE'}) smach.StateMachine.add('DELIVER_BEVERAGE', DeliverOrderWithBasket(robot, "beverage"), transitions={'succeeded':'NAVIGATE_TO_KITCHEN', 'failed':'NAVIGATE_TO_KITCHEN'}) smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS), transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'NAVIGATE_TO_KITCHEN_BACKUP', 'goal_not_defined':'NAVIGATE_TO_KITCHEN_BACKUP'}) smach.StateMachine.add('NAVIGATE_TO_KITCHEN_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.1), transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'DELIVER_COMBO', 'goal_not_defined':'DELIVER_COMBO'}) smach.StateMachine.add('DELIVER_COMBO', DeliverOrderWithBasket(robot, "combo"), transitions={'succeeded':'NAVIGATE_BACK_TO_THE_KITCHEN_2', 'failed':'NAVIGATE_BACK_TO_THE_KITCHEN_2'}) smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS), transitions={'arrived': 'SAY_DONE_WITH_CHALLENGE', 'unreachable':'SAY_DONE_WITH_CHALLENGE', 'goal_not_defined':'SAY_DONE_WITH_CHALLENGE'}) smach.StateMachine.add('SAY_DONE_WITH_CHALLENGE', states.Say(robot, "I'm done with this challenge and you are the banana king!"), transitions={ 'spoken' :'done'}) analyse_designators(sm, "restaurant") return sm
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) robot.ed.reset() with sm: smach.StateMachine.add("INITIALIZE", robot_smach_states.Initialize(robot), transitions={ "initialized": "SAY_WAITING_FOR_TRIGGER", "abort": "Aborted" }) # Start challenge via StartChallengeRobust, skipped atm smach.StateMachine.add("START_CHALLENGE_ROBUST", robot_smach_states.StartChallengeRobust( robot, challenge_knowledge.starting_point, door=False), transitions={ "Done": "SAY_WAITING_FOR_TRIGGER", "Failed": "Aborted", "Aborted": "Aborted" }) smach.StateMachine.add( 'SAY_WAITING_FOR_TRIGGER', robot_smach_states.Say(robot, [ "Trigger me if you need me!", "Waiting for trigger", "Waiting for you to call me!" ], block=False), transitions={"spoken": "WAIT_FOR_TRIGGER"}) smach.StateMachine.add('WAIT_FOR_TRIGGER', robot_smach_states.WaitForTrigger( robot, ["gpsr"], "/amigo/trigger"), transitions={ "gpsr": "VERIFY", "preempted": "VERIFY" }) smach.StateMachine.add('VERIFY', VerifyWorldModelInfo(robot), transitions={ "done": "NAVIGATE_TO_ASK_WAYPOINT", "failed": "SAY_KNOWLEDGE_NOT_COMPLETE" }) smach.StateMachine.add( 'SAY_KNOWLEDGE_NOT_COMPLETE', robot_smach_states.Say(robot, [ "My knowledge of the world is not complete!", "Please give me some more information!" ], block=False), transitions={"spoken": "SAY_WAITING_FOR_TRIGGER"}) smach.StateMachine.add("NAVIGATE_TO_ASK_WAYPOINT", robot_smach_states.NavigateToWaypoint( robot=robot, waypoint_designator=EntityByIdDesignator( robot=robot, id=challenge_knowledge.ask_waypoint), radius=0.3), transitions={ 'arrived': 'DETERMINE_WHAT_TO_CLEAN_INSPECT', 'unreachable': 'DETERMINE_WHAT_TO_CLEAN_INSPECT', 'goal_not_defined': 'DETERMINE_WHAT_TO_CLEAN_INSPECT' }) smach.StateMachine.add( "DETERMINE_WHAT_TO_CLEAN_INSPECT", DetermineWhatToCleanInspect(robot), transitions={ place["entity_id"]: "CLEAN_INSPECT_%s" % place["entity_id"] for place in challenge_knowledge.inspection_places }) for place in challenge_knowledge.inspection_places: smach.StateMachine.add( "CLEAN_INSPECT_%s" % place["entity_id"], CleanInspect(robot, place["entity_id"], place["room_id"], place["navigate_area"], place["segment_areas"]), transitions={"done": "NAVIGATE_TO_ASK_WAYPOINT"}) return sm
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # - - - - - - - - - - - - - - - - - - - Callback States - - - - - - - - - - - - - - - - - - - @smach.cb_interface(outcomes=['spoken']) def sayHelloCB(userdata=None): printOk("sayHelloCB") robot.speech.speak("Hello " + personNameDes.resolve() + "!", block=False) return 'spoken' # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'INIT_WM', 'abort': 'Aborted' }) smach.StateMachine.add( "INIT_WM", states.InitializeWorldModel(robot), transitions={'done': 'ENTER_ROOM_CONTAINER'}) # smach.StateMachine.add( 'SELECT_NEXT_CONTAINER', # test_states.SelectNextContainer(robot, containerResultDes), # transitions={ 'go_to_enter_room':'ENTER_ROOM_CONTAINER', # 'go_to_wait_person':'WAIT_PERSON_CONTAINER', # 'go_to_pick_up':'PICK_UP_CONTAINER', # 'go_to_recognize_people':'RECOGNIZE_PEOPLE_CONTAINER', # 'go_to_search_people':'SEARCH_PEOPLE_CONTAINER', # 'go_to_end_challenge':'END_CHALLENGE'}) # Navigation test smach.StateMachine.add('ENTER_ROOM_CONTAINER', EnterRoomContainer(robot), transitions={ 'container_success': 'WAIT_PERSON_CONTAINER', 'container_failed': 'WAIT_PERSON_CONTAINER' }) # Human Interaction Test smach.StateMachine.add('WAIT_PERSON_CONTAINER', WaitPersonContainer(robot), transitions={ 'container_success': 'LEARN_NAME_CONTAINER', 'container_failed': 'WAIT_PERSON_CONTAINER' }) # Speech Test smach.StateMachine.add('LEARN_NAME_CONTAINER', LearnNameContainer(robot, personNameDes), transitions={ 'container_failed': 'SAY_HELLO', 'container_success': 'SAY_HELLO' }) smach.StateMachine.add( 'SAY_HELLO', smach.CBState(sayHelloCB), transitions={'spoken': 'LEARN_FACE_CONTAINER'}) # Face Learning Test smach.StateMachine.add('LEARN_FACE_CONTAINER', LearnFaceContainer(robot, personNameDes), transitions={ 'container_success': 'RECOGNIZE_PEOPLE_CONTAINER', 'container_failed': 'RECOGNIZE_PEOPLE_CONTAINER' }) # Face Recognition Test smach.StateMachine.add('RECOGNIZE_PEOPLE_CONTAINER', RecognizePeopleContainer(robot), transitions={ 'container_success': 'PICK_UP_CONTAINER', 'container_failed': 'PICK_UP_CONTAINER' }) # Manipulation Test smach.StateMachine.add('PICK_UP_CONTAINER', PickUpContainer(robot, objectsIDsDes), transitions={ 'container_success': 'SEARCH_PEOPLE_CONTAINER', 'container_failed': 'SEARCH_PEOPLE_CONTAINER' }) # Face Recogniton Test smach.StateMachine.add('SEARCH_PEOPLE_CONTAINER', SearchPeopleContainer(robot, personNameDes), transitions={ 'container_success': 'END_CHALLENGE', 'container_failed': 'END_CHALLENGE' }) smach.StateMachine.add('END_CHALLENGE', states.Say( robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'})
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['done', 'aborted']) with sm: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SET_ARM_POSITIONS', 'abort': 'aborted' }) smach.StateMachine.add('SET_ARM_POSITIONS', SetPlateCarryingPose(robot), transitions={'done': 'WAIT_FOR_KEY_PRESS'}) smach.StateMachine.add('WAIT_FOR_KEY_PRESS', WaitForKeyPress(), transitions={ '1': 'SAY_IK_KOM', '2': 'SAY_ALSJEBLIEFT', '3': 'SAY_ALTIJD', '4': 'SAY_STEKKER', '5': 'SAY_OEPS' }) smach.StateMachine.add('SAY_IK_KOM', states.Say(robot, "Ik kom eraan!", look_at_standing_person=True, language='nl', voice='marjolijn', block=False), transitions={'spoken': 'WAIT_FOR_KEY_PRESS'}) smach.StateMachine.add('SAY_ALSJEBLIEFT', states.Say(robot, "Alsjeblieft Rick. De enveloppen.", look_at_standing_person=True, language='nl', voice='marjolijn', block=False), transitions={'spoken': 'WAIT_FOR_KEY_PRESS'}) smach.StateMachine.add('SAY_ALTIJD', states.Say(robot, "Voor jou altijd, Rick.", look_at_standing_person=True, language='nl', voice='marjolijn', block=False), transitions={'spoken': 'WAIT_FOR_KEY_PRESS'}) smach.StateMachine.add( 'SAY_STEKKER', states.Say( robot, "Ik heb stiekem gekeken, Rick. Maar als ik dat verklap, trekken ze de stekker eruit!", look_at_standing_person=True, language='nl', voice='marjolijn', block=False), transitions={'spoken': 'WAIT_FOR_KEY_PRESS'}) smach.StateMachine.add( 'RESET_HEAD', HeadCancel(robot), transitions={'done': 'GO_BACK_TO_STARTING_POINT'}) smach.StateMachine.add('GO_BACK_TO_STARTING_POINT', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id="starting_point"), radius=knowledge.back_radius, speak=False), transitions={ 'arrived': 'done', 'unreachable': 'WAIT_FOR_KEY_PRESS', 'goal_not_defined': 'aborted' }) smach.StateMachine.add('SAY_OEPS', states.Say(robot, "Oeps!", look_at_standing_person=True, language='nl', voice='marjolijn', block=False), transitions={'spoken': 'WAIT_FOR_KEY_PRESS'}) return sm
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) self.target_destination = ds.EntityByIdDesignator( robot, id=challenge_knowledge.default_place) self.car_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_car['id']) self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator( robot, self.target_destination, name="placement", area=challenge_knowledge.default_area), name="place_position") self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, {}, name="empty_arm_designator") # With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked. # For this challenge, unlocking is not needed. self.bag_arm_designator = self.empty_arm_designator.lockable() self.bag_arm_designator.lock() # We don't actually grab something, so there is no need for an actual thing to grab self.current_item = ds.VariableDesignator(Entity( "dummy", "dummy", "/{}/base_link".format(robot.robot_name), kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [], datetime.datetime.now()), name="current_item") with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SET_INITIAL_POSE', 'abort': 'Aborted' }) smach.StateMachine.add('SET_INITIAL_POSE', states.SetInitialPose( robot, challenge_knowledge.starting_point), transitions={ 'done': 'FOLLOW_OPERATOR', 'preempted': 'Aborted', 'error': 'FOLLOW_OPERATOR' }) # Follow the operator until (s)he states that you have arrived at the "car". # smach.StateMachine.add('FOLLOW_OPERATOR', # states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True), # transitions={'stopped': 'ASK_FOR_TASK', # 'lost_operator': 'ASK_FOR_TASK', # 'no_operator': 'FOLLOW_OPERATOR'}) # Use NEW: smach.StateMachine.add('FOLLOW_OPERATOR', states.FollowOperator2(robot), transitions={ 'Done': 'ASK_FOR_TASK', 'Failed': 'ASK_FOR_TASK', 'Aborted': 'FOLLOW_OPERATOR' }) smach.StateMachine.add('ASK_FOR_TASK', states.Say(robot, ["Are we at the car already?"], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_TASK'}) smach.StateMachine.add('WAIT_FOR_TASK', states.HearOptions(robot, ['yes', 'no']), transitions={ 'no': 'FOLLOW_OPERATOR', 'yes': 'CONFIRM_CAR_LOCATION', 'no_result': 'ASK_FOR_TASK' }) smach.StateMachine.add( 'CONFIRM_CAR_LOCATION', states.Say( robot, ["OK, I will remember this location as the car location."], block=True, look_at_standing_person=True), transitions={'spoken': 'REMEMBER_CAR_LOCATION'}) smach.StateMachine.add('REMEMBER_CAR_LOCATION', hmc_states.StoreCarWaypoint(robot), transitions={ 'success': 'ASK_FOR_DESTINATION', 'abort': 'Aborted' }) smach.StateMachine.add( 'ASK_FOR_DESTINATION', states.Say(robot, ["Where should I bring the groceries?"], block=True, look_at_standing_person=True), transitions={'spoken': 'RECEIVE_DESTINATION'}) smach.StateMachine.add( 'RECEIVE_DESTINATION', hmc_states.WaitForOperatorCommand( robot, possible_commands=challenge_knowledge.destinations, commands_as_userdata=True, target=self.target_destination), transitions={ 'success': 'GRAB_ITEM', 'abort': 'Aborted' }) # # smach.StateMachine.add('CONFIRM_DESTINATION', # states.Say(robot, [ # "I will deliver the groceries to the %s" % ds.EntityByIdDesignator(self.target_destination)], # block=True, # look_at_standing_person=True), # transitions={'spoken': 'GRAB_ITEM'}) # Grab the item (bag) the operator hands to the robot, when they are at the "car". smach.StateMachine.add( 'GRAB_ITEM', # states.HandoverFromHuman(robot, self.bag_arm_designator, "current_item", # self.current_item, # arm_configuration=challenge_knowledge.carrying_bag_pose), # transitions={'succeeded': 'ARM_DRIVING_POSE', # 'timeout': 'BACKUP_CLOSE_GRIPPER', # # For now in simulation timeout is considered a success. # 'failed': 'BACKUP_CLOSE_GRIPPER'}) states.Say(robot, [ "I can't pick up the groceries since I don't have arms. Please place them in my basket." ], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_GRAB_ITEM'}) smach.StateMachine.add('WAIT_FOR_GRAB_ITEM', states.WaitTime(robot), transitions={ 'waited': 'SAY_GOING_TO_ROOM', 'preempted': 'Aborted' }) # smach.StateMachine.add('BACKUP_CLOSE_GRIPPER', # states.SetGripper(robot, self.bag_arm_designator, gripperstate=GripperState.CLOSE), # transitions={'succeeded': 'ARM_DRIVING_POSE', # 'failed': 'ARM_DRIVING_POSE'}) # # smach.StateMachine.add('ARM_DRIVING_POSE', # states.ArmToJointConfig(robot, self.bag_arm_designator, # challenge_knowledge.driving_bag_pose), # transitions={'succeeded': 'SAY_GOING_TO_ROOM', # 'failed': 'SAY_GOING_TO_ROOM'}) smach.StateMachine.add( 'SAY_GOING_TO_ROOM', states.Say(robot, [ "Let me bring in your groceries", "Helping you carry stuff", "I'm going back inside" ], block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_DESTINATION'}) smach.StateMachine.add( 'GOTO_DESTINATION', states.NavigateToSymbolic( robot, {self.target_destination: "in_front_of"}, self.target_destination), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'TURN_180_TO_REPLAN', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'TURN_180_TO_REPLAN', hmc_states.TurnToReplan(robot), transitions={ 'success': 'GOTO_DESTINATION_BACKUP', 'abort': 'GOTO_DESTINATION_BACKUP', # implement avoid obstacle behaviour later #'goal_not_defined': 'Aborted'}) }) smach.StateMachine.add( 'GOTO_DESTINATION_BACKUP', states.NavigateToSymbolic( robot, {self.target_destination: "in_front_of"}, self.target_destination), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) # Put the item (bag) down when the robot has arrived at the "drop-off" location (house). smach.StateMachine.add( 'PUTDOWN_ITEM', # hmc_states.DropBagOnGround(robot, self.bag_arm_designator, # challenge_knowledge.drop_bag_pose), states.Say(robot, [ "I can't put the groceries down since I have no arms. Please take them from my basket and put it down." ], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_PUTDOWN_ITEM'}) smach.StateMachine.add('WAIT_FOR_PUTDOWN_ITEM', states.WaitTime(robot), transitions={ 'waited': 'ASKING_FOR_HELP', 'preempted': 'Aborted' }) smach.StateMachine.add( 'ASKING_FOR_HELP', # TODO: look and then face new operator states.Say( robot, "Please follow me and help me carry groceries into the house", block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_CAR'}) #'LEARN_OPERATOR'}) # smach.StateMachine.add('LEARN_OPERATOR', # hmc_states.LearnOperator(robot), # transitions={'learned': 'GOTO_CAR', # 'failed': 'GOTO_CAR'}) smach.StateMachine.add( 'GOTO_CAR', states.NavigateToWaypoint( robot, self.car_waypoint, challenge_knowledge.waypoint_car['radius']), # TODO: detect closed door transitions={ 'unreachable': 'OPEN_DOOR', 'arrived': 'AT_END', 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'OPEN_DOOR', # TODO: implement functionality states.Say(robot, "Please open the door for me"), transitions={'spoken': 'GOTO_CAR'}) smach.StateMachine.add( 'AT_END', states.Say(robot, [ "We arrived at the car, goodbye", "You have reached your destination, goodbye", "The car is right here, see you later!" ], block=True, look_at_standing_person=True), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "help_me_carry")
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")