def api_load_unload(self, wht: WarehouseTask) -> RobotMission: """Load or unload HU of a warehouse task.""" # Default mission mission = RobotMission() # Get relevant parameters if wht.vlpla: action = {'getTrolley': {'dockName': wht.vlpla}} elif wht.nlpla: action = {'returnTrolley': {'dockName': wht.nlpla}} else: _LOGGER.error( 'Neither source nor target bin in warehouse task') return mission spec = {'actions': [action]} mission_name = str(time.time()) # Create CR success = self.create_cr(mission_name, self.labels, spec) # On success, set ID and STATE if success: mission.name = mission_name mission.status = RobotMission.STATE_ACCEPTED return mission
def __init__(self, robot_config: RobotConfigurationController, robot_mission_api: RobotMissionAPI, confirm_api: Callable, request_ewm_work_api: Callable, send_wht_error_api: Callable, notify_who_completion_api: Callable, save_state_api: Callable, initial: str = 'noWarehouseorder') -> None: """Constructor.""" cls = self.__class__ # Initialize state machine super().__init__(self, states=cls.states, transitions=cls.transitions, send_event=True, queued=True, prepare_event=self._set_in_transition, before_state_change=self._run_before_state_change, after_state_change=self._run_after_state_change, finalize_event=self._unset_in_transition, initial=initial) # Configuration of the robot self.robot_config = robot_config # List of warehouse order assigned to this robot self.warehouseorders = OrderedDict() # List of sub warehouse orders of robot's warehouse orders for Pick, Pack and Pass # Scenario. Those are not assigned to the robot self.sub_warehouseorders = OrderedDict() # Warehouse order / warehouse task currently in process self.active_who = None self.active_wht = None self.active_sub_who = None # State before robot error occurred self.state_before_error = None self.error_count = defaultdict(int) # Marks if the state machine is currently in a transition self.in_transition = False # APIs to control the robot self.mission_api = robot_mission_api self._mission = RobotMission() self.confirm_api = confirm_api self.save_state_api = save_state_api # APIs for EWM Ordermanager self.request_ewm_work = request_ewm_work_api self.send_wht_error = send_wht_error_api self.notify_who_completion = notify_who_completion_api # Timestamp when state machine entered the current state self.state_enter_ts = time.time() # Warehouse order type currently in process self.who_type = '' # Warehouse order timestamps self.who_ts = {'start': 0.0, 'load': 0.0, 'unload': 0.0, 'finish': 0.0}
def update_mission_status_cb(self, name: str, custom_res: Dict) -> None: """Update self.mission_status.""" # Only process custom resources labeled with own robot name if custom_res['metadata'].get( 'labels', {}).get('cloudrobotics.com/robot-name' ) != self.robot_config.robot_name: return # OrderedDict must not be changed when iterating (self.mission_status) with self.mission_status_lock: # if status is set, update the dictionary if custom_res.get('status'): new_mstatus = self.mission_status.get(name, RobotMission(name)) new_mstatus.status = custom_res['status'].get('status', '') new_mstatus.active_action = custom_res['status'].get( 'activeAction', {}).get('status', '') self.mission_status[name] = new_mstatus # Delete finished missions with status SUCCEEDED, CANCELED, FAILED # Keep maximum of 20 missions finished = 0 delete_missions = [] cleanup = 0 cleanup_mission_dict = [] # Start counting from the back of mission OrderedDict for mission, status in reversed(self.mission_status.items()): if status.status in [ RobotMission.STATE_SUCCEEDED, RobotMission.STATE_CANCELED, RobotMission.STATE_FAILED ]: finished += 1 if finished >= 20: # Save mission to be deleted delete_missions.append(mission) # Collect missions to clean up if status.status == RobotMission.STATE_DELETED: cleanup += 1 if cleanup >= 50: cleanup_mission_dict.append(mission) # Delete mission CR and mark it as DELETED for mission in delete_missions: deleted = False if self.handler.check_cr_exists(mission): self.handler.delete_cr(mission) deleted = True _LOGGER.info('RobCo mission CR %s was cleaned up', mission) else: deleted = True if deleted: # If mission was deleted mark it as deleted self.mission_status[ mission].status = RobotMission.STATE_DELETED # Clean up self.mission_status dictionary for mission in cleanup_mission_dict: self.mission_status.pop(mission, None)
def mission(self, mission: RobotMission) -> None: """ Set self._mission. Ensure self._mission.target_name is not overwritten accidently. """ if self._mission.target_name: mission.target_name = self._mission.target_name self._mission = mission
def api_return_charge_state(self, mission: RobotMission) -> RobotMission: """Return state of a charge mission.""" mission = self.mission_status.get(mission.name, RobotMission(mission.name)) # If charging failed, try the next charger in list at the next try if mission.status == RobotMission.STATE_FAILED: self.charger = next(self._chargers_generator) return mission
def api_moveto_staging_position(self) -> RobotMission: """Move robot to a staging position of the map.""" # Default mission mission = RobotMission() # Get relevant parameters # TODO implement real move to staging method action = {'moveToNamedPosition': {'targetName': 'Staging'}} spec = {'actions': [action]} mission_name = str(time.time()) # Create CR success = self.create_cr(mission_name, self.labels, spec) # On success, set ID and STATE if success: mission.name = mission_name mission.status = RobotMission.STATE_ACCEPTED return mission
def api_moveto_storagebin_position( self, storagebin: StorageBin) -> RobotMission: """Move robot to a storage bin position of the map.""" # Default mission mission = RobotMission() # Get relevant parameters action = {'moveToNamedPosition': {'targetName': storagebin.lgpla}} spec = {'actions': [action]} mission_name = str(time.time()) # Create CR success = self.create_cr(mission_name, self.labels, spec) # On success, set ID and STATE if success: mission.name = mission_name mission.status = RobotMission.STATE_ACCEPTED return mission
def api_charge_robot(self) -> RobotMission: """Charge robot at the charging position.""" # Default mission mission = RobotMission() # Get relevant parameters action = {'charge': {'chargerName': self.charger, 'thresholdBatteryPercent': self.battery_min, 'targetBatteryPercent': self.battery_ok}} spec = {'actions': [action]} mission_name = str(time.time()) # Create CR success = self.create_cr(mission_name, self.labels, spec) # On success, set ID and STATE if success: mission.name = mission_name mission.status = RobotMission.STATE_ACCEPTED return mission
def api_return_mission_state(self, mission_name: str) -> str: """Return state of a mission.""" mission = self.mission_status.get(mission_name) # Create mission in unknown state if it is not known if mission is None: with self.mission_status_lock: self.mission_status[mission_name] = RobotMission(mission_name) mission = self.mission_status[mission_name] return mission.status
def api_return_charge_state(self, mission: RobotMission) -> RobotMission: """Return state of a charge mission.""" mission = self.mission_status.get(mission.name, RobotMission(mission.name)) # If charging failed, try the next charger in list at the next try if mission.status == RobotMission.STATE_FAILED: # Check if chargers changed in the meantime if self._chargers != self.robot_config.chargers: self._chargers = self.robot_config.chargers.copy() self._chargers_generator = self._iterate_chargers() _LOGGER.info('Available chargers changed to: %s', self._chargers) # Get next charger from generator self.charger = next(self._chargers_generator, '') return mission
def on_enter_moving(self, event: EventData) -> None: """Start moving to target staging or charging.""" cls = self.__class__ # Get target target target = event.kwargs.get('target') # Start move mission if target == cls.TARGET_CHARGING: mission = self.mission_api.api_moveto_charging_position() elif target == cls.TARGET_STAGING: mission = self.mission_api.api_moveto_staging_position() else: _LOGGER.error('Unknown target "%s". Valid targets are %s', target, cls.VALID_TARGETS) mission = RobotMission() if mission.name: _LOGGER.info('Started moving to "%s" with mission name "%s"', target, mission.name) else: _LOGGER.error('Mission name of move to "%s" is empty', target) self._mission.name = mission.name self._mission.status = mission.status self._mission.target_name = target
def api_return_mission_state(self, mission_name: str) -> str: """Return state of a mission.""" mission = self.mission_status.get(mission_name, RobotMission(mission_name)) return mission.status
def api_return_mission_activeaction(self, mission_name: str) -> str: """Return active_action of a mission.""" mission = self.mission_status.get(mission_name, RobotMission(mission_name)) return mission.active_action
def api_return_load_state(self, mission: RobotMission) -> RobotMission: """Return state of a load mission.""" mission = self.mission_status.get(mission.name, RobotMission(mission.name)) return mission
def __init__( self, mission_api: RobotMissionAPI, confirm_pick: Optional[Callable] = None, confirm_target: Optional[Callable] = None, state_restore: Optional[WarehouseOrderStateRestore] = None ) -> None: """Constructor.""" self.init_robot_fromenv() # Robot Mission API self.mission_api = mission_api # Robot state machine if state_restore: # Restore a previous state machine state who_type = state_restore.statemachine[:state_restore.statemachine. rfind('_')] _LOGGER.info( 'Restore state machine: WarehouseOrderType %s, WarehouseOrder %s, WarehouseTask %s' ', State %s, Mission %s', who_type, state_restore.warehouseorder.who, state_restore.tanum, state_restore.statemachine, state_restore.mission) # Init state machine in specific state self.state_machine = RobotEWMMachine( self.mission_api, self.confirm_warehousetask, self.request_work, self.send_warehousetask_error, self.notify_who_completion, self.save_wht_progress, initial=state_restore.statemachine) # Restore state machine attributes self.state_machine.who_type = who_type who_ident = WhoIdentifier(state_restore.warehouseorder.lgnum, state_restore.warehouseorder.who) self.state_machine.warehouseorders[ who_ident] = state_restore.warehouseorder self.state_machine.active_who = state_restore.warehouseorder if state_restore.subwarehouseorder: sub_who_ident = WhoIdentifier( state_restore.subwarehouseorder.lgnum, state_restore.subwarehouseorder.who) self.state_machine.sub_warehouseorders[ sub_who_ident] = state_restore.subwarehouseorder self.state_machine.active_sub_who = state_restore.subwarehouseorder for wht in state_restore.subwarehouseorder.warehousetasks: if wht.tanum == state_restore.tanum: self.state_machine.active_wht = wht break else: for wht in state_restore.warehouseorder.warehousetasks: if wht.tanum == state_restore.tanum: self.state_machine.active_wht = wht break self.state_machine.mission = RobotMission( name=state_restore.mission) else: # Initialize a fresh state machine _LOGGER.info('Initialize fresh state machine') self.state_machine = RobotEWMMachine(self.mission_api, self.confirm_warehousetask, self.request_work, self.send_warehousetask_error, self.notify_who_completion, self.save_wht_progress) # Callable to confirm a warehouse task self.send_wht_confirmation = self.send_wht_confirmation_default # Callable to update the progress the robot made when processing a warehouse task self.send_wht_progress_update = self.send_wht_progress_update_default # Callable to request work from order manager self.send_robot_request = self.send_robot_request_default # Confirmation callables for Pick, Pack and Pass if confirm_pick: self.confirm_pick = confirm_pick else: self.confirm_pick = self.confirm_false if confirm_target: self.confirm_target = confirm_target else: self.confirm_target = self.confirm_false # Timestamp for last time a request work was sent self._requested_work_time = None # Timestamp when robot was working for the last time self.last_time_working = time.time() self.idle_time_start = 0 # Number of failed status updates self._failed_status_updates = 0 # Robot is at staging position self.at_staging_position = False
def __init__(self, mission_api: RobotMissionAPI, robot_config: RobotConfigurationController, order_controller: OrderController, robot_request: RobotRequestController, confirm_pick: Optional[Callable] = None, confirm_target: Optional[Callable] = None) -> None: """Constructor.""" # Robot Mission API self.mission_api = mission_api # Robot Configuration self.robot_config = robot_config # Order Controller self.order_controller = order_controller # Robot Request controller self.robot_request_controller = robot_request # Robot state machine state_restore = self.get_validated_state_restore() if state_restore: # Restore a previous state machine state # Init state machine in specific state self.state_machine = RobotEWMMachine( self.robot_config, self.mission_api, self.confirm_warehousetask, self.request_work, self.send_warehousetask_error, self.notify_who_completion, self.robot_config.save_robot_state, initial=state_restore.statemachine) # Restore state machine attributes self.state_machine.mission = RobotMission( name=state_restore.mission, target_name=state_restore.missiontarget) if state_restore.who: who_type = state_restore.statemachine[:state_restore. statemachine.rfind('_')] _LOGGER.info( 'Restore state machine: WarehouseOrderType %s, WarehouseOrder %s, ' 'WarehouseTask %s, State %s, Mission %s', who_type, state_restore.who, state_restore.tanum, state_restore.statemachine, state_restore.mission) warehouseorder = self.order_controller.get_warehouseorder( state_restore.lgnum, state_restore.who) self.state_machine.who_type = who_type who_ident = WhoIdentifier(state_restore.lgnum, state_restore.who) self.state_machine.warehouseorders[who_ident] = warehouseorder self.state_machine.active_who = warehouseorder if state_restore.subwho: sub_warehouseorder = self.order_controller.get_warehouseorder( state_restore.lgnum, state_restore.subwho) sub_who_ident = WhoIdentifier(state_restore.lgnum, state_restore.subwho) self.state_machine.sub_warehouseorders[ sub_who_ident] = sub_warehouseorder self.state_machine.active_sub_who = sub_warehouseorder for wht in sub_warehouseorder.warehousetasks: if wht.tanum == state_restore.tanum: self.state_machine.active_wht = wht break else: for wht in warehouseorder.warehousetasks: if wht.tanum == state_restore.tanum: self.state_machine.active_wht = wht break else: _LOGGER.info('Restore state machine: State %s, Mission %s', state_restore.statemachine, state_restore.mission) else: # Initialize a fresh state machine _LOGGER.info('Initialize fresh state machine') self.state_machine = RobotEWMMachine( self.robot_config, self.mission_api, self.confirm_warehousetask, self.request_work, self.send_warehousetask_error, self.notify_who_completion, self.robot_config.save_robot_state) # Confirmation callables for Pick, Pack and Pass if confirm_pick: self.confirm_pick = confirm_pick else: self.confirm_pick = self.confirm_false if confirm_target: self.confirm_target = confirm_target else: self.confirm_target = self.confirm_false # Timestamp for last time a request work was sent self._requested_work_time = None # Timestamp when robot was working for the last time self.last_time_working = time.time() self.idle_time_start = 0 # Number of failed status updates self._failed_status_updates = 0 # Robot is at staging position self.at_staging_position = False