def __init__(self, robot): StateMachine.__init__(self, outcomes=['succeeded', 'failed']) dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id) with self: StateMachine.add("NAVIGATE_BACK_TO_DISHWASHER", NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher), transitions={'arrived': 'CUSTOM_PLACE', 'unreachable': 'SAY_DISHWASHER_NOT_REACHABLE', 'goal_not_defined': 'SAY_DISHWASHER_NOT_REACHABLE'}) StateMachine.add('SAY_DISHWASHER_NOT_REACHABLE', Say(robot, [ "I cannot reach the dishwasher, let me try again in a couple of seconds! Hopefully this will go right this time."], block=True), transitions={'spoken': 'NAVIGATE_BACK_TO_DISHWASHER2'}) StateMachine.add("NAVIGATE_BACK_TO_DISHWASHER2", NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher), transitions={'arrived': 'CUSTOM_PLACE', 'unreachable': 'failed', 'goal_not_defined': 'failed'}) StateMachine.add('CUSTOM_PLACE', CustomPlace(robot), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
def __init__(self, robot): StateMachine.__init__(self, outcomes=['succeeded', 'failed']) source_entity_designator = EdEntityDesignator(robot, id=source_entity) navigation_area_designator1 = VariableDesignator('in_front_of') navigation_area_designator2 = VariableDesignator('in_back_of') with self: StateMachine.add("NAVIGATE_TO_INSPECT", NavigateToSymbolic(robot, {source_entity_designator: navigation_area_designator1}, source_entity_designator), transitions={'arrived': 'SIMPLE_GRASP', 'unreachable': 'NAVIGATE_TO_INSPECT2', 'goal_not_defined': 'NAVIGATE_TO_INSPECT2'}) StateMachine.add('SIMPLE_GRASP', FindAndGrab(robot), transitions={'succeeded': 'succeeded', 'failed': 'NAVIGATE_TO_INSPECT2'}) StateMachine.add("NAVIGATE_TO_INSPECT2", NavigateToSymbolic(robot, {source_entity_designator: navigation_area_designator2}, source_entity_designator), transitions={'arrived': 'SIMPLE_GRASP2', 'unreachable': 'failed', 'goal_not_defined': 'failed'}) StateMachine.add('SIMPLE_GRASP2', FindAndGrab(robot), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
def base_move(pre): global base_moving_base global base_plan_viable global wp global objects_to_navigate_ds global base_goal_reached base_goal_reached = False wp = objects_to_navigate_ds.pop() navSM = NavigateToSymbolic(robot, {wp: "in_front_of"}, wp) # Create a simple SMACH state machine sm = smach.StateMachine(outcomes=['moving','unreachable','goal_not_defined', 'preempted']) # Open the container with sm: # Add states to the container smach.StateMachine.add('GET_PLAN', getPlan(robot, navSM.generateConstraint, True), transitions={'unreachable' : 'unreachable', 'goal_not_defined' : 'goal_not_defined', 'goal_ok' : 'moving', 'preempted' : 'preempted'}) # Execute SMACH plan outcome = sm.execute() if outcome == 'moving': base_moving_base = True elif outcome == 'unreachable'or 'goal_not_defined': base_plan_viable = 2 objects_to_navigate_ds.append(wp) Amigo_AssignInputVariables()
def __init__(self, robot, table_id, table_navigation_area, table_close_navigation_area, placement_height=0.7): StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=["item_picked"]) table = EdEntityDesignator(robot=robot, id=table_id) with self: # StateMachine.add("NAVIGATE_TO_TABLE", # NavigateToSymbolic(robot, {table: table_navigation_area}, table), # transitions={'arrived': 'NAVIGATE_TO_TABLE_CLOSE', # 'unreachable': 'failed', # 'goal_not_defined': 'failed'}) StateMachine.add( "NAVIGATE_TO_TABLE_CLOSE", NavigateToSymbolic(robot, {table: table_close_navigation_area}, table), transitions={ 'arrived': 'PLACE_ITEM_ON_TABLE', 'unreachable': 'SAY_PICK_AWAY_THE_CHAIR', 'goal_not_defined': 'failed' }) StateMachine.add("FORCE_DRIVE", ForceDrive(robot, -0.1, 0, 0, 5.0), transitions={'done': 'SAY_PICK_AWAY_THE_CHAIR'}) StateMachine.add( "SAY_PICK_AWAY_THE_CHAIR", Say( robot, "Please pick away the chair, I cannot get close enough to the {}" .format(table_id)), transitions={'spoken': 'WAIT_FOR_PICK_AWAY_CHAIR'}) StateMachine.add('WAIT_FOR_PICK_AWAY_CHAIR', WaitTime(robot, 5), transitions={ 'waited': 'SAY_THANKS', 'preempted': 'failed' }) StateMachine.add('SAY_THANKS', Say(robot, "Thank you darling"), transitions={'spoken': 'NAVIGATE_TO_TABLE_CLOSE'}) StateMachine.add("PLACE_ITEM_ON_TABLE", PlaceItemOnTable(robot, table_id, placement_height), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' })
def __init__(self, robot, cupboard_id, cupboard_navigation_area, required_items): StateMachine.__init__(self, outcomes=["succeeded", "failed"], output_keys=["item_picked"]) cupboard = EdEntityDesignator(robot=robot, id=cupboard_id) with self: StateMachine.add("NAVIGATE_TO_CUPBOARD", NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area}, cupboard), transitions={'arrived': 'PICK_ITEM_FROM_CUPBOARD', 'unreachable': 'failed', 'goal_not_defined': 'failed'}) StateMachine.add("PICK_ITEM_FROM_CUPBOARD", PickItemFromCupboardDrawer(robot, cupboard_id, required_items), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
def __init__(self, robot, dishwasher_id, dishwasher_navigate_area): StateMachine.__init__(self, outcomes=["succeeded", "failed"]) dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id) with self: StateMachine.add("NAVIGATE_TO_DISHWASHER", NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher), transitions={'arrived': 'OPEN_DISHWASHER', 'unreachable': 'failed', 'goal_not_defined': 'failed'}) StateMachine.add("OPEN_DISHWASHER", OpenDishwasher(robot, dishwasher_id), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
def __init__(self, robot, rack_id, rack_navigation_area): StateMachine.__init__(self, outcomes=["succeeded", "failed"]) rack = EdEntityDesignator(robot=robot, id=rack_id) with self: StateMachine.add("NAVIGATE_TO_RACK", NavigateToSymbolic(robot, {rack: rack_navigation_area}, rack), transitions={ 'arrived': 'GRAB_RACK', 'unreachable': 'failed', 'goal_not_defined': 'failed' }) StateMachine.add("GRAB_RACK", GrabRack(robot, rack_id), transitions={'done': 'succeeded'})
def __init__(self, robot, cupboard_id, cupboard_navigation_area): StateMachine.__init__(self, outcomes=["succeeded", "failed"]) cupboard = EdEntityDesignator(robot=robot, id=cupboard_id) with self: StateMachine.add( "NAVIGATE_TO_CUPBOARD", NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area}, cupboard), transitions={ 'arrived': 'CLOSE_CUPBOARD', 'unreachable': 'failed', 'goal_not_defined': 'failed' }) StateMachine.add("CLOSE_CUPBOARD", CloseCupboard(robot, cupboard_id), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' })
def setup_statemachine(robot): furniture = EntityByIdDesignator(robot, 'selected_furniture') # arm_designator = ArmDesignator(robot.arms, robot.arms['left']) arm_designator = UnoccupiedArmDesignator(robot.arms, robot.arms['left']) sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: # Start challenge via StartChallengeRobust, skipped atm smach.StateMachine.add("START_CHALLENGE_ROBUST", StartChallengeRobust(robot, challenge_knowledge.initial_pose), transitions={"Done": "NAVIGATE_TO_SSL_WAYPOINT", "Failed": "Aborted", "Aborted": "Aborted"}) # smach.StateMachine.add('SAY_STARTING_OPEN_CHALLENGE', # robot_smach_states.Say(robot, ["Hi there, welcome to the open challenge!"], block=False), # transitions={"spoken": "NAVIGATE_TO_SSL_WAYPOINT"}) smach.StateMachine.add("NAVIGATE_TO_SSL_WAYPOINT", NavigateToWaypoint(robot=robot, waypoint_designator=EntityByIdDesignator( robot=robot, id=challenge_knowledge.ssl_waypoint), radius=0.3), transitions={'arrived': 'SSL_DEMO', 'unreachable': 'SSL_DEMO', 'goal_not_defined': 'SSL_DEMO'}) smach.StateMachine.add("SSL_DEMO", SSLDemo(robot), transitions={"done": "NAVIGATE_TO_LASER_DEMO", 'preempted': 'Aborted'}) smach.StateMachine.add("NAVIGATE_TO_LASER_DEMO", NavigateToWaypoint(robot=robot, waypoint_designator=EntityByIdDesignator( robot=robot, id=challenge_knowledge.raytrace_waypoint), radius=0.3), transitions={'arrived': 'RESET_HEAD_BEFORE_RAYTRACE_DEMO', 'unreachable': 'RESET_HEAD_BEFORE_RAYTRACE_DEMO', 'goal_not_defined': 'RESET_HEAD_BEFORE_RAYTRACE_DEMO'}) smach.StateMachine.add("RESET_HEAD_BEFORE_RAYTRACE_DEMO", ResetHead(robot), transitions={'done': 'SAY_PEOPLE_DETECTOR'}) smach.StateMachine.add("SAY_PEOPLE_DETECTOR", Say(robot, "Now I will show you my awesome people detector"), transitions={"spoken": "WAIT_FOR_TRIGGER_BEFORE_RAYTRACE_DEMO"}) smach.StateMachine.add("WAIT_FOR_TRIGGER_BEFORE_RAYTRACE_DEMO", WaitForTrigger(robot, ["continue"], "/amigo/trigger"), transitions={'continue': 'RAYTRACE_DEMO', 'preempted': 'RAYTRACE_DEMO'}) smach.StateMachine.add("RAYTRACE_DEMO", RayTraceDemo(robot, breakout_id=challenge_knowledge.raytrace_waypoint), transitions={"done": "SAY_RAYTRACE_SELECTOR"}) smach.StateMachine.add("SAY_RAYTRACE_SELECTOR", Say(robot, "You can interact with me by pointing at objects!"), transitions={"spoken": "RAYTRACE_SELECTOR"}) smach.StateMachine.add("RAYTRACE_SELECTOR", SimpleRayTraceSelector(robot, waypoint=None, furniture_designator=furniture), transitions={ "waypoint": 'NAVIGATE_TO_WAYPOINT', "furniture": 'NAVIGATE_TO_FURNITURE', "grasp": 'INSPECT_AND_GRAB', "done": 'RAYTRACE_SELECTOR' }) smach.StateMachine.add("NAVIGATE_TO_FURNITURE", NavigateToSymbolic(robot, entity_designator_area_name_map={furniture: 'in_front_of'}, entity_lookat_designator=furniture), transitions={ 'arrived': 'NAVIGATE_BACK_TO_LASER_DEMO', 'unreachable': 'NAVIGATE_BACK_TO_LASER_DEMO', 'goal_not_defined': 'NAVIGATE_BACK_TO_LASER_DEMO', }) smach.StateMachine.add("NAVIGATE_TO_WAYPOINT", NavigateToWaypoint(robot=robot, waypoint_designator=EntityByIdDesignator( robot=robot, id='final_waypoint'), radius=0.3), transitions={'arrived': 'NAVIGATE_BACK_TO_LASER_DEMO', 'unreachable': 'NAVIGATE_BACK_TO_LASER_DEMO', 'goal_not_defined': 'NAVIGATE_BACK_TO_LASER_DEMO'}) smach.StateMachine.add("INSPECT_AND_GRAB", InspectAndGrab(robot, supporting_entity_designator=furniture, arm_designator=arm_designator), transitions={ 'succeeded': 'NAVIGATE_BACK_TO_LASER_DEMO', 'inspect_failed': 'NAVIGATE_BACK_TO_LASER_DEMO', 'grasp_failed': 'NAVIGATE_BACK_TO_LASER_DEMO' }) smach.StateMachine.add("HANDOVER_TO_HUMAN", HandoverToHuman(robot, arm_designator, timeout=10), transitions={ 'succeeded': 'NAVIGATE_BACK_TO_LASER_DEMO', 'failed': 'NAVIGATE_BACK_TO_LASER_DEMO', }) smach.StateMachine.add("NAVIGATE_BACK_TO_LASER_DEMO", NavigateToWaypoint(robot=robot, waypoint_designator=EntityByIdDesignator( robot=robot, id=challenge_knowledge.raytrace_waypoint), radius=0.3), transitions={'arrived': 'RAYTRACE_SELECTOR', 'unreachable': 'RAYTRACE_SELECTOR', 'goal_not_defined': 'RAYTRACE_SELECTOR'}) # smach.StateMachine.add("NAVIGATE_TO_ORDER_COUNTER", # robot_smach_states.NavigateToWaypoint(robot=robot, # waypoint_designator=EntityByIdDesignator( # robot=robot, # id=challenge_knowledge.order_counter_waypoint), # radius=0.3), # transitions={'arrived': 'ORDER_COUNTER', # 'unreachable': 'ORDER_COUNTER', # 'goal_not_defined': 'ORDER_COUNTER'}) # # smach.StateMachine.add("ORDER_COUNTER", # OrderCounter(robot, room_id=challenge_knowledge.audience_room, # beercounter=beercounter), # transitions={"done": "NAVIGATE_TO_HSR_DEMO"}) # # smach.StateMachine.add("NAVIGATE_TO_HSR_DEMO", # robot_smach_states.NavigateToWaypoint(robot=robot, # waypoint_designator=EntityByIdDesignator( # robot=robot, # id=challenge_knowledge.hsr_demo_waypoint), # radius=0.025), # transitions={'arrived': 'WAIT_FOR_BEER', # 'unreachable': 'CANNOT_REACH_BEER_LOCATION', # 'goal_not_defined': 'CANNOT_REACH_BEER_LOCATION'}) # # smach.StateMachine.add("CANNOT_REACH_BEER_LOCATION", # robot_smach_states.Say(robot, "I cannot reach my beer buddy, let's give it another try"), # transitions={"spoken": "Done"}) # # smach.StateMachine.add("WAIT_FOR_BEER", # HsrInteraction(robot=robot, beercounter=beercounter), # transitions={"done": "RETURN_TO_AUDIENCE"}) # # smach.StateMachine.add("RETURN_TO_AUDIENCE", # robot_smach_states.NavigateToWaypoint(robot=robot, # waypoint_designator=EntityByIdDesignator( # robot=robot, # id=challenge_knowledge.order_counter_waypoint), # radius=0.3), # transitions={'arrived': 'SAY_BEER', # 'unreachable': 'SAY_BEER', # 'goal_not_defined': 'SAY_BEER'}) # # smach.StateMachine.add("SAY_BEER", # robot_smach_states.Say(robot, "Hey guys, here's your beer. That will be all. " # "By the way, if you leave the balcony door open," # "birds will fly in"), # transitions={"spoken": "Done"}) return sm