# Copyright (c) 2016-2017 Anki, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License in the file LICENSE.txt or at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. ''' Actions encapsulate specific high-level tasks that the Cozmo robot can perform. They have a definite beginning and end. These tasks include picking up an object, rotating in place, saying text, etc. Actions are usually triggered by a call to a method on the :class:`cozmo.robot.Robot` class such as :meth:`~cozmo.robot.Robot.turn_in_place` The call will return an object that subclasses :class:`Action` that can be used to cancel the action, or be observed to wait or be notified when the action completes (or fails) by calling its :meth:`~cozmo.event.Dispatcher.wait_for` or :meth:`~cozmo.event.Dispatcher.add_event_handler` methods. Warning: Unless you pass ``in_parallel=True`` when starting the action, no other action can be active at the same time. Attempting to trigger a non-parallel action when another action is already in progress will result in a :class:`~cozmo.exceptions.RobotBusy` exception being raised. When using ``in_parallel=True`` you may see an action fail with the result :attr:`ActionResults.TRACKS_LOCKED` - this indicates that another in-progress action has already locked that movement track (e.g. two actions cannot move the head at the same time). ''' # __all__ should order by constants, event classes, other classes, functions. __all__ = ['ACTION_IDLE', 'ACTION_RUNNING', 'ACTION_SUCCEEDED', 'ACTION_FAILED', 'ACTION_ABORTING', 'EvtActionStarted', 'EvtActionCompleted', 'Action', 'ActionResults'] from collections import namedtuple import sys from . import logger from . import event from . import exceptions from ._clad import _clad_to_engine_iface, _clad_to_engine_cozmo, _clad_to_game_cozmo, CladEnumWrapper #: string: Action idle state ACTION_IDLE = 'action_idle' #: string: Action running state ACTION_RUNNING = 'action_running' #: string: Action succeeded state ACTION_SUCCEEDED = 'action_succeeded' #: string: Action failed state ACTION_FAILED = 'action_failed' #: string: Action failed state ACTION_ABORTING = 'action_aborting' _VALID_STATES = {ACTION_IDLE, ACTION_RUNNING, ACTION_SUCCEEDED, ACTION_FAILED, ACTION_ABORTING} class _ActionResult(namedtuple('_ActionResult', 'name id')): # Tuple mapping between CLAD ActionResult name and ID # All instances will be members of ActionResults # Keep _ActionResult as lightweight as a normal namedtuple __slots__ = () def __str__(self): return 'ActionResults.%s' % self.name class ActionResults(CladEnumWrapper): """The possible result values for an Action. An Action's result is set when the action completes. """ _clad_enum = _clad_to_game_cozmo.ActionResult _entry_type = _ActionResult #: Action completed successfully. SUCCESS = _ActionResult("SUCCESS", _clad_enum.SUCCESS) #: Action is still running. RUNNING = _ActionResult("RUNNING", _clad_enum.RUNNING) #: Action was cancelled (e.g. via :meth:`~cozmo.robot.Robot.abort_all_actions` or #: :meth:`Action.abort`). CANCELLED_WHILE_RUNNING = _ActionResult("CANCELLED_WHILE_RUNNING", _clad_enum.CANCELLED_WHILE_RUNNING) #: Action aborted itself (e.g. had invalid attributes, or a runtime failure). ABORT = _ActionResult("ABORT", _clad_enum.ABORT) #: Animation Action aborted itself (e.g. there was an error playing the animation). ANIM_ABORTED = _ActionResult("ANIM_ABORTED", _clad_enum.ANIM_ABORTED) #: There was an error related to vision markers. BAD_MARKER = _ActionResult("BAD_MARKER", _clad_enum.BAD_MARKER) # (Undocumented) There was a problem related to a subscribed or unsupported message tag (indicates bug in engine) BAD_MESSAGE_TAG = _ActionResult("BAD_MESSAGE_TAG", _clad_enum.BAD_MESSAGE_TAG) #: There was a problem with the Object ID provided (e.g. there is no Object with that ID). BAD_OBJECT = _ActionResult("BAD_OBJECT", _clad_enum.BAD_OBJECT) #: There was a problem with the Pose provided. BAD_POSE = _ActionResult("BAD_POSE", _clad_enum.BAD_POSE) # (Undocumented) The SDK-provided tag was bad (shouldn't occur - would indicate a bug in the SDK) BAD_TAG = _ActionResult("BAD_TAG", _clad_enum.BAD_TAG) # (Undocumented) Shouldn't occur outside of factory FAILED_SETTING_CALIBRATION = _ActionResult("FAILED_SETTING_CALIBRATION", _clad_enum.FAILED_SETTING_CALIBRATION) #: There was an error following the planned path. FOLLOWING_PATH_BUT_NOT_TRAVERSING = _ActionResult("FOLLOWING_PATH_BUT_NOT_TRAVERSING", _clad_enum.FOLLOWING_PATH_BUT_NOT_TRAVERSING) #: The action was interrupted by another Action or Behavior. INTERRUPTED = _ActionResult("INTERRUPTED", _clad_enum.INTERRUPTED) #: The robot ended up in an "off treads state" not valid for this action (e.g. #: the robot was placed on its back while executing a turn) INVALID_OFF_TREADS_STATE = _ActionResult("INVALID_OFF_TREADS_STATE", _clad_to_game_cozmo.ActionResult.INVALID_OFF_TREADS_STATE) #: The Up Axis of a carried object doesn't match the desired placement pose. MISMATCHED_UP_AXIS = _ActionResult("MISMATCHED_UP_AXIS", _clad_enum.MISMATCHED_UP_AXIS) #: No valid Animation name was found. NO_ANIM_NAME = _ActionResult("NO_ANIM_NAME", _clad_enum.NO_ANIM_NAME) #: An invalid distance value was given. NO_DISTANCE_SET = _ActionResult("NO_DISTANCE_SET", _clad_enum.NO_DISTANCE_SET) #: There was a problem with the Face ID (e.g. Cozmo doesn't no where it is). NO_FACE = _ActionResult("NO_FACE", _clad_enum.NO_FACE) #: No goal pose was set. NO_GOAL_SET = _ActionResult("NO_GOAL_SET", _clad_enum.NO_GOAL_SET) #: No pre-action poses were found (e.g. could not get into position). NO_PREACTION_POSES = _ActionResult("NO_PREACTION_POSES", _clad_enum.NO_PREACTION_POSES) #: No object is being carried, but the action requires one. NOT_CARRYING_OBJECT_ABORT = _ActionResult("NOT_CARRYING_OBJECT_ABORT", _clad_enum.NOT_CARRYING_OBJECT_ABORT) #: Initial state of an Action to indicate it has not yet started. NOT_STARTED = _ActionResult("NOT_STARTED", _clad_enum.NOT_STARTED) #: No sub-action was provided. NULL_SUBACTION = _ActionResult("NULL_SUBACTION", _clad_enum.NULL_SUBACTION) #: Cozmo was unable to plan a path. PATH_PLANNING_FAILED_ABORT = _ActionResult("PATH_PLANNING_FAILED_ABORT", _clad_enum.PATH_PLANNING_FAILED_ABORT) #: The object that Cozmo is attempting to pickup is unexpectedly moving (e.g #: it is being moved by someone else). PICKUP_OBJECT_UNEXPECTEDLY_MOVING = _ActionResult("PICKUP_OBJECT_UNEXPECTEDLY_MOVING", _clad_enum.PICKUP_OBJECT_UNEXPECTEDLY_MOVING) #: The object that Cozmo thought he was lifting didn't start moving, so he #: must have missed. PICKUP_OBJECT_UNEXPECTEDLY_NOT_MOVING = _ActionResult("PICKUP_OBJECT_UNEXPECTEDLY_NOT_MOVING", _clad_enum.PICKUP_OBJECT_UNEXPECTEDLY_NOT_MOVING) # (Undocumented) Shouldn't occur in SDK usage SEND_MESSAGE_TO_ROBOT_FAILED = _ActionResult("SEND_MESSAGE_TO_ROBOT_FAILED", _clad_enum.SEND_MESSAGE_TO_ROBOT_FAILED) #: Cozmo is unexpectedly still carrying an object. STILL_CARRYING_OBJECT = _ActionResult("STILL_CARRYING_OBJECT", _clad_enum.STILL_CARRYING_OBJECT) #: The Action timed out before completing correctly. TIMEOUT = _ActionResult("TIMEOUT", _clad_enum.TIMEOUT) #: One or more animation tracks (Head, Lift, Body, Face, Backpack Lights, Audio) #: are already being used by another Action. TRACKS_LOCKED = _ActionResult("TRACKS_LOCKED", _clad_enum.TRACKS_LOCKED) #: There was an internal error related to an unexpected type of dock action. UNEXPECTED_DOCK_ACTION = _ActionResult("UNEXPECTED_DOCK_ACTION", _clad_enum.UNEXPECTED_DOCK_ACTION) # (Undocumented) Shouldn't occur outside of factory. UNKNOWN_TOOL_CODE = _ActionResult("UNKNOWN_TOOL_CODE", _clad_enum.UNKNOWN_TOOL_CODE) # (Undocumented) There was a problem in the subclass's update. UPDATE_DERIVED_FAILED = _ActionResult("UPDATE_DERIVED_FAILED", _clad_enum.UPDATE_DERIVED_FAILED) #: Cozmo did not see the expected result (e.g. unable to see cubes in their #: expected position after a related action). VISUAL_OBSERVATION_FAILED = _ActionResult("VISUAL_OBSERVATION_FAILED", _clad_enum.VISUAL_OBSERVATION_FAILED) #: The Action failed, but may succeed if retried. RETRY = _ActionResult("RETRY", _clad_enum.RETRY) #: Failed to get into position. DID_NOT_REACH_PREACTION_POSE = _ActionResult("DID_NOT_REACH_PREACTION_POSE", _clad_enum.DID_NOT_REACH_PREACTION_POSE) #: Failed to follow the planned path. FAILED_TRAVERSING_PATH = _ActionResult("FAILED_TRAVERSING_PATH", _clad_enum.FAILED_TRAVERSING_PATH) #: The previous attempt to pick and place an object failed. LAST_PICK_AND_PLACE_FAILED = _ActionResult("LAST_PICK_AND_PLACE_FAILED", _clad_enum.LAST_PICK_AND_PLACE_FAILED) #: The required motor isn't moving so the action cannot complete. MOTOR_STOPPED_MAKING_PROGRESS = _ActionResult("MOTOR_STOPPED_MAKING_PROGRESS", _clad_enum.MOTOR_STOPPED_MAKING_PROGRESS) #: Not carrying an object when it was expected, but may succeed if the action is retried. NOT_CARRYING_OBJECT_RETRY = _ActionResult("NOT_CARRYING_OBJECT_RETRY", _clad_enum.NOT_CARRYING_OBJECT_RETRY) #: Cozmo is expected to be on the charger, but is not. NOT_ON_CHARGER = _ActionResult("NOT_ON_CHARGER", _clad_enum.NOT_ON_CHARGER) #: Cozmo was unable to plan a path, but may succeed if the action is retried. PATH_PLANNING_FAILED_RETRY = _ActionResult("PATH_PLANNING_FAILED_RETRY", _clad_enum.PATH_PLANNING_FAILED_RETRY) #: There is no room to place the object at the desired destination. PLACEMENT_GOAL_NOT_FREE = _ActionResult("PLACEMENT_GOAL_NOT_FREE", _clad_enum.PLACEMENT_GOAL_NOT_FREE) #: Cozmo failed to drive off the charger. STILL_ON_CHARGER = _ActionResult("STILL_ON_CHARGER", _clad_enum.STILL_ON_CHARGER) #: Cozmo's pitch is at an unexpected angle for the Action. UNEXPECTED_PITCH_ANGLE = _ActionResult("UNEXPECTED_PITCH_ANGLE", _clad_enum.UNEXPECTED_PITCH_ANGLE) ActionResults._init_class() class EvtActionStarted(event.Event): '''Triggered when a robot starts an action.''' action = "The action that started" class EvtActionCompleted(event.Event): '''Triggered when a robot action has completed or failed.''' action = "The action that completed" state = 'The state of the action; either cozmo.action.ACTION_SUCCEEDED or cozmo.action.ACTION_FAILED' failure_code = 'A failure code such as "cancelled"' failure_reason = 'A human-readable failure reason' class Action(event.Dispatcher): """An action holds the state of an in-progress robot action """ # We allow sub-classes of Action to optionally disable logging messages # related to those actions being aborted - this is useful for actions # that are aborted frequently (by design) and would otherwise spam the log _enable_abort_logging = True def __init__(self, *, conn, robot, **kw): super().__init__(**kw) #: :class:`~cozmo.conn.CozmoConnection`: The connection on which the action was sent. self.conn = conn #: :class:`~cozmo.robot.Robot`: Th robot instance executing the action. self.robot = robot self._action_id = None self._state = ACTION_IDLE self._failure_code = None self._failure_reason = None self._result = None self._completed_event = None self._completed_event_pending = False def __repr__(self): extra = self._repr_values() if len(extra) > 0: extra = ' '+extra if self._state == ACTION_FAILED: extra += (" failure_reason='%s' failure_code=%s result=%s" % (self._failure_reason, self._failure_code, self.result)) return '<%s state=%s%s>' % (self.__class__.__name__, self.state, extra) def _repr_values(self): return '' def _encode(self): raise NotImplementedError() def _start(self): self._state = ACTION_RUNNING self.dispatch_event(EvtActionStarted, action=self) def _set_completed(self, msg): self._state = ACTION_SUCCEEDED self._completed_event_pending = False self._dispatch_completed_event(msg) def _dispatch_completed_event(self, msg): # Override to extra action-specific data from msg and generate # an action-specific completion event. Do not call super if overriden. # Must generate a subclass of EvtActionCompleted. self._completed_event = EvtActionCompleted(action=self, state=self._state) self.dispatch_event(self._completed_event) def _set_failed(self, code, reason): self._state = ACTION_FAILED self._failure_code = code self._failure_reason = reason self._completed_event_pending = False self._completed_event = EvtActionCompleted(action=self, state=self._state, failure_code=code, failure_reason=reason) self.dispatch_event(self._completed_event) def _set_aborting(self, log_abort_messages): if not self.is_running: raise ValueError("Action isn't currently running") if self._enable_abort_logging and log_abort_messages: logger.info('Aborting action=%s', self) self._state = ACTION_ABORTING #### Properties #### @property def is_running(self): '''bool: True if the action is currently in progress.''' return self._state == ACTION_RUNNING @property def is_completed(self): '''bool: True if the action has completed (either succeeded or failed).''' return self._state in (ACTION_SUCCEEDED, ACTION_FAILED) @property def is_aborting(self): '''bool: True if the action is aborting (will soon be either succeeded or failed).''' return self._state == ACTION_ABORTING @property def has_succeeded(self): '''bool: True if the action has succeeded.''' return self._state == ACTION_SUCCEEDED @property def has_failed(self): '''bool: True if the action has failed.''' return self._state == ACTION_FAILED @property def failure_reason(self): '''tuple of (failure_code, failure_reason): Both values will be None if no failure has occurred.''' return (self._failure_code, self._failure_reason) @property def result(self): """An attribute of :class:`ActionResults`: The result of running the action.""" return self._result @property def state(self): '''string: The current internal state of the action as a string. Will match one of the constants: :const:`ACTION_IDLE` :const:`ACTION_RUNNING` :const:`ACTION_SUCCEEDED` :const:`ACTION_FAILED` :const:`ACTION_ABORTING` ''' return self._state #### Private Event Handlers #### def _recv_msg_robot_completed_action(self, evt, *, msg): result = msg.result types = _clad_to_game_cozmo.ActionResult self._result = ActionResults.find_by_id(result) if self._result is None: logger.error("ActionResults has no entry for result id %s", result) if result == types.SUCCESS: # dispatch to the specific type to extract result info self._set_completed(msg) elif result == types.RUNNING: # XXX what does one do with this? it seems to occur after a cancel request! logger.warning('Received "running" action notification for action=%s', self) self._set_failed('running', 'Action was still running') elif result == types.NOT_STARTED: # not sure we'll see this? self._set_failed('not_started', 'Action was not started') elif result == types.TIMEOUT: self._set_failed('timeout', 'Action timed out') elif result == types.TRACKS_LOCKED: self._set_failed('tracks_locked', 'Action failed due to tracks locked') elif result == types.BAD_TAG: # guessing this is bad self._set_failed('bad_tag', 'Action failed due to bad tag') logger.error("Received FAILURE_BAD_TAG for action %s", self) elif result == types.CANCELLED_WHILE_RUNNING: self._set_failed('cancelled', 'Action was cancelled while running') elif result == types.INTERRUPTED: self._set_failed('interrupted', 'Action was interrupted') else: # All other results should fall under either the abort or retry # categories, determine the category by shifting the result result_category = result >> _clad_to_game_cozmo.ARCBitShift.NUM_BITS result_categories = _clad_to_game_cozmo.ActionResultCategory if result_category == result_categories.ABORT: self._set_failed('aborted', 'Action failed') elif result_category == result_categories.RETRY: self._set_failed('retry', 'Action failed but can be retried') else: # Shouldn't be able to get here self._set_failed('unknown', 'Action failed with unknown reason') logger.error('Received unknown action result status %s', msg) #### Public Event Handlers #### #### Commands #### def abort(self, log_abort_messages=False): '''Trigger the robot to abort the running action. Args: log_abort_messages (bool): True to log info on the action that is aborted. Raises: ValueError if the action is not currently being executed. ''' self.robot._action_dispatcher._abort_action(self, log_abort_messages) async def wait_for_completed(self, timeout=None): '''Waits for the action to complete. Args: timeout (int or None): Maximum time in seconds to wait for the event. Pass None to wait indefinitely. Returns: The :class:`EvtActionCompleted` event instance Raises: :class:`asyncio.TimeoutError` ''' if self.is_completed: # Already complete return self._completed_event return await self.wait_for(EvtActionCompleted, timeout=timeout) def on_completed(self, handler): '''Triggers a handler when the action completes. Args: handler (callable): An event handler which accepts arguments suited to the :class:`EvtActionCompleted` event. See :meth:`cozmo.event.add_event_handler` for more information. ''' return self.add_event_handler(EvtActionCompleted, handler) class _ActionDispatcher(event.Dispatcher): _next_action_id = _clad_to_game_cozmo.ActionConstants.FIRST_SDK_TAG def __init__(self, robot, **kw): super().__init__(**kw) self.robot = robot self._in_progress = {} self._aborting = {} def _get_next_action_id(self): # Post increment _current_action_id (and loop within the SDK_TAG range) next_action_id = self.__class__._next_action_id if self.__class__._next_action_id == _clad_to_game_cozmo.ActionConstants.LAST_SDK_TAG: self.__class__._next_action_id = _clad_to_game_cozmo.ActionConstants.FIRST_SDK_TAG else: self.__class__._next_action_id += 1 return next_action_id @property def aborting_actions(self): '''generator: yields each action that is currently aborting Returns: A generator yielding :class:`cozmo.action.Action` instances ''' for _, action in self._aborting.items(): yield action @property def has_in_progress_actions(self): '''bool: True if any SDK-triggered actions are still in progress.''' return len(self._in_progress) > 0 @property def in_progress_actions(self): '''generator: yields each action that is currently in progress Returns: A generator yielding :class:`cozmo.action.Action` instances ''' for _, action in self._in_progress.items(): yield action async def wait_for_all_actions_completed(self): '''Waits until all actions are complete. In this case, all actions include not just in_progress actions but also include actions that we're aborting but haven't received a completed message for yet. ''' while True: action = next(self.in_progress_actions, None) if action is None: action = next(self.aborting_actions, None) if action: await action.wait_for_completed() else: # all actions are now complete return def _send_single_action(self, action, in_parallel=False, num_retries=0): action_id = self._get_next_action_id() action.robot = self.robot action._action_id = action_id if self.has_in_progress_actions and not in_parallel: # Note - it doesn't matter if previous action was started as in_parallel, # starting any subsequent action with in_parallel==False will cancel # any previous actions, so we throw an exception here and require that # the client explicitly cancel or wait on earlier actions action = list(self._in_progress.values())[0] raise exceptions.RobotBusy('Robot is already performing %d action(s) %s' % (len(self._in_progress), action)) if action.is_running: raise ValueError('Action is already running') if action.is_completed: raise ValueError('Action already ran') if in_parallel: position = _clad_to_game_cozmo.QueueActionPosition.IN_PARALLEL else: position = _clad_to_game_cozmo.QueueActionPosition.NOW qmsg = _clad_to_engine_iface.QueueSingleAction( idTag=action_id, numRetries=num_retries, position=position, action=_clad_to_engine_iface.RobotActionUnion()) action_msg = action._encode() cls_name = action_msg.__class__.__name__ # For some reason, the RobotActionUnion type uses properties with a lowercase # first character, instead of uppercase like all the other unions cls_name = cls_name[0].lower() + cls_name[1:] setattr(qmsg.action, cls_name, action_msg) self.robot.conn.send_msg(qmsg) self._in_progress[action_id] = action action._start() def _is_sdk_action_id(self, action_id): return ((action_id >= _clad_to_game_cozmo.ActionConstants.FIRST_SDK_TAG) and (action_id <= _clad_to_game_cozmo.ActionConstants.LAST_SDK_TAG)) def _is_engine_action_id(self, action_id): return ((action_id >= _clad_to_game_cozmo.ActionConstants.FIRST_ENGINE_TAG) and (action_id <= _clad_to_game_cozmo.ActionConstants.LAST_ENGINE_TAG)) def _is_game_action_id(self, action_id): return ((action_id >= _clad_to_game_cozmo.ActionConstants.FIRST_GAME_TAG) and (action_id <= _clad_to_game_cozmo.ActionConstants.LAST_GAME_TAG)) def _action_id_type(self, action_id): if self._is_sdk_action_id(action_id): return "sdk" elif self._is_engine_action_id(action_id): return "engine" elif self._is_game_action_id(action_id): return "game" else: return "unknown" def _recv_msg_robot_completed_action(self, evt, *, msg): action_id = msg.idTag is_sdk_action = self._is_sdk_action_id(action_id) action = self._in_progress.get(action_id) was_aborted = False if action is None: action = self._aborting.get(action_id) was_aborted = action is not None if action is None: if is_sdk_action: logger.error('Received completed action message for unknown SDK action_id=%s', action_id) return else: if not is_sdk_action: action_id_type = self._action_id_type(action_id) logger.error('Received completed action message for sdk-known %s action_id=%s (was_aborted=%s)', action_id_type, action_id, was_aborted) action._completed_event_pending = True if was_aborted: if action._enable_abort_logging: logger.debug('Received completed action message for aborted action=%s', action) del self._aborting[action_id] else: logger.debug('Received completed action message for in-progress action=%s', action) del self._in_progress[action_id] # XXX This should generate a real event, not a msg # Should also dispatch to self so the parent can be notified. action.dispatch_event(evt) def _abort_action(self, action, log_abort_messages): # Mark this in-progress action as aborting - it should get a "Cancelled" # message back in the next engine tick, and can basically be considered # cancelled from now. action._set_aborting(log_abort_messages) if action._completed_event_pending: # The action was marked as still running but the ActionDispatcher # has already received a completion message (and removed it from # _in_progress) - the action is just waiting to receive a # robot_completed_action message that is still being dispatched # via asyncio.ensure_future logger.debug('Not sending abort for action=%s to engine as it just completed', action) else: # move from in-progress to aborting dicts self._aborting[action._action_id] = action del self._in_progress[action._action_id] msg = _clad_to_engine_iface.CancelActionByIdTag(idTag=action._action_id) self.robot.conn.send_msg(msg) def _abort_all_actions(self, log_abort_messages): # Mark any in-progress actions as aborting - they should get a "Cancelled" # message back in the next engine tick, and can basically be considered # cancelled from now. actions_to_abort = self._in_progress self._in_progress = {} for action_id, action in actions_to_abort.items(): action._set_aborting(log_abort_messages) self._aborting[action_id] = action logger.info('Sending abort request for all actions') # RobotActionType.UNKNOWN is a wildcard that matches all actions when cancelling. msg = _clad_to_engine_iface.CancelAction(actionType=_clad_to_engine_cozmo.RobotActionType.UNKNOWN) self.robot.conn.send_msg(msg)