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