def __init__(self, robot, item): StateMachine.__init__(self, outcomes=['found', 'not_found']) source_entity_designator = EdEntityDesignator(robot, id=source_entity) # description_designator = VariableDesignator({ # 'type': 'cup' # }) area_name_designator = VariableDesignator('on_top_of') navigation_area_designator1 = VariableDesignator('in_back_of') navigation_area_designator2 = VariableDesignator('in_front_of') with self: StateMachine.add('FIND', CustomFind(robot, source_entity_designator, area_name_designator, navigation_area_designator1, item), transitions={'found': 'found', 'not_found': 'FIND2'}) StateMachine.add('FIND2', CustomFind(robot, source_entity_designator, area_name_designator, navigation_area_designator2, item), transitions={'found': 'found', 'not_found': 'FIND3'}) StateMachine.add('FIND3', CustomFind(robot, source_entity_designator, area_name_designator, navigation_area_designator1, item), transitions={'found': 'found', 'not_found': 'FIND4'}) StateMachine.add('FIND4', CustomFind(robot, source_entity_designator, area_name_designator, navigation_area_designator2, item), transitions={'found': 'found', 'not_found': 'not_found'})
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 __init__(self, robot, knowledge, source_entity_designator, description_designator, area_name_designator, navigation_area_designator, found_entity_designator): smach.StateMachine.__init__(self, outcomes=['succeeded', 'inspect_failed', 'not_found']) segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult]) with self: smach.StateMachine.add('INSPECT_SOURCE_ENTITY', states.world_model.Inspect(robot=robot, entityDes=source_entity_designator, objectIDsDes=segmented_entities_designator, searchArea=area_name_designator, navigation_area=navigation_area_designator), transitions={'done': 'CHECK_IF_ENTITY_FOUND', 'failed': 'inspect_failed'}) smach.StateMachine.add('CHECK_IF_ENTITY_FOUND', CheckIfDescribedEntityAvailable( robot=robot, knowledge=knowledge, description_designator=description_designator, found_entity_designator=found_entity_designator.writeable, candidate_entities_designator=segmented_entities_designator), transitions={'succeeded': 'succeeded', 'failed': 'not_found'})
def __init__(self, robot, supporting_entity_designator, arm_designator): smach.StateMachine.__init__( self, outcomes=['succeeded', 'inspect_failed', 'grasp_failed']) self._robot = robot self._supporting_entity_designator = supporting_entity_designator self._arm_designator = arm_designator self._classification_result_designator = VariableDesignator( [], resolve_type=[ClassificationResult]) self._grasp_entity_designator = VariableDesignator(resolve_type=Entity) with self: smach.StateMachine.add( 'INSPECT', states.Inspect( robot=self._robot, entityDes=self._supporting_entity_designator, objectIDsDes=self._classification_result_designator, searchArea="on_top_of", navigation_area="in_front_of"), transitions={ 'done': 'SELECT_EASIEST_GRASP_ENTITY', 'failed': 'inspect_failed' }) smach.StateMachine.add( 'SELECT_EASIEST_GRASP_ENTITY', SelectEasiestGraspEntity(robot=self._robot, classification_result_designator=self. _classification_result_designator, grasp_entity_designator=self. _grasp_entity_designator.writeable), transitions={ 'selected': 'GRAB', 'failed': 'inspect_failed' }) smach.StateMachine.add('GRAB', states.Grab( robot=self._robot, item=self._grasp_entity_designator, arm=self._arm_designator), transitions={ 'done': 'succeeded', 'failed': 'grasp_failed' })
def __init__(self, robot, source_entity_designator, area_name_designator, navigation_area_designator, found_entity_designator): StateMachine.__init__(self, outcomes=['found', 'not_found']) segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult]) with self: StateMachine.add('SAY_LOOKING', Say(robot, ["I'm going to look for the cup"], block=False), transitions={'spoken': 'INSPECT_SOURCE_ENTITY'}) StateMachine.add('INSPECT_SOURCE_ENTITY', Inspect(robot=robot, entityDes=source_entity_designator, objectIDsDes=segmented_entities_designator, searchArea=area_name_designator, navigation_area=navigation_area_designator), transitions={'done': 'GET_CUP_ID', 'failed': 'not_found'}) StateMachine.add('GET_CUP_ID', GetCupId(robot=robot, found_entity_designator=found_entity_designator.writeable, candidate_entities_designator=segmented_entities_designator), transitions={'succeeded': 'found', 'failed': 'not_found'})
# import states from another file import test_states as test_states # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # INITIALIZATIONS # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # load knowledge common_knowledge = load_knowledge("common") challenge_knowledge = load_knowledge("challenge_test") # define print shortcuts from common knowledge printOk, printError, printWarning = common_knowledge.make_prints( "[Challenge Test] ") personNameDes = VariableDesignator("", resolve_type=str) objectsIDsDes = VariableDesignator([], resolve_type=[ClassificationResult]) containerResultDes = VariableDesignator(0, resolve_type=int) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # NAVIGATION_CONTAINER # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - class EnterRoomContainer(smach.StateMachine): def __init__(self, robot): smach.StateMachine.__init__( self, outcomes=['container_success', 'container_failed']) with self: smach.StateMachine.add('SAY_STARTING_TEST',
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 __init__(self, robot, location_des): """ Visit all selected locations from the list, and handle the found objects :param location_des: is a designator resolving to a dictionary with fields ... TODO """ smach.StateMachine.__init__(self, outcomes=['done']) segment_areas = ValueByKeyDesignator(location_des, "segment_areas", [str], name='segment_areas') segment_area = VariableDesignator(resolve_type=str, name='segment_area') navigation_area_des = ValueByKeyDesignator(location_des, key='navigation_area', resolve_type=str, name='navigation_area') location_id_des = ValueByKeyDesignator(location_des, key='name', resolve_type=str, name='location_id') room_des = EdEntityDesignator(robot, id_designator=ValueByKeyDesignator( location_des, key="room", resolve_type=str)) # Set up the designators for this machine e_classifications_des = VariableDesignator( [], resolve_type=[ClassificationResult], name='e_classifications_des') e_des = EdEntityDesignator(robot, id_designator=location_id_des, name='e_des') with self: smach.StateMachine.add('ITERATE_NEXT_AREA', IterateDesignator(segment_areas, segment_area.writeable), transitions={ "next": 'INSPECT', "stop_iteration": "done" }) # Segment the area and handle segmented objects for the specified navigation area # for i, segment_area in enumerate(segment_areas): smach.StateMachine.add( 'INSPECT', NavigateToSymbolic(robot, {e_des: navigation_area_des}, e_des), transitions={ 'arrived': 'SEGMENT_SAY', 'unreachable': "SAY_UNREACHABLE", 'goal_not_defined': "SAY_UNREACHABLE" }) smach.StateMachine.add("SEGMENT_SAY", Say(robot, "Looking {area} the {entity}", area=segment_area, entity=location_id_des, block=False), transitions={"spoken": "RISE"}) smach.StateMachine.add('RISE', RiseForInspect(robot, e_des, segment_area), transitions={ 'succeeded': 'SEGMENT', 'failed': 'SEGMENT' }) smach.StateMachine.add( 'SEGMENT', SegmentObjects(robot, e_classifications_des.writeable, e_des, segment_area), transitions={'done': "HANDLE_DETECTED_ENTITIES"}) smach.StateMachine.add("SAY_UNREACHABLE", Say(robot, "I failed to inspect the {furn}", furn=location_id_des, block=True), transitions={"spoken": "done"}) smach.StateMachine.add("HANDLE_DETECTED_ENTITIES", HandleDetectedEntities( robot, e_classifications_des, location_id_des, segment_area, room_des), transitions={"done": "ITERATE_NEXT_AREA"})
robot.get_arm().send_joint_goal("reset") # Fill the designator and user data the furniture inspection furniture_designator.write( robot.ed.get_entity(final_result.entity_id)) user_data['laser_dot'] = result.intersection_point return 'done' with self: self.add('PREPARE_OPERATOR', CBState(_prepare_operator), transitions={'done': 'GET_OPERATOR'}) self.add('GET_OPERATOR', CBState(_get_operator), transitions={'done': 'GET_FURNITURE'}) self.add('GET_FURNITURE', CBState(_get_furniture), transitions={ 'done': 'done', 'failed': 'GET_OPERATOR' }) if __name__ == '__main__': rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0]) furniture_designator = VariableDesignator(resolve_type=Entity) hero = Hero() hero.reset() while not rospy.is_shutdown(): GetFurnitureFromOperatorPose(hero, furniture_designator.writeable).execute()
def setup_statemachine(robot): item = VariableDesignator(resolve_type=Entity) arm = Designator(robot.leftArm) sm = StateMachine(outcomes=['done']) with sm: # Start challenge via StartChallengeRobust StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT, use_entry_points=True), transitions={'Done': 'OPEN_DISHWASHER', 'Aborted': 'done', 'Failed': 'OPEN_DISHWASHER'}) StateMachine.add('OPEN_DISHWASHER', NavigateAndOpenDishwasher(robot), transitions={'succeeded': 'GRAB', 'failed': 'GRAB'}) # StateMachine.add('FIND_OBJECT', FindObject(robot, item), # transitions={'found': 'GRAB', # 'not_found': 'SAY_EXIT'}) # # StateMachine.add('GRAB', Grab(robot, item, arm), # transitions={'done': 'NAVIGATE_BACK_TO_DISHWASHER', # 'failed': 'FIND_OBJECT2'}) # # StateMachine.add('FIND_OBJECT2', FindObject(robot, item), # transitions={'found': 'GRAB2', # 'not_found': 'SAY_EXIT'}) # # StateMachine.add('GRAB2', Grab(robot, item, arm), # transitions={'done': 'NAVIGATE_BACK_TO_DISHWASHER', # 'failed': 'SAY_EXIT'}) StateMachine.add('GRAB', GrabRobust(robot), transitions={'succeeded': 'PLACE_DISHWASHER', 'failed': 'SAY_EXIT'}) StateMachine.add('PLACE_DISHWASHER', NavigateAndPlaceDishwasher(robot), transitions={'succeeded': 'SAY_EXIT', 'failed': 'SAY_EXIT'}) StateMachine.add('SAY_EXIT', Say(robot, ["I will move to the exit now. See you guys later!"], block=False), transitions={'spoken': 'GO_TO_EXIT'}) StateMachine.add('GO_TO_EXIT', NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_1), radius=0.7), transitions={'arrived': 'done', 'unreachable': 'GO_TO_EXIT_2', 'goal_not_defined': 'GO_TO_EXIT_2'}) StateMachine.add('GO_TO_EXIT_2', NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_2), radius=0.5), transitions={'arrived': 'done', 'unreachable': 'GO_TO_EXIT_3', 'goal_not_defined': 'GO_TO_EXIT_3'}) StateMachine.add('GO_TO_EXIT_3', NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_3), radius=0.5), transitions={'arrived': 'done', 'unreachable': 'done', 'goal_not_defined': 'done'}) return sm
def __init__(self, robot, ask_follow=True, learn_face=True, operator_radius=1, lookat_radius=1.2, timeout=1.0, start_timeout=10, operator_timeout=20, distance_threshold=None, lost_timeout=60, lost_distance=0.8, operator_id_des=VariableDesignator(resolve_type=str), standing_still_timeout=20, operator_standing_still_timeout=3.0, replan=False): """ Constructor :param robot: robot object :param ask_follow: :param learn_face: :param operator_radius: :param lookat_radius: :param timeout: :param start_timeout: :param operator_timeout: :param distance_threshold: :param lost_timeout: How long to look for the operator when we lost him/her? :param lost_distance: :param operator_id_des: :param standing_still_timeout: :param operator_standing_still_timeout: :param replan: """ smach.State.__init__( self, outcomes=["stopped", 'lost_operator', "no_operator"]) self._robot = robot self._time_started = None self._operator = None self._operator_id = None self._operator_name = "operator" self._operator_radius = operator_radius self._lookat_radius = lookat_radius self._start_timeout = start_timeout self._breadcrumbs = [] # List of Entity's self._breadcrumb_distance = 0.1 # meters between dropped breadcrumbs self._operator_timeout = operator_timeout self._ask_follow = ask_follow self._learn_face = learn_face self._lost_timeout = lost_timeout self._lost_distance = lost_distance self._standing_still_timeout = standing_still_timeout self._operator_standing_still_timeout = operator_standing_still_timeout self._operator_id_des = operator_id_des self._operator_distance = None self._operator_pub = rospy.Publisher( '/%s/follow_operator/operator_position' % robot.robot_name, geometry_msgs.msg.PointStamped, queue_size=10) self._plan_marker_pub = rospy.Publisher( '/%s/global_planner/visualization/markers/global_plan' % robot.robot_name, Marker, queue_size=10) self._breadcrumb_pub = rospy.Publisher( '/%s/follow_operator/breadcrumbs' % robot.robot_name, Marker, queue_size=10) self._face_pos_pub = rospy.Publisher( '/%s/follow_operator/operator_detected_face' % robot.robot_name, geometry_msgs.msg.PointStamped, queue_size=10) self._last_pose_stamped = None self._last_pose_stamped_time = None self._last_operator_fs = None self._replan_active = False self._last_operator = None self._replan_allowed = replan self._replan_timeout = 15 # seconds before another replan is allowed self._replan_time = rospy.Time.now() - rospy.Duration( self._replan_timeout) self._replan_attempts = 0 self._max_replan_attempts = 3 self._period = 0.5
def __init__(self, robot, location_id, room_id, navigate_area, segment_areas, known_types): smach.StateMachine.__init__(self, outcomes=['done']) # Set up the designators for this machine e_classifications_des = VariableDesignator( [], resolve_type=[ClassificationResult], name='e_classifications_des') e_des = EdEntityDesignator(robot, id=location_id, name='e_des') room_des = EntityByIdDesignator(robot, id=room_id, name='room_des') with self: # Loop over the areas that we have to segment and handle segmented objects for the specified navigation area for i, segment_area in enumerate(segment_areas): smach.StateMachine.add("RESET_ED_%d" % i, robot_smach_states.ResetED(robot), transitions={'done': 'NAVIGATE_%d' % i}) smach.StateMachine.add("NAVIGATE_%d" % i, robot_smach_states.NavigateToSymbolic( robot, { e_des: navigate_area, room_des: "in" }, e_des), transitions={ 'arrived': 'SEGMENT_SAY_%d' % i, 'unreachable': "SAY_UNREACHABLE_%d" % i, 'goal_not_defined': "SAY_UNREACHABLE_%d" % i }) smach.StateMachine.add( "SEGMENT_SAY_%d" % i, robot_smach_states.Say( robot, ["Looking %s the %s" % (segment_area, location_id)], block=False), transitions={"spoken": "SEGMENT_%d" % i}) smach.StateMachine.add( 'SEGMENT_%d' % i, robot_smach_states.SegmentObjects( robot, e_classifications_des.writeable, e_des, segment_area), transitions={'done': "HANDLE_DETECTED_ENTITIES_%d" % i}) # Determine the next state, either it is the next iter or done next_state = "RESET_ED_%d" % ( i + 1) if i + 1 < len(segment_areas) else "done" smach.StateMachine.add( "SAY_UNREACHABLE_%d" % i, robot_smach_states.Say( robot, ["I failed to inspect the %s" % location_id], block=True), transitions={"spoken": next_state}) smach.StateMachine.add("HANDLE_DETECTED_ENTITIES_%d" % i, HandleDetectedEntities( robot, e_classifications_des, known_types, location_id, segment_area), transitions={"done": next_state})
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: # Start challenge via StartChallengeRobust smach.StateMachine.add("START_CHALLENGE_ROBUST", states.StartChallengeRobust( robot, challenge_knowledge.starting_point, use_entry_points=True), transitions={ "Done": "SAY_GOTO_TARGET2", "Aborted": "SAY_GOTO_TARGET2", "Failed": "SAY_GOTO_TARGET2" }) smach.StateMachine.add( 'SAY_GOTO_TARGET1', states.Say(robot, [ "I will go to target 1 now", "I will now go to target 1", "Lets go to target 1", "Going to target 1" ], block=False), transitions={'spoken': 'GOTO_TARGET1'}) ###################################################################################################################################################### # # TARGET 1 # ###################################################################################################################################################### smach.StateMachine.add('GOTO_TARGET1', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target1), challenge_knowledge.target1_radius1), transitions={ 'arrived': 'SAY_TARGET1_REACHED', 'unreachable': 'RESET_ED_TARGET1', 'goal_not_defined': 'RESET_ED_TARGET1' }) smach.StateMachine.add( 'SAY_TARGET1_REACHED', states.Say(robot, [ "Reached target 1", "I have arrived at target 1", "I am now at target 1" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET3'}) smach.StateMachine.add('RESET_ED_TARGET1', states.ResetED(robot), transitions={'done': 'GOTO_TARGET1_BACKUP'}) smach.StateMachine.add('GOTO_TARGET1_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target1), challenge_knowledge.target1_radius2), transitions={ 'arrived': 'SAY_TARGET1_REACHED', 'unreachable': 'TIMEOUT1', 'goal_not_defined': 'TIMEOUT1' }) smach.StateMachine.add('TIMEOUT1', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'GOTO_TARGET1', 'time_out': 'SAY_TARGET1_FAILED' }) # Should we mention that we failed??? smach.StateMachine.add( 'SAY_TARGET1_FAILED', states.Say(robot, [ "I am not able to reach target 1", "I cannot reach target 1", "Target 1 is unreachable" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET3'}) ###################################################################################################################################################### # # TARGET 2 # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOTO_TARGET2', states.Say(robot, [ "I will go to target 2 now", "I will now go to target 2", "Lets go to target 2", "Going to target 2" ], block=False), transitions={'spoken': 'GOTO_TARGET2_PRE'}) smach.StateMachine.add( 'GOTO_TARGET2_PRE', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=challenge_knowledge.target2_pre), challenge_knowledge.target2_pre_radius1, EntityByIdDesignator(robot, id=challenge_knowledge.target2)), transitions={ 'arrived': 'GOTO_TARGET2', 'unreachable': 'TIMEOUT2', 'goal_not_defined': 'TIMEOUT2' }) smach.StateMachine.add('GOTO_TARGET2', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target2), challenge_knowledge.target2_radius1), transitions={ 'arrived': 'SAY_TARGET2_REACHED', 'unreachable': 'DETERMINE_OBJECT', 'goal_not_defined': 'DETERMINE_OBJECT' }) smach.StateMachine.add( 'DETERMINE_OBJECT', DetermineObject(robot, challenge_knowledge.target2, challenge_knowledge.target2_obstacle_radius), transitions={ 'done': 'GOTO_TARGET2_AGAIN', 'timeout': 'GOTO_TARGET2_AGAIN' }) smach.StateMachine.add('GOTO_TARGET2_AGAIN', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target2), challenge_knowledge.target2_radius1), transitions={ 'arrived': 'SAY_TARGET2_REACHED', 'unreachable': 'RESET_ED_TARGET2', 'goal_not_defined': 'RESET_ED_TARGET2' }) smach.StateMachine.add( 'SAY_TARGET2_REACHED', states.Say(robot, [ "Reached target 2", "I have arrived at target 2", "I am now at target 2" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET1'}) smach.StateMachine.add('RESET_ED_TARGET2', states.ResetED(robot), transitions={'done': 'GOTO_TARGET2_BACKUP'}) smach.StateMachine.add('GOTO_TARGET2_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target2), challenge_knowledge.target2_radius2), transitions={ 'arrived': 'SAY_TARGET2_REACHED', 'unreachable': 'TIMEOUT2', 'goal_not_defined': 'TIMEOUT2' }) smach.StateMachine.add('TIMEOUT2', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'GOTO_TARGET2_PRE', 'time_out': 'SAY_TARGET2_FAILED' }) smach.StateMachine.add( 'SAY_TARGET2_FAILED', states.Say(robot, [ "I am unable to reach target 2", "I cannot reach target 2", "Target 2 is unreachable" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET1'}) ###################################################################################################################################################### # # TARGET 3 # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOTO_TARGET3', states.Say(robot, [ "I will go to target 3 now", "I will now go to target 3", "Lets go to target 3", "Going to target 3" ], block=False), transitions={'spoken': 'GOTO_TARGET3'}) smach.StateMachine.add('GOTO_TARGET3', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target3), challenge_knowledge.target3_radius1), transitions={ 'arrived': 'SAY_TARGET3_REACHED', 'unreachable': 'RESET_ED_TARGET3', 'goal_not_defined': 'RESET_ED_TARGET3' }) smach.StateMachine.add( 'SAY_TARGET3_REACHED', states.Say(robot, [ "Reached target 3", "I have arrived at target 3", "I am now at target 3" ], block=True), transitions={'spoken': 'TURN'}) smach.StateMachine.add('RESET_ED_TARGET3', states.ResetED(robot), transitions={'done': 'GOTO_TARGET3_BACKUP'}) smach.StateMachine.add('GOTO_TARGET3_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target3), challenge_knowledge.target3_radius2), transitions={ 'arrived': 'SAY_TARGET3_REACHED', 'unreachable': 'TIMEOUT3', 'goal_not_defined': 'TIMEOUT3' }) smach.StateMachine.add('TIMEOUT3', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'GOTO_TARGET3', 'time_out': 'SAY_TARGET3_FAILED' }) # Should we mention that we failed??? smach.StateMachine.add( 'SAY_TARGET3_FAILED', states.Say(robot, [ "I am unable to reach target 3", "I cannot reach target 3", "Target 3 is unreachable" ], block=True), transitions={'spoken': 'TURN'}) ###################################################################################################################################################### # # Follow waiter # ###################################################################################################################################################### smach.StateMachine.add('TURN', Turn(robot, challenge_knowledge.rotation), transitions={'turned': 'SAY_STAND_IN_FRONT'}) smach.StateMachine.add( 'SAY_STAND_IN_FRONT', states.Say(robot, "Please stand in front of me!", block=True, look_at_standing_person=True), transitions={'spoken': 'FOLLOW_WITH_DOOR_CHECK'}) # TODO: Fix concurrence door_id_designator = VariableDesignator( challenge_knowledge.target_door_1) open_door_wp1_des = VariableDesignator(resolve_type=str) open_door_wp2_des = VariableDesignator(resolve_type=str) cc = smach.Concurrence( ['stopped', 'no_operator', 'lost_operator'], default_outcome='no_operator', child_termination_cb=lambda so: True, outcome_map={ 'stopped': { 'FOLLOW_OPERATOR': 'stopped' }, # 'stopped': {'FOLLOW_OPERATOR': 'stopped', 'DETERMINE_DOOR': 'door_found'}, 'no_operator': { 'FOLLOW_OPERATOR': 'no_operator' }, # 'no_operator': {'FOLLOW_OPERATOR': 'no_operator', 'DETERMINE_DOOR': 'door_found'}, # 'lost_operator': {'FOLLOW_OPERATOR': 'lost_operator', 'DETERMINE_DOOR': 'preempted'}, 'lost_operator': { 'FOLLOW_OPERATOR': 'lost_operator' } }) with cc: smach.Concurrence.add('FOLLOW_OPERATOR', states.FollowOperator(robot, replan=True)) smach.Concurrence.add('DETERMINE_DOOR', DetermineDoor(robot, door_id_designator)) smach.StateMachine.add('FOLLOW_WITH_DOOR_CHECK', cc, transitions={ 'no_operator': 'FOLLOW_WITH_DOOR_CHECK', 'stopped': 'SAY_SHOULD_I_RETURN', 'lost_operator': 'SAY_SHOULD_I_RETURN' }) # smach.StateMachine.add( 'FOLLOW_OPERATOR', states.FollowOperator(robot, replan=True), transitions={ 'no_operator':'SAY_SHOULD_I_RETURN', 'stopped' : 'SAY_SHOULD_I_RETURN', 'lost_operator' : 'SAY_SHOULD_I_RETURN'}) smach.StateMachine.add('SAY_SHOULD_I_RETURN', states.Say(robot, "Should I return to target 3?", look_at_standing_person=True), transitions={'spoken': 'HEAR_SHOULD_I_RETURN'}) smach.StateMachine.add('HEAR_SHOULD_I_RETURN', states.HearOptions(robot, ["yes", "no"]), transitions={ 'no_result': 'SAY_STAND_IN_FRONT', "yes": "SELECT_WAYPOINTS", "no": "SAY_STAND_IN_FRONT" }) smach.StateMachine.add('SELECT_WAYPOINTS', SelectWaypoints(door_id_designator, open_door_wp1_des, open_door_wp2_des), transitions={'done': 'SAY_GOBACK_ARENA'}) ###################################################################################################################################################### # # RETURN TO ARENA DOOR # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOBACK_ARENA', states.Say(robot, [ "I will go back to the arena", "I will return to the arena", "Lets return to the arena", "Going back to the arena", "Returning to the arena" ], block=False), transitions={'spoken': 'GOTO_ARENA_DOOR'}) smach.StateMachine.add( 'GOTO_ARENA_DOOR', states.NavigateToWaypoint( robot, EdEntityDesignator(robot, id_designator=door_id_designator), challenge_knowledge.target_door_radius), transitions={ 'arrived': 'ARENA_DOOR_REACHED', 'unreachable': 'RESET_ED_ARENA_DOOR', 'goal_not_defined': 'RESET_ED_ARENA_DOOR' }) smach.StateMachine.add('ARENA_DOOR_REACHED', states.Say(robot, [ "I am at the door of the arena", "I have arrived at the door of the arena", "I am now at the door of the arena" ], block=True), transitions={'spoken': 'SAY_OPEN_DOOR'}) smach.StateMachine.add('RESET_ED_ARENA_DOOR', states.ResetED(robot), transitions={'done': 'GOTO_ARENA_DOOR_BACKUP'}) smach.StateMachine.add( 'GOTO_ARENA_DOOR_BACKUP', states.NavigateToWaypoint( robot, EdEntityDesignator(robot, id_designator=door_id_designator), challenge_knowledge.target_door_radius), transitions={ 'arrived': 'ARENA_DOOR_REACHED', 'unreachable': 'TIMEOUT_ARENA_DOOR', 'goal_not_defined': 'TIMEOUT_ARENA_DOOR' }) smach.StateMachine.add('TIMEOUT_ARENA_DOOR', checkTimeOut( robot, challenge_knowledge.time_out_seconds_door), transitions={ 'not_yet': 'GOTO_ARENA_DOOR', 'time_out': 'SAY_GOTO_ARENA_DOOR_FAILED' }) smach.StateMachine.add('SAY_GOTO_ARENA_DOOR_FAILED', states.Say(robot, [ "I am unable to reach the arena door", "I cannot reach the arena door", "The arena door is unreachable" ], block=True), transitions={'spoken': 'Done'}) ###################################################################################################################################################### # # Opening Door # ###################################################################################################################################################### smach.StateMachine.add( 'OPEN_DOOR', states.OpenDoorByPushing( robot, EdEntityDesignator(robot, id_designator=open_door_wp1_des), EdEntityDesignator(robot, id_designator=open_door_wp2_des)), transitions={ 'succeeded': 'SAY_RETURN_TARGET3', 'failed': 'TIMEOUT_ARENA_DOOR_OPENING' }) smach.StateMachine.add( 'SAY_OPEN_DOOR', states.Say(robot, [ "I am going to open the door", "Going to open the door of the arena", "Door, open sesame" ], block=True), transitions={'spoken': 'OPEN_DOOR'}) smach.StateMachine.add( 'SAY_OPEN_DOOR_AGAIN', states.Say(robot, [ "I failed to open the door. I will try it again", "Let me try again to open the door" ], block=True), transitions={'spoken': 'OPEN_DOOR'}) smach.StateMachine.add('TIMEOUT_ARENA_DOOR_OPENING', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'SAY_OPEN_DOOR_AGAIN', 'time_out': 'SAY_OPEN_DOOR_FAILED' }) smach.StateMachine.add( 'SAY_OPEN_DOOR_FAILED', states.Say(robot, [ "I was not able to open the door. I am done with this challange", "I was not able to open the door. I am done with this challange" ], block=True), transitions={'spoken': 'Done'}) ###################################################################################################################################################### # # RETURN TO TARGET 3 # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_RETURN_TARGET3', states.Say(robot, [ "I will go back to target 3 now", "I will return to target 3", "Lets go to target 3 again", "Going to target 3, again" ], block=False), transitions={'spoken': 'RETURN_TARGET3'}) smach.StateMachine.add( 'RETURN_TARGET3', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=challenge_knowledge.target4), challenge_knowledge.target4_radius1), transitions={ 'arrived': 'SAY_TARGET3_RETURN_REACHED', 'unreachable': 'RESET_ED_RETURN_TARGET3', 'goal_not_defined': 'RESET_ED_RETURN_TARGET3' }) smach.StateMachine.add( 'SAY_TARGET3_RETURN_REACHED', states.Say(robot, [ "Reached target 3 again", "I have arrived at target 3 again", "I am now at target 3 again" ], block=True), transitions={'spoken': 'SAY_GOTO_EXIT'}) smach.StateMachine.add( 'RESET_ED_RETURN_TARGET3', states.ResetED(robot), transitions={'done': 'GOTO_RETURN_TARGET3_BACKUP'}) smach.StateMachine.add('GOTO_RETURN_TARGET3_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target4), challenge_knowledge.target4_radius2), transitions={ 'arrived': 'SAY_TARGET3_RETURN_REACHED', 'unreachable': 'TIMEOUT3_RETURN', 'goal_not_defined': 'TIMEOUT3_RETURN' }) smach.StateMachine.add('TIMEOUT3_RETURN', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'RETURN_TARGET3', 'time_out': 'SAY_RETURN_TARGET3_FAILED' }) # Should we mention that we failed??? smach.StateMachine.add( 'SAY_RETURN_TARGET3_FAILED', states.Say(robot, [ "I am unable to reach target 3 again", "I cannot reach target 3 again", "Target 3 is unreachable" ], block=True), transitions={'spoken': 'SAY_GOTO_EXIT'}) ###################################################################################################################################################### # # TARGET EXIT # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOTO_EXIT', states.Say(robot, [ "I will move to the exit now. See you guys later!", "I am done with this challenge. Going to the exit" ], block=False), transitions={'spoken': 'GO_TO_EXIT'}) # Amigo goes to the exit (waypoint stated in knowledge base) smach.StateMachine.add('GO_TO_EXIT', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit1), radius=0.6), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_EXIT', 'goal_not_defined': 'RESET_ED_EXIT' }) smach.StateMachine.add('RESET_ED_EXIT', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT_BACKUP'}) smach.StateMachine.add('GO_TO_EXIT_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit2), radius=0.6), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_EXIT2', 'goal_not_defined': 'RESET_ED_EXIT2' }) smach.StateMachine.add('RESET_ED_EXIT2', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT_BACKUP2'}) smach.StateMachine.add('GO_TO_EXIT_BACKUP2', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit3), radius=0.6), transitions={ 'arrived': 'GO_TO_EXIT_BACKUP3', 'unreachable': 'RESET_ED_EXIT3', 'goal_not_defined': 'RESET_ED_EXIT3' }) smach.StateMachine.add('RESET_ED_EXIT3', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT_BACKUP3'}) smach.StateMachine.add('GO_TO_EXIT_BACKUP3', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit4), radius=0.6), transitions={ 'arrived': 'AT_END', 'unreachable': 'AT_END', 'goal_not_defined': 'AT_END' }) smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) analyse_designators(sm, "navigation") return sm
def __init__(self, robot, source_location, source_navArea, target_location, target_navArea, target_placeArea="on_top_of", source_searchArea="on_top_of"): """ Let the given robot move to a location and remove all entities from that table one at a time :param robot: Robot to use :param source_location: Location which will be cleared :param target_location: Location where the objects will be placed :return: """ smach.StateMachine.__init__(self, outcomes=['done', 'failed']) # Check types or designator resolve types #check_type(source_location, Entity) #check_type(target_location, Entity) segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult]) selected_entity_designator = EntityByIdDesignator(robot, "TBD", name='selected_entity_designator', ) arm_des = UnoccupiedArmDesignator(robot, {}).lockable() arm_des.lock() place_position = states.util.designators.EmptySpotDesignator(robot, EdEntityDesignator( robot, id=target_location.id), area="on_top_of" ) with self: smach.StateMachine.add('INSPECT_SOURCE_ENTITY', states.Inspect(robot, source_location, objectIDsDes=segmented_entities_designator, searchArea=source_searchArea, navigation_area=source_navArea), transitions={'done': 'DETERMINE_IF_CLEAR', 'failed': 'failed'} ) #smach.StateMachine.add('DETERMINE_IF_CLEAR', # isitclear(robot=robot, # objectIDsDes=segmented_entities_designator), # transitions={'clear': 'done', # 'not_clear': 'failed'}) smach.StateMachine.add('DETERMINE_IF_CLEAR', SelectEntity(robot, segmented_entities_designator, selected_entity_designator), transitions={'no_entities_left': 'done', 'entity_selected': 'GRAB'} ) smach.StateMachine.add('GRAB', states.Grab(robot, selected_entity_designator, arm_des), transitions={'done': 'INSPECT_TARGET', 'failed': 'failed'} ) smach.StateMachine.add('INSPECT_TARGET', states.Inspect(robot, target_location, searchArea=target_placeArea, navigation_area=target_navArea), transitions={'done': 'PLACE', 'failed': 'failed'} ) smach.StateMachine.add('PLACE', states.Place(robot, selected_entity_designator, place_position, arm_des), transitions={'done': 'INSPECT_SOURCE_ENTITY', 'failed': 'failed'} )