diff --git a/docs/runtime.md b/docs/runtime.md new file mode 100644 index 0000000..6fce7ae --- /dev/null +++ b/docs/runtime.md @@ -0,0 +1,63 @@ +# Execution contract + +A loaded behavior initially contains only its root instance. `update()` first +checks reevaluating decisions from the bottom of the active stack upward, then +executes the current frame. Decision outcomes descend immediately through further +decisions to an action. An action runs at most once per update. + +Actions retain their instance state between updates. `return self.pop()` completes +an action. If the action has a chain successor, its cleanup runs before the +successor is initialized, and that successor runs on the next update. Without a +successor, the previous decision becomes current and decides on the next update. +The only remaining frame cannot pop itself. + +A reevaluated decision whose selected branch changes cancels every frame above +it, then initializes and executes the newly selected path in the same update. +Different outcomes handled by the same `ELSE` branch do not rebuild the path. +An invalid outcome is reported before the existing path is canceled. + +`interrupt()` cancels all active frames, including the old root, and initializes +a fresh instance of the declared start element. It runs on the next update. +A callback that pops, interrupts, or otherwise changes the stack cancels the +remainder of the current traversal; its returned decision outcome is discarded. +Return immediately after `pop()` or `interrupt()` to avoid continuing work on an +instance that has left the active stack. + +`on_pop()` runs once for each discarded instance, in order from the top down. +Frames are detached before cleanup. Cleanup exceptions are logged, and cleanup +continues for the remaining detached frames. If cleanup itself changes the stack, +the pending transition does not overwrite that change. Constructors initialize +instance state; they must not change the DSD stack. Each activation receives an +independent copy of its parameters. + +Behavior loading parses and binds the replacement before canceling the old +behavior. Parse and registration errors therefore leave the active behavior +intact. Once cleanup starts, replacement construction can still fail; callers +must handle load errors. Do not rely on rollback of already canceled hardware +operations. + +Ordinary update failures retain the existing log-and-continue policy. The first +error of an update is available as `dsd.last_error`; a successful subsequent +update clears it. An empty or non-string decision outcome is a normal library +error and does not terminate the host process. Update errors do not imply that +an action succeeded, was retried safely, or was rolled back. Error recovery and +explicit action outcomes remain under discussion in +[issue #115](https://github.com/bit-bots/dynamic_stack_decider/issues/115). + +`do_not_reevaluate()` suppresses the next reevaluation. Actions with `r:false` or +`reevaluate:false` renew suppression while running. Completing a normal frame +clears suppression; advancing a chain retains it. Interruption and behavior +replacement clear it. Root-chain completion and suppression scope remain under +discussion in [issue #121](https://github.com/bit-bots/dynamic_stack_decider/issues/121). + +Debug snapshots describe the state after the update. Debug failures are logged +and recorded in `last_error` without preventing behavior execution. The tree and +current-action topics use transient-local durability, so matching late +subscribers receive their latest values. An empty current-action name means that +no action is active. Debugging requires both a topic and a ROS node; otherwise it +is disabled. + +DSD is synchronous. Call `update()` from one owner, avoid blocking work in +`perform()`, and marshal external changes through that owner. Recursive updates +are rejected. The stack returned by `get_stack()` is for inspection and must not +be mutated by callers. diff --git a/dynamic_stack_decider/dynamic_stack_decider/abstract_action_element.py b/dynamic_stack_decider/dynamic_stack_decider/abstract_action_element.py index 2ff036c..5f61b37 100644 --- a/dynamic_stack_decider/dynamic_stack_decider/abstract_action_element.py +++ b/dynamic_stack_decider/dynamic_stack_decider/abstract_action_element.py @@ -1,6 +1,8 @@ +"""Base class for persistent actions.""" + from __future__ import annotations -from abc import ABCMeta, abstractmethod +from abc import abstractmethod from typing import TYPE_CHECKING from dynamic_stack_decider.abstract_stack_element import AbstractStackElement @@ -10,46 +12,21 @@ from dynamic_stack_decider.dsd import DSD -class AbstractActionElement(AbstractStackElement[BlackboardT], metaclass=ABCMeta): - """ - One action is similar to a state of an FSM. - As in this case, the system stays in this state in contrast to the decision elements which are called only for determining the active action. - It defines the actions which the robot does, for example performing a kick. - Another example is an action which takes care of going to the ball, the action remains on top of the stack until the ball is reached. - The action only makes decisions which are necessary for its purpose, like some adjustments to the kicking movement. - Actions do not push further elements on the stack but command actions on lower-level modules like new movement goals. - If the action is complete, it can remove itself from the stack by performing a pop command. - """ +class AbstractActionElement(AbstractStackElement[BlackboardT]): + """Perform work until completion or preemption; implement cleanup in ``on_pop``.""" def __init__(self, blackboard: BlackboardT, dsd: DSD[BlackboardT], parameters: Parameters) -> None: - """ - Constructor of the action element - :param blackboard: Shared blackboard for data exchange between elements - :param dsd: The stack decider which has this element on its stack. - :param parameters: Parameters which serve as arguments to this element - """ super().__init__(blackboard, dsd, parameters) - # Reevaluation can be disabled by setting 'r' or 'reevaluate' to False self.never_reevaluate = not parameters.get("r", True) or not parameters.get("reevaluate", True) def do_not_reevaluate(self) -> None: - """ - Prohibits the next reevaluate. - This is useful if you have an action which has to be completed without interruption, e.g. a kick animation. - """ + """Suppress the next reevaluation, for example while finishing an atomic motion.""" self._dsd.set_do_not_reevaluate() def repr_dict(self) -> ElementData: - """ - Represent this stack element as dictionary which is JSON encodable - """ - return { - "type": "action", - "name": self.name, - "debug_data": self._debug_data, - } + return {"type": "action", "name": self.name, "debug_data": self._debug_data} @abstractmethod def perform(self, reevaluate: bool = False) -> None: - """Execute the element; decisions return an outcome and actions return nothing.""" + """Perform one tick of work. Complete with ``return self.pop()``.""" raise NotImplementedError(f"Implement perform() in {self.name}") diff --git a/dynamic_stack_decider/dynamic_stack_decider/abstract_decision_element.py b/dynamic_stack_decider/dynamic_stack_decider/abstract_decision_element.py index 77ddc97..148b63d 100644 --- a/dynamic_stack_decider/dynamic_stack_decider/abstract_decision_element.py +++ b/dynamic_stack_decider/dynamic_stack_decider/abstract_decision_element.py @@ -1,55 +1,26 @@ +"""Base class for outcome-producing decisions.""" + from __future__ import annotations -from abc import ABCMeta, abstractmethod +from abc import abstractmethod from dynamic_stack_decider.abstract_stack_element import AbstractStackElement from dynamic_stack_decider.types import BlackboardT, ElementData -class AbstractDecisionElement(AbstractStackElement[BlackboardT], metaclass=ABCMeta): - """ - The logic is encapsulated in two types of elements. - The decision elements define the logical path similar to a behavior tree. - Corresponding decisions can be as complex as wished, using in the most simple case - if-else clauses or in a more complicated situation, for example, neural networks. - Either way, one decision-element should capsule one logical decision and determine - the following module, without giving any calls to the hardware level. - One example is the decision if the robot has sufficient knowledge about the - current ball position, using data from the vision components and the team-communication. - By using a push method they add further elements to the stack. - Thereby, each decision element can have other decision or actions as following active elements. - All decisions can, therefore, be displayed as a decision tree. - """ +class AbstractDecisionElement(AbstractStackElement[BlackboardT]): + """Select a branch from the blackboard; reevaluation should avoid hardware side effects.""" _reevaluate = False - def repr_dict(self) -> ElementData: - """ - Represent this stack element as dictionary which is JSON encodable - """ - return { - "type": "decision", - "name": self.name, - "debug_data": self._debug_data, - } - def get_reevaluate(self) -> bool: - """ - Each decision element may define a \textit{reevaluate} criteria. - If the corresponding method returns true, the element will be executed even if it is in the middle of the stack. - This way a precondition can be checked, for example, every tenth iteration. - As an example, every iteration it could be checked if the ball position is known to the robot with a given certainty. - If the now pushed element is different from the element which is currently above in the stack, the whole - stack above will be dropped and the newly selected element executed. - - This method returns whether the element should be reevaluated. This means that the element is recomputed - even if it is not on top of the stack. If the decision pushes a different element than in the original perform, - the stack above this decision is cleared. - This can be used to regularly check preconditions, e.g. if the robot knows where the ball is. - """ + """Request reevaluation while this decision is below the current frame.""" return self._reevaluate + def repr_dict(self) -> ElementData: + return {"type": "decision", "name": self.name, "debug_data": self._debug_data} + @abstractmethod def perform(self, reevaluate: bool = False) -> str: - """Execute the element; decisions return an outcome and actions return nothing.""" + """Return a non-empty outcome matching a branch or handled by ``ELSE``.""" raise NotImplementedError(f"Implement perform() in {self.name}") diff --git a/dynamic_stack_decider/dynamic_stack_decider/abstract_stack_element.py b/dynamic_stack_decider/dynamic_stack_decider/abstract_stack_element.py index 8832310..2a40338 100644 --- a/dynamic_stack_decider/dynamic_stack_decider/abstract_stack_element.py +++ b/dynamic_stack_decider/dynamic_stack_decider/abstract_stack_element.py @@ -1,6 +1,9 @@ +"""Shared lifecycle and debugging helpers for actions and decisions.""" + from __future__ import annotations -from abc import ABCMeta, abstractmethod +import json +from abc import ABC, abstractmethod from typing import TYPE_CHECKING, Generic from dynamic_stack_decider.logger import get_logger @@ -10,108 +13,44 @@ from dynamic_stack_decider.dsd import DSD -class AbstractStackElement(Generic[BlackboardT], metaclass=ABCMeta): - """ - The AbstractStackElement is the basis of all elements on the stack. - It provides some help functions which should not be overloaded. - The work of an element is done in the :func:`perform`. - Each element which inherits from the AbstractStackElement can be used as a root element on the stack. - """ - - _dsd: DSD[BlackboardT] - parameters: Parameters - +class AbstractStackElement(Generic[BlackboardT], ABC): def __init__(self, blackboard: BlackboardT, dsd: DSD[BlackboardT], parameters: Parameters) -> None: - """ - :param blackboard: Shared blackboard for data exchange between elements - :param dsd: The stack decider which has this element on its stack. - :param parameters: Parameters which serve as arguments to this element - """ - self._debug_data: dict[str, object] = {} - """ - This is a dict in which data can be saved that should get represented on a __repr__ call. - It can be set using publish_debug_data() - """ - self._dsd = dsd - self.parameters = parameters self.blackboard = blackboard + self.parameters = parameters + self._debug_data: dict[str, object] = {} @property def name(self) -> str: - """ - Returns the name of the action - """ - return self.__class__.__name__ + return type(self).__name__ def pop(self) -> None: - """ - Help method which pops the element of the stack. - - This method should always be called with a return: - return self.pop() - - If no return is used, further code is executed after the pop, which leads to difficult to debug behavior. - """ + """Complete this frame. Return immediately afterward to avoid using a discarded instance.""" self._dsd.pop() def on_pop(self) -> None: - """ - This method is called when the element is popped from the stack. - It can be used to clean up resources, cancel actions or similar tasks. - Overload this method if you need to do something when the element is popped. - """ - pass + """Release resources when completed, preempted, interrupted, or replaced by another behavior.""" @abstractmethod def perform(self, reevaluate: bool = False) -> str | None: - """ - This method is called when the element is on top of the stack. - This method has to be overloaded by the implementation! - - :param reevaluate: True if the current method call is a reevaluate of the state. Meaning the module is not on top of the stack. - """ - msg = f"You should override perform() in {self.__class__.__name__}" - raise NotImplementedError(msg) + """Execute the element, or check a decision during reevaluation.""" + raise NotImplementedError(f"Implement perform() in {self.name}") def interrupt(self) -> None: - """ - An interrupt leads to a complete clearing of the stack. - """ + """Restart the behavior. Return immediately afterward.""" self._dsd.interrupt() def publish_debug_data(self, label: str, data: object) -> None: - """ - Publish debug data. Can be viewed using the DSD visualization - - This method is safe to call without wrapping it in a try-catch block although invalid values will - be wrapped in a `str()` call - - :param label: A label that describes the given data - :param data: data that should be displayed for debugging purposes - """ - if type(data) not in (dict, list, int, float, str, bool): - get_logger().debug( - f"The supplied debug data of type {type(data)} is not JSON serializable and will be wrapped in str()", - throttle_duration_sec=1, - ) + """Store JSON-compatible debug data, representing unsupported values as strings.""" + try: + data = json.loads(json.dumps(data, default=str, allow_nan=False)) + except (TypeError, ValueError): data = str(data) - get_logger().debug(f"{label}: {data}") self._debug_data[label] = data def clear_debug_data(self) -> None: - """ - Clear existing debug data - - This is needed when old values are no longer supposed to be visible - """ - self._debug_data = {} + self._debug_data.clear() def repr_dict(self) -> ElementData: - """Represent this stack element as dictionary which is JSON encodable""" - return { - "type": "abstract", - "name": self.name, - "debug_data": self._debug_data, - } + return {"type": "abstract", "name": self.name, "debug_data": self._debug_data} diff --git a/dynamic_stack_decider/dynamic_stack_decider/dsd.py b/dynamic_stack_decider/dynamic_stack_decider/dsd.py index d96392b..3a1c40c 100644 --- a/dynamic_stack_decider/dynamic_stack_decider/dsd.py +++ b/dynamic_stack_decider/dynamic_stack_decider/dsd.py @@ -1,5 +1,8 @@ +"""Execute persistent actions selected by reevaluable decisions.""" + from __future__ import annotations +import copy import importlib import inspect import json @@ -10,6 +13,7 @@ from typing import Generic from rclpy.node import Node +from rclpy.publisher import Publisher from rclpy.qos import DurabilityPolicy, QoSProfile from std_msgs.msg import String @@ -18,394 +22,326 @@ from dynamic_stack_decider.abstract_stack_element import AbstractStackElement from dynamic_stack_decider.logger import get_logger from dynamic_stack_decider.parser import DsdParser -from dynamic_stack_decider.tree import ( - AbstractTreeElement, - ActionTreeElement, - DecisionTreeElement, - Tree, -) +from dynamic_stack_decider.tree import AbstractTreeElement, ActionTreeElement, DecisionTreeElement, Tree from dynamic_stack_decider.types import BlackboardT, StackData def discover_elements(path: str | Path) -> dict[str, type[AbstractStackElement[BlackboardT]]]: - """ - Extract all the classes from the files in the given path and return a dictionary containing them - - :param path: The absolute path containing the files that should be registered - :return: A dictionary with class names as keys and classes as values - :raises ValueError: if path is not an existing directory or file - """ - elements: dict[str, type[AbstractStackElement[BlackboardT]]] = {} + """Discover locally defined element classes, logging modules that cannot be imported.""" path = Path(path) + if not path.is_file() and not path.is_dir(): + raise ValueError(f"Path {path} is not a directory or file") + elements: dict[str, type[AbstractStackElement[BlackboardT]]] = {} original_pythonpath = sys.path.copy() - def discover_module_elements(module_name: str) -> None: + def discover_module(module_name: str) -> None: try: - if sys.modules.get(module_name, False): - # reload the module if it is already imported, - # because we might load a different dsd and the module is - # always e.g. names 'actions' and therefore not reloaded with the new dsd + if module_name in sys.modules: importlib.reload(sys.modules[module_name]) module = importlib.import_module(module_name) - # add all classes which are defined directly in the target module (not imported) elements.update( inspect.getmembers( module, - lambda m: ( - inspect.isclass(m) and inspect.getmodule(m) == module and issubclass(m, AbstractStackElement) + lambda member: ( + inspect.isclass(member) + and inspect.getmodule(member) == module + and issubclass(member, AbstractStackElement) + and not inspect.isabstract(member) ), ) ) - except Exception as e: - get_logger().error(f"Error while loading class {module_name}: {e}") - - if path.is_file(): - # update PYTHONPATH so that path is importable as a module - sys.path.append(str(path.parent)) - if path.name == "__init__.py": - module_name = path.parent.name + except Exception as error: + get_logger().error(f"Cannot import element module {module_name}: {error}") + + try: + if path.is_file(): + is_package = path.name == "__init__.py" + sys.path.append(str(path.parent.parent if is_package else path.parent)) + discover_module(path.parent.name if is_package else path.stem) else: - module_name = path.name.rsplit(".", 1)[0] - discover_module_elements(module_name) - - elif path.is_dir(): - sys.path.append(str(path.parent)) - # discover elements defined in the modules __init__.py file - discover_module_elements(path.name) - # discover elements of all submodules - for _, module_name, _ in pkgutil.walk_packages([str(path)], prefix=path.name + "."): - discover_module_elements(module_name) - - else: - raise ValueError(f"Path {path} is not a directory or file") - - # restore original PYTHONPATH so that everything stays consistent - sys.path = original_pythonpath - + sys.path.append(str(path.parent)) + discover_module(path.name) + for _, module_name, _ in pkgutil.walk_packages([str(path)], prefix=f"{path.name}."): + discover_module(module_name) + finally: + sys.path[:] = original_pythonpath return elements class DSD(Generic[BlackboardT]): - """ - One decision is defined as the root decision, the starting point. - Each decision element, which is pushed on the stack, is immediately executed until no further element is pushed. - Following, each iteration, for each element is checked if it requires to be reevaluated and finally the - top element of the stack will be executed, usually an action. - If the outcome of a reevaluated element changes, the entire stack on top of this element will be dropped and the - stack newly constructed. - As soon as the action is complete, the element will be popped off the stack and the module underneath will be - executed in the next iteration. - If this is a decision, it again pushes a further decision or an action and the new top element will be executed. - - By this structure, it is always visible which action the robot tries to perform and which decisions were made. - - If a new element is pushed on top of the stack, it is directly executed. - In most cases, the pushing element is completing its execution with the push of another element. - Any following code will be executed as soon as the stack is not further expanded. - """ + """Retain an active decision path and execute one action per update. - start_element: AbstractTreeElement[BlackboardT] | None = None - stack_exec_index = -1 - stack_reevaluate = False - do_not_reevaluate = False - old_representation = "" - debug_active_action_cache: str | None = None + Decisions descend immediately to their selected child. Actions remain active + until they pop or a reevaluated decision selects a different branch. Completing + an action in a chain initializes its successor for the next update. + """ def __init__(self, blackboard: BlackboardT, debug_topic: str | None = None, node: Node | None = None) -> None: - """ - :param blackboard: Blackboard instance which will be available to all modules - :param debug_topic: Topic on which debug data should be published - """ - self.blackboard = blackboard self.node = node - self.tree: Tree[BlackboardT] | None = None - # The stack is implemented as a list of tuples consisting of the tree element - # and the actual module instance + self.start_element: AbstractTreeElement[BlackboardT] | None = None self.stack: list[tuple[AbstractTreeElement[BlackboardT], AbstractStackElement[BlackboardT]]] = [] - self.actions: dict[str, type[AbstractActionElement[BlackboardT]]] = {} self.decisions: dict[str, type[AbstractDecisionElement[BlackboardT]]] = {} - - # Check if debugging is active - self.debug_active = debug_topic is not None - - # Create debug publisher if necessary and possible + self.stack_exec_index = -1 + self.stack_reevaluate = False + self.do_not_reevaluate = False + self._executing = False + self._stack_revision = 0 + self.last_error: Exception | None = None + self.debug_active = debug_topic is not None and node is not None + self.debug_active_action_cache: str | None = None + self.debug_tree_publisher: Publisher[String] | None = None + self.debug_stack_publisher: Publisher[String] | None = None + self.debug_current_action_publisher: Publisher[String] | None = None if self.debug_active and node is not None: - get_logger().debug("Debugging is active") - # Create tree publisher - debug_tree_topic = f"{debug_topic}/dsd_tree" - # We use a latched publisher because the tree is only published once most of the time latched_qos = QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.debug_tree_publisher = node.create_publisher(String, debug_tree_topic, latched_qos) - get_logger().debug(f"Debugging tree on '{debug_tree_topic}' (latched)") - # Create stack publisher - debug_stack_topic = f"{debug_topic}/dsd_stack" - self.debug_stack_publisher = node.create_publisher(String, debug_stack_topic, 10) - get_logger().debug(f"Debugging stack on '{debug_stack_topic}'") - # Publish the currently active action - debug_current_action_topic = f"{debug_topic}/dsd_current_action" - self.debug_current_action_publisher = node.create_publisher(String, debug_current_action_topic, 10) - get_logger().debug(f"Debugging current action on '{debug_current_action_topic}'") + self.debug_tree_publisher = node.create_publisher(String, f"{debug_topic}/dsd_tree", latched_qos) + self.debug_stack_publisher = node.create_publisher(String, f"{debug_topic}/dsd_stack", 10) + self.debug_current_action_publisher = node.create_publisher( + String, f"{debug_topic}/dsd_current_action", latched_qos + ) def register_actions(self, module_path: str | Path) -> None: - """ - Register every class in a given path as an action - :param module_path: A path containing files with classes extending AbstractActionElement - """ + """Replace the action registry with classes discovered at a file or package path.""" discovered: dict[str, type[AbstractStackElement[BlackboardT]]] = discover_elements(module_path) - self.actions = {k: v for k, v in discovered.items() if issubclass(v, AbstractActionElement)} + self.actions = {name: cls for name, cls in discovered.items() if issubclass(cls, AbstractActionElement)} def register_decisions(self, module_path: str | Path) -> None: - """ - Register every class in a given path as a decision - :param module_path: A path containing files with classes extending AbstractDecisionElement - """ + """Replace the decision registry with classes discovered at a file or package path.""" discovered: dict[str, type[AbstractStackElement[BlackboardT]]] = discover_elements(module_path) - self.decisions = {k: v for k, v in discovered.items() if issubclass(v, AbstractDecisionElement)} + self.decisions = {name: cls for name, cls in discovered.items() if issubclass(cls, AbstractDecisionElement)} def load_behavior(self, path: str | Path) -> None: - """ - Load a .dsd file into the behavior to execute it. This should be called after the actions - and decisions have been loaded. - :param path: The path to the .dsd file describing the behavior - :return: - """ + """Parse and bind a behavior before canceling the previously loaded behavior.""" parser: DsdParser[BlackboardT] = DsdParser(self.node) - self.tree = parser.parse(path) - if self.tree.root_element is None: + tree = parser.parse(path) + if tree.root_element is None: raise ValueError("Behavior has no root element") - self._bind_modules(self.tree.root_element) - self.set_start_element(self.tree.root_element) - self.debug_publish_tree() + self._bind_modules(tree.root_element) + self.set_start_element(tree.root_element) + self.tree = tree + self.debug_active_action_cache = None + try: + self.debug_publish_tree() + except Exception as error: + self._record_error(error) def _bind_modules(self, element: AbstractTreeElement[BlackboardT]) -> None: - """ - Recursively traverse the tree and bind the registered action and decision classes to - the corresponding tree elements - :param element: The starting element - """ - if isinstance(element, ActionTreeElement): - assert element.name in self.actions, ( - f'Provided element "{element.name}" was not found in registered actions!' - ) - element.module = self.actions[element.name] - # Bind the rest of the chain (e.g. the following actions of a sequence) - if element.next_element is not None: - self._bind_modules(element.next_element) - elif isinstance(element, DecisionTreeElement): - assert element.name in self.decisions, ( - f'Provided element "{element.name}" was not found in registered decisions!' - ) - element.module = self.decisions[element.name] - for child in element.children.values(): - self._bind_modules(child) - else: - raise ValueError(f'Unknown parser tree element type "{type(element)}" for element "{element}"!') + pending = [element] + while pending: + current = pending.pop() + if isinstance(current, ActionTreeElement): + if current.name not in self.actions: + raise ValueError( + f"Action {current.name!r} is not registered ({current.source_file}:{current.source_line})" + ) + current.module = self.actions[current.name] + if current.next_element is not None: + pending.append(current.next_element) + elif isinstance(current, DecisionTreeElement): + if current.name not in self.decisions: + raise ValueError( + f"Decision {current.name!r} is not registered ({current.source_file}:{current.source_line})" + ) + current.module = self.decisions[current.name] + pending.extend(current.children.values()) + else: + raise ValueError(f"Unknown tree element type {type(current).__name__}") def _init_element(self, element: AbstractTreeElement[BlackboardT]) -> AbstractStackElement[BlackboardT]: - """Initializes the module belonging to the given element.""" if element.module is None: raise ValueError(f"Element {element.name!r} has no bound implementation") - return element.module(self.blackboard, self, element.parameters) + return element.module(self.blackboard, self, copy.deepcopy(element.parameters)) + + def _append(self, element: AbstractTreeElement[BlackboardT]) -> None: + revision = self._stack_revision + instance = self._init_element(element) + if self._stack_revision != revision: + raise RuntimeError("Element constructor changed the stack") + self.stack.append((element, instance)) + self._stack_revision += 1 + + def _record_error(self, error: Exception) -> None: + if self.last_error is None: + self.last_error = error + get_logger().error("".join(traceback.format_exception(type(error), error, error.__traceback__))) + + def _discard_from(self, index: int) -> bool: + """Detach frames before cleanup; return whether cleanup left the remaining stack intact.""" + # Cleanup can call back into DSD; detach every removed frame before invoking it. + removed = self.stack[index:] + del self.stack[index:] + self._stack_revision += 1 + revision = self._stack_revision + for _, instance in reversed(removed): + try: + instance.on_pop() + except Exception as error: + self._record_error(error) + return self._stack_revision == revision def set_start_element(self, start_element: AbstractTreeElement[BlackboardT]) -> None: - """ - This method defines the start element on the stack, which stays always on the bottom of the stack. - It should be called in __init__. - """ + """Cancel the active path and initialize a new start element without executing it.""" + self.stack_reevaluate = False + self.do_not_reevaluate = False + if not self._discard_from(0): + raise RuntimeError("Cleanup changed the stack while replacing the behavior") self.start_element = start_element - if self.start_element is None: - raise RuntimeError("No behavior has been loaded") - self.stack = [(self.start_element, self._init_element(self.start_element))] + self._append(start_element) def interrupt(self) -> None: - """ - An interrupt is an event which clears the complete stack to reset the behavior. - In the special case of RoboCup, we use it when the game-state changes, but it can also be used for - example if the robot is kidnapped or paused. - In the following iteration, the stack will be newly created starting at the root element. - """ - if self.stack_reevaluate: - # we were currently checking preconditions - # we stop this, so that update() knows that it has to stop - self.stack_reevaluate = False + """Cancel every active frame and initialize the declared start element for the next update.""" if self.start_element is None: raise RuntimeError("No behavior has been loaded") - self.stack = [(self.start_element, self._init_element(self.start_element))] + self.stack_reevaluate = False + self.do_not_reevaluate = False + if self._discard_from(0): + self._append(self.start_element) + + def _reevaluate_stack(self) -> bool: + """Return true when a callback canceled the remainder of this update.""" + self.stack_reevaluate = True + revision = self._stack_revision + try: + for index, (tree_element, instance) in enumerate(self.stack[:-1]): + self.stack_exec_index = index + if not isinstance(instance, AbstractDecisionElement): + continue + reevaluate = instance.get_reevaluate() + if not self.stack_reevaluate or self._stack_revision != revision: + return True + if not reevaluate: + continue + if not isinstance(tree_element, DecisionTreeElement): + raise TypeError("Decision instance is not bound to a decision tree element") + result = instance.perform(True) + # pop() or interrupt() invalidates the saved frame and its returned outcome. + if not self.stack_reevaluate or self._stack_revision != revision: + return True + child = tree_element.get_child(result) + if child.activation_reason != self.stack[index + 1][0].activation_reason: + self.stack_reevaluate = False + if not self._discard_from(index + 1): + return True + self._append(child) + return False + return False + finally: + self.stack_reevaluate = False + self.stack_exec_index = -1 + + def _execute_top(self) -> None: + while self.stack: + tree_element, instance = self.stack[-1] + if isinstance(instance, AbstractActionElement) and instance.never_reevaluate: + self.set_do_not_reevaluate() + revision = self._stack_revision + result = instance.perform() + # A callback may have replaced the path; do not follow its stale outcome. + if self._stack_revision != revision: + return + if not isinstance(instance, AbstractDecisionElement): + return + if not isinstance(tree_element, DecisionTreeElement): + raise TypeError("Decision instance is not bound to a decision tree element") + self._append(tree_element.get_child(result)) def update(self, reevaluate: bool = True) -> None: - """ - Calls the element which is currently on top of the stack. - Before doing this, all preconditions are checked (all decision elements where reevaluate is true). + """Reevaluate the active path and run its action, logging failures in ``last_error``. - :param: reevaluate: Can be set to False to avoid the reevaluation + Do not call update recursively or concurrently. Callbacks that change the + stack cancel the rest of the current traversal. Debugging runs after the + tick and cannot prevent behavior execution. """ + if self._executing: + raise RuntimeError("DSD.update() is already running") + self.last_error = None + self._executing = True try: - self.debug_publish_stack() - self.debug_publish_current_action() - - if reevaluate and not self.do_not_reevaluate: - self.stack_exec_index = 0 - self.stack_reevaluate = True - for tree_element, instance in self.stack[:-1]: - # check all elements except the top one, but not the actions - if isinstance(instance, AbstractDecisionElement) and instance.get_reevaluate(): - if not isinstance(tree_element, DecisionTreeElement): - raise TypeError("Decision instance is not bound to a decision tree element") - result = instance.perform(True) - # Check whether this result would be an ELSE case - result_is_else = result not in tree_element.children.keys() - # Push element if necessary. Necessary means that the result is unequal to the activation reason of - # the next element in the stack, i.e. the decision has changed. However we have to account for ELSE! - if result_is_else and self.stack[self.stack_exec_index + 1][0].activation_reason == "ELSE": - # In this case the result returned by the decision does not match any of its possible results, - # therefore it goes in the 'ELSE' category. If the activation reason is 'ELSE', the decision did - # not change, that means no change in the stack is necessary. - pass - elif result != self.stack[self.stack_exec_index + 1][0].activation_reason: - # In this case, however, the activation reason actually did change. Therefore, we have to - # discard everything in the stack above the current decision and push the new result. - for _ in range(self.stack_exec_index + 1, len(self.stack)): - self.stack.pop()[1].on_pop() - self.stack_reevaluate = False - self.push(tree_element.get_child(result)) - - if not self.stack_reevaluate: - # We had some external interrupt, we stop here - return - self.stack_exec_index += 1 - self.stack_reevaluate = False - # Get the top module - current_tree_element, current_instance = self.stack[-1] + if not self.stack: + raise RuntimeError("No active behavior; load a behavior before update()") + if reevaluate and not self.do_not_reevaluate and self._reevaluate_stack(): + return if reevaluate: - # reset flag self.do_not_reevaluate = False - if isinstance(current_instance, AbstractActionElement) and current_instance.never_reevaluate: - # Deactivate reevaluation if action had never_reevaluate flag - self.set_do_not_reevaluate() - # Run the top module - top_result = current_instance.perform() - if isinstance(current_instance, AbstractDecisionElement): - if not isinstance(current_tree_element, DecisionTreeElement): - raise TypeError("Decision instance is not bound to a decision tree element") - self.push(current_tree_element.get_child(top_result)) - except Exception: - get_logger().error(str(traceback.format_exc())) + self._execute_top() + except Exception as error: + self._record_error(error) + finally: + self.stack_reevaluate = False + self.stack_exec_index = -1 + self._executing = False + for publish in (self.debug_publish_stack, self.debug_publish_current_action): + try: + publish() + except Exception as error: + self._record_error(error) def push(self, element: AbstractTreeElement[BlackboardT]) -> None: - """ - Put a new element on the stack and start it directly. - - This should only be called by the DSD, not from any of the modules - - :param element: The tree element that should be put on top of the stack. - """ - self.stack.append((element, self._init_element(element))) - - # we call the new element without another reevaluate - self.update(False) + """Append a frame and execute it immediately when called outside an update.""" + self._append(element) + if not self._executing: + self.update(False) def pop(self) -> None: + """Complete the current frame, initializing a chain successor for the next update. + + During reevaluation, remove the reevaluated frame and everything above it. + Popping the only frame remains a no-op. Suppression is retained when + advancing a chain, matching the existing sequence contract. """ - Removes the element from the stack. The previous element will not be called again. - """ - if len(self.stack) > 1: - if self.stack_reevaluate: - # we are currently reevaluating. we shorten the stack here - if self.stack_exec_index > 0: - ## only shorten stack if it still has one element - for _ in range(self.stack_exec_index, len(self.stack)): - self.stack.pop()[1].on_pop() - # stop reevaluating - self.stack_reevaluate = False - else: - current_tree_element = self.stack[-1][0] - if ( - isinstance(current_tree_element, ActionTreeElement) - and current_tree_element.next_element is not None - ): - # This action is part of a chain (e.g. a sequence). Instead of returning to the - # element below, we advance to the next element in the chain. - # We do not reset do_not_reevaluate because an action in the chain may control - # the stack beyond its own lifetime but within the chain's lifetime. - self.stack.pop()[1].on_pop() - next_element = current_tree_element.next_element - self.stack.append((next_element, self._init_element(next_element))) - return - # Remove the last element of the stack - self.stack.pop()[1].on_pop() - - # We will reevaluate even when the popped element set do_not_reevaluate - # because no module should control the stack beyond its lifetime + if len(self.stack) <= 1: + return + if self.stack_reevaluate: + self.stack_reevaluate = False + if self.stack_exec_index > 0: + self._discard_from(self.stack_exec_index) + self.do_not_reevaluate = False + return + tree_element = self.stack[-1][0] + successor = tree_element.next_element if isinstance(tree_element, ActionTreeElement) else None + if not self._discard_from(len(self.stack) - 1): + return + if successor is not None: + # Suppression spans the chain under the compatibility contract (see issue #121). + self._append(successor) + else: self.do_not_reevaluate = False def set_do_not_reevaluate(self) -> None: - """No reevaluation on next iteration""" + """Suppress reevaluation on the next update.""" self.do_not_reevaluate = True def get_stack(self) -> list[tuple[AbstractTreeElement[BlackboardT], AbstractStackElement[BlackboardT]]]: - """ - Returns the current stack - """ + """Return the active path, ordered from root to current frame. Treat it as read-only.""" return self.stack def debug_publish_stack(self) -> None: - """ - Publishes a JSON representation of the current stack - """ - if self.debug_active and self.debug_stack_publisher.get_subscription_count() != 0: - # Construct JSON encodable object which represents the current stack - data: StackData | None = None - for tree_elem, elem_instance in reversed(self.stack): - elem_data: StackData = { - **elem_instance.repr_dict(), - "activation_reason": tree_elem.activation_reason, - "id": id(tree_elem), - "next": data, - } - elem_data["activation_reason"] = tree_elem.activation_reason - # Attach the id of the corresponding tree element so the visualization can match this - # stack element to the exact tree element (names alone are ambiguous within a chain). - elem_data["id"] = id(tree_elem) - elem_data["next"] = data - data = elem_data - - msg = String(data=json.dumps(data)) - self.debug_stack_publisher.publish(msg) + publisher = self.debug_stack_publisher + if not self.debug_active or publisher is None or publisher.get_subscription_count() == 0: + return + data: StackData | None = None + for tree_element, instance in reversed(self.stack): + data = { + **instance.repr_dict(), + "activation_reason": tree_element.activation_reason, + "id": id(tree_element), + "next": data, + } + publisher.publish(String(data=json.dumps(data, default=str))) def debug_publish_tree(self) -> None: - """ - Publishes a JSON representation of the parsed tree - """ - if self.debug_active: - # Construct JSON encodable object which represents the current stack - if self.tree is None: - return - data = self.tree.repr_dict() - msg = String(data=json.dumps(data)) - self.debug_tree_publisher.publish(msg) + if self.debug_active and self.debug_tree_publisher is not None and self.tree is not None: + self.debug_tree_publisher.publish(String(data=json.dumps(self.tree.repr_dict(), default=str))) def debug_publish_current_action(self) -> None: - """ - Publishes the name of the currently active action - """ - # Check if debugging is active and if there is something on the stack - if not self.debug_active or len(self.stack) == 0: + publisher = self.debug_current_action_publisher + if not self.debug_active or publisher is None: return - - # Get the top element - stack_top = self.stack[-1][1] - # Check if it is an action and retrieve it - if isinstance(stack_top, AbstractActionElement): - current_action = stack_top - else: - return - - # Only publish if the action changed - if current_action.name != self.debug_active_action_cache: - # Publish the name of the current action - self.debug_current_action_publisher.publish(String(data=current_action.name)) - # Cache the current action name - self.debug_active_action_cache = current_action.name + top = self.stack[-1][1] if self.stack else None + name = top.name if isinstance(top, AbstractActionElement) else "" + if name != self.debug_active_action_cache: + publisher.publish(String(data=name)) + self.debug_active_action_cache = name diff --git a/dynamic_stack_decider/dynamic_stack_decider/tree.py b/dynamic_stack_decider/dynamic_stack_decider/tree.py index 496e8f0..b83226a 100644 --- a/dynamic_stack_decider/dynamic_stack_decider/tree.py +++ b/dynamic_stack_decider/dynamic_stack_decider/tree.py @@ -1,6 +1,5 @@ from __future__ import annotations -import sys from typing import TYPE_CHECKING, Generic from dynamic_stack_decider.types import BlackboardT, ParameterReferences, Parameters, TreeData @@ -121,9 +120,8 @@ def add_child_element(self, element: AbstractTreeElement[BlackboardT], activatin def get_child(self, activating_result: str | None) -> AbstractTreeElement[BlackboardT]: """Get the child for a given result""" - if not activating_result: - # give advice about error - sys.exit("Decision return was None. You probably forgot to return a string in your perform method.") + if not isinstance(activating_result, str) or not activating_result: + raise ValueError(f"Decision {self.name!r} must return a non-empty string, got {activating_result!r}") if activating_result in self.children.keys(): return self.children[activating_result] diff --git a/dynamic_stack_decider/test/test_discovery.py b/dynamic_stack_decider/test/test_discovery.py index 19d617e..8d24653 100644 --- a/dynamic_stack_decider/test/test_discovery.py +++ b/dynamic_stack_decider/test/test_discovery.py @@ -69,3 +69,38 @@ def test_register_decisions_filters_to_decisions(): # Actions are filtered out assert "FixtureActionA" not in dsd.decisions assert all(issubclass(cls, AbstractDecisionElement) for cls in dsd.decisions.values()) + + +def test_init_file_discovery_does_not_require_existing_sys_path(tmp_path): + import sys + + package = tmp_path / "dsd_unique_fixture_package" + package.mkdir() + path = package / "__init__.py" + path.write_text( + "from dynamic_stack_decider import AbstractActionElement\n" + "class DiscoveredAction(AbstractActionElement):\n" + " def perform(self, reevaluate=False): pass\n" + ) + original_path = list(sys.path) + try: + assert "DiscoveredAction" in discover_elements(path) + assert sys.path == original_path + finally: + sys.modules.pop(package.name, None) + + +def test_discovery_restores_sys_path_if_package_walk_fails(monkeypatch): + import sys + + import dynamic_stack_decider.dsd as runtime + + original_path = list(sys.path) + + def fail(*args, **kwargs): + raise RuntimeError("package walk failed") + + monkeypatch.setattr(runtime.pkgutil, "walk_packages", fail) + with pytest.raises(RuntimeError, match="package walk failed"): + discover_elements(FIXTURES) + assert sys.path == original_path diff --git a/dynamic_stack_decider/test/test_elements.py b/dynamic_stack_decider/test/test_elements.py index 32bcc58..8dfff09 100644 --- a/dynamic_stack_decider/test/test_elements.py +++ b/dynamic_stack_decider/test/test_elements.py @@ -127,3 +127,21 @@ def test_base_repr_dict(): "name": "Idle", "debug_data": {}, } + + +def test_nested_debug_values_are_serializable(): + import json + + action = make_action() + action.publish_debug_data("nested", {"items": [object(), None]}) + assert isinstance(action._debug_data["nested"]["items"][0], str) + assert action._debug_data["nested"]["items"][1] is None + json.dumps(action.repr_dict(), allow_nan=False) + + +def test_cyclic_debug_data_is_represented_as_a_string(): + action = make_action() + value = [] + value.append(value) + action.publish_debug_data("cyclic", value) + assert isinstance(action._debug_data["cyclic"], str) diff --git a/dynamic_stack_decider/test/test_ros_debug.py b/dynamic_stack_decider/test/test_ros_debug.py new file mode 100644 index 0000000..1037dd1 --- /dev/null +++ b/dynamic_stack_decider/test/test_ros_debug.py @@ -0,0 +1,62 @@ +"""Exercise debug topics through a real ROS node and DDS publishers/subscribers.""" + +import json +import time +from collections.abc import Callable, Iterator + +import pytest +import rclpy +from rclpy.context import Context +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile +from std_msgs.msg import String + +from .runtime_elements import make_dsd + + +@pytest.fixture +def ros_node() -> Iterator[Node]: + context = Context() + rclpy.init(context=context) + node = Node("dsd_debug_test", context=context) + try: + yield node + finally: + node.destroy_node() + context.shutdown() + + +def wait_for(node: Node, predicate: Callable[[], bool]) -> None: + executor = SingleThreadedExecutor(context=node.context) + executor.add_node(node) + try: + deadline = time.monotonic() + 8 + while not predicate() and time.monotonic() < deadline: + executor.spin_once(timeout_sec=0.05) + assert predicate(), "ROS debug data was not received before the deadline" + finally: + executor.remove_node(node) + executor.shutdown() + + +@pytest.mark.ros +def test_debug_topics_and_late_subscribers(ros_node: Node) -> None: + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle\n", node=ros_node, debug_topic="test_debug") + dsd.blackboard.results["Chooser"] = "GO" + trees: list[String] = [] + stacks: list[String] = [] + actions: list[String] = [] + latched = QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) + ros_node.create_subscription(String, "test_debug/dsd_tree", trees.append, latched) + ros_node.create_subscription(String, "test_debug/dsd_stack", stacks.append, 10) + wait_for(ros_node, lambda: bool(trees) and dsd.debug_stack_publisher.get_subscription_count() > 0) + dsd.update() + wait_for(ros_node, lambda: bool(stacks)) + # A subscriber connecting after the update receives the active action too. + ros_node.create_subscription(String, "test_debug/dsd_current_action", actions.append, latched) + wait_for(ros_node, lambda: bool(actions)) + assert json.loads(trees[-1].data)["name"] == "Chooser" + stack = json.loads(stacks[-1].data) + assert stack["next"]["name"] == "Idle" + assert actions[-1].data == "Idle" diff --git a/dynamic_stack_decider/test/test_runtime.py b/dynamic_stack_decider/test/test_runtime.py index 664538d..2b87fdd 100644 --- a/dynamic_stack_decider/test/test_runtime.py +++ b/dynamic_stack_decider/test/test_runtime.py @@ -483,10 +483,7 @@ def test_reevaluation_pop_at_root_does_not_shorten_stack(): def test_reevaluation_pop_in_the_middle_shortens_stack(): - # A reevaluating decision that is not at the bottom pops itself during reevaluation. This shortens - # the stack down to the reevaluation index. The engine then swallows the resulting internal error - # (this is an unusual edge case); the important contract is that update() does not raise and the - # stack ends up shortened. + # A popped decision cancels reevaluation before the removed frame can be inspected. dsd = make_dsd( """ -->T @@ -541,7 +538,7 @@ def test_unknown_decision_result_is_swallowed(): assert stack_names(dsd) == ["Chooser"] -def test_decision_returning_none_exits(): +def test_decision_returning_none_is_reported(): dsd = make_dsd( """ -->T @@ -550,16 +547,16 @@ def test_decision_returning_none_exits(): """ ) - # Returning None from a decision is a programming error and triggers sys.exit - with pytest.raises(SystemExit): - dsd.update() + dsd.update() + assert isinstance(dsd.last_error, ValueError) + assert stack_names(dsd) == ["ReturnNone"] # --------------------------------------------------------------------------- # _bind_modules error paths # --------------------------------------------------------------------------- def test_unknown_action_name_raises_on_load(): - with pytest.raises(AssertionError): + with pytest.raises(ValueError): make_dsd( """ -->T @@ -570,7 +567,7 @@ def test_unknown_action_name_raises_on_load(): def test_unknown_decision_name_raises_on_load(): - with pytest.raises(AssertionError): + with pytest.raises(ValueError): make_dsd( """ -->T @@ -741,3 +738,261 @@ def test_fresh_blackboard_between_dsds(): dsd1.update() assert ("perform", "Idle") in bb1.events assert bb2.events == [] + + +def test_interrupt_cleans_every_frame_once_in_reverse_order(monkeypatch): + dsd = make_dsd("-->\n$Chooser\n GO --> $Guard\n GO --> @Idle\n") + dsd.blackboard.results.update(Chooser="GO", Guard="GO") + dsd.update() + cleaned = [] + for _, instance in dsd.stack: + monkeypatch.setattr(instance, "on_pop", lambda name=instance.name: cleaned.append(name)) + dsd.interrupt() + assert cleaned == ["Idle", "Guard", "Chooser"] + assert stack_names(dsd) == ["Chooser"] + assert dsd.last_error is None + + +def test_interrupt_clears_reevaluation_suppression(): + dsd = make_dsd("-->\n$Guard\n A --> @Idle + r:false\n B --> @Run\n") + dsd.blackboard.results["Guard"] = "A" + dsd.update() + assert dsd.do_not_reevaluate + dsd.interrupt() + assert not dsd.do_not_reevaluate + dsd.blackboard.results["Guard"] = "B" + dsd.update() + assert stack_names(dsd) == ["Guard", "Run"] + + +@pytest.mark.parametrize("decision", ["InterruptOnReeval", "PopOnReeval"]) +def test_stack_mutation_during_reevaluation_does_not_log_an_internal_error(decision): + dsd = make_dsd(f"-->\n$Guard\n GO --> ${decision}\n GO --> @Idle\n") + dsd.blackboard.results.update(Guard="GO", **{decision: "GO"}) + dsd.update() + dsd.update() + assert stack_names(dsd) == ["Guard"] + assert dsd.last_error is None + assert not dsd.stack_reevaluate + + +def test_interrupt_during_normal_decision_does_not_push_stale_outcome(monkeypatch): + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle\n") + root = dsd.stack[0][1] + + def perform(reevaluate=False): + dsd.interrupt() + return "GO" + + monkeypatch.setattr(root, "perform", perform) + dsd.update() + assert stack_names(dsd) == ["Chooser"] + assert dsd.blackboard.events == [] + assert dsd.last_error is None + + +def test_interrupt_from_reevaluation_predicate_cancels_tick(monkeypatch): + dsd = make_dsd("-->\n$Guard\n GO --> @Idle\n") + dsd.blackboard.results["Guard"] = "GO" + dsd.update() + + def get_reevaluate(): + dsd.interrupt() + return True + + monkeypatch.setattr(dsd.stack[0][1], "get_reevaluate", get_reevaluate) + dsd.update() + assert stack_names(dsd) == ["Guard"] + assert dsd.last_error is None + + +def test_reevaluation_error_resets_traversal_state(monkeypatch): + dsd = make_dsd("-->\n$Guard\n GO --> @Idle\n") + dsd.blackboard.results["Guard"] = "GO" + dsd.update() + + def fail(reevaluate=False): + raise ValueError("broken guard") + + monkeypatch.setattr(dsd.stack[0][1], "perform", fail) + dsd.update() + assert isinstance(dsd.last_error, ValueError) + assert not dsd.stack_reevaluate + assert dsd.stack_exec_index == -1 + monkeypatch.setattr(dsd.stack[0][1], "get_reevaluate", lambda: False) + dsd.update() + assert dsd.last_error is None + assert dsd.blackboard.events.count(("perform", "Idle")) == 2 + + +def test_cleanup_exception_does_not_skip_other_frames(monkeypatch): + dsd = make_dsd("-->\n$Guard\n A --> $Chooser\n GO --> @Idle\n B --> @Run\n") + dsd.blackboard.results.update(Guard="A", Chooser="GO") + dsd.update() + cleaned = [] + + def fail(): + cleaned.append("Idle") + raise ValueError("cleanup failed") + + monkeypatch.setattr(dsd.stack[-1][1], "on_pop", fail) + monkeypatch.setattr(dsd.stack[-2][1], "on_pop", lambda: cleaned.append("Chooser")) + dsd.blackboard.results["Guard"] = "B" + dsd.update() + assert cleaned == ["Idle", "Chooser"] + assert stack_names(dsd) == ["Guard", "Run"] + assert isinstance(dsd.last_error, ValueError) + + +def test_cleanup_interrupt_does_not_push_stale_branch(monkeypatch): + dsd = make_dsd("-->\n$Guard\n A --> @Idle\n B --> @Run\n") + dsd.blackboard.results["Guard"] = "A" + dsd.update() + monkeypatch.setattr(dsd.stack[-1][1], "on_pop", dsd.interrupt) + dsd.blackboard.results["Guard"] = "B" + dsd.update() + assert stack_names(dsd) == ["Guard"] + assert ("perform", "Run") not in dsd.blackboard.events + assert dsd.last_error is None + + +def test_invalid_reevaluation_outcome_keeps_existing_action(): + dsd = make_dsd("-->\n$Guard\n GO --> @Idle\n") + dsd.blackboard.results["Guard"] = "GO" + dsd.update() + dsd.blackboard.results["Guard"] = "INVALID" + dsd.update() + assert stack_names(dsd) == ["Guard", "Idle"] + assert ("pop", "Idle") not in dsd.blackboard.events + assert isinstance(dsd.last_error, KeyError) + + +def test_chain_cleanup_precedes_successor_construction(monkeypatch): + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle, @Run\n") + dsd.blackboard.results["Chooser"] = "GO" + dsd.update() + order = [] + monkeypatch.setattr(dsd.stack[-1][1], "on_pop", lambda: order.append("cleanup")) + original_init = dsd._init_element + + def initialize(element): + order.append("initialize") + return original_init(element) + + monkeypatch.setattr(dsd, "_init_element", initialize) + dsd.pop() + assert order == ["cleanup", "initialize"] + assert stack_names(dsd) == ["Chooser", "Run"] + assert ("perform", "Run") not in dsd.blackboard.events + + +def test_parameters_are_independent_of_tree_and_next_activation(): + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle + items:[1]\n") + dsd.blackboard.results["Chooser"] = "GO" + dsd.update() + dsd.stack[-1][1].parameters["items"].append(2) + dsd.pop() + dsd.update() + assert dsd.stack[-1][1].parameters == {"items": [1]} + assert dsd.stack[-1][0].parameters == {"items": [1]} + + +def test_failed_load_keeps_current_behavior(tmp_path): + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle\n") + dsd.blackboard.results["Chooser"] = "GO" + dsd.update() + old_tree = dsd.tree + old_stack = list(dsd.stack) + bad = tmp_path / "unknown.dsd" + bad.write_text("-->\n@Unregistered\n") + with pytest.raises(ValueError, match="not registered"): + dsd.load_behavior(bad) + assert dsd.tree is old_tree + assert dsd.stack == old_stack + assert ("pop", "Idle") not in dsd.blackboard.events + + +def test_behavior_reload_cleans_old_action(tmp_path): + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle\n") + dsd.blackboard.results["Chooser"] = "GO" + dsd.update() + path = tmp_path / "replacement.dsd" + path.write_text("-->\n@Run\n") + dsd.load_behavior(path) + assert stack_names(dsd) == ["Run"] + assert dsd.blackboard.events[-1] == ("pop", "Idle") + assert ("perform", "Run") not in dsd.blackboard.events + + +def test_deep_decision_path_does_not_use_python_recursion(): + from dynamic_stack_decider.dsd import DSD + from dynamic_stack_decider.tree import ActionTreeElement, DecisionTreeElement + + from .runtime_elements import Guard, Idle + + root = DecisionTreeElement("Guard", None) + root.module = Guard + current = root + for _ in range(1200): + child = DecisionTreeElement("Guard", current) + child.module = Guard + current.add_child_element(child, "GO") + current = child + action = ActionTreeElement("Idle", current) + action.module = Idle + current.add_child_element(action, "GO") + blackboard = Blackboard() + blackboard.results["Guard"] = "GO" + dsd = DSD(blackboard) + dsd.set_start_element(root) + dsd.update() + assert len(dsd.stack) == 1202 + assert blackboard.events[-1] == ("perform", "Idle") + assert dsd.last_error is None + + +def test_recursive_update_is_reported_without_executing_another_tick(monkeypatch): + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle\n") + dsd.blackboard.results["Chooser"] = "GO" + dsd.update() + monkeypatch.setattr(dsd.stack[-1][1], "perform", lambda: dsd.update()) + dsd.update() + assert isinstance(dsd.last_error, RuntimeError) + assert "already running" in str(dsd.last_error) + assert stack_names(dsd) == ["Chooser", "Idle"] + + +def test_debug_topic_without_node_does_not_disable_execution(): + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle\n", debug_topic="debug") + dsd.blackboard.results["Chooser"] = "GO" + dsd.update() + assert ("perform", "Idle") in dsd.blackboard.events + assert not dsd.debug_active + assert dsd.last_error is None + + +def test_debug_failure_cannot_prevent_action_execution(monkeypatch): + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle\n") + dsd.blackboard.results["Chooser"] = "GO" + + def fail(): + raise ValueError("debug failure") + + monkeypatch.setattr(dsd, "debug_publish_stack", fail) + dsd.update() + dsd.update() + assert dsd.blackboard.events.count(("perform", "Idle")) == 2 + assert isinstance(dsd.last_error, ValueError) + + +def test_debug_publishes_state_after_action_pop(): + node = FakeNode() + dsd = make_dsd("-->\n$Chooser\n GO --> @Idle\n", node=node, debug_topic="debug") + dsd.blackboard.results["Chooser"] = "GO" + dsd.update() + dsd.pop() + dsd.blackboard.results["Chooser"] = "INVALID" + dsd.update() + stack = json.loads(node.publishers["debug/dsd_stack"].messages[-1].data) + assert stack["next"] is None + assert node.publishers["debug/dsd_current_action"].messages[-1].data == "" diff --git a/dynamic_stack_decider/test/test_tree.py b/dynamic_stack_decider/test/test_tree.py index 41b9799..eb07500 100644 --- a/dynamic_stack_decider/test/test_tree.py +++ b/dynamic_stack_decider/test/test_tree.py @@ -33,9 +33,9 @@ def test_get_child_unknown_result_raises_keyerror(): d.get_child("Y") -def test_get_child_none_result_exits(): +def test_get_child_none_result_raises_valueerror(): d = DecisionTreeElement("D", None) - with pytest.raises(SystemExit): + with pytest.raises(ValueError, match="non-empty string"): d.get_child(None) diff --git a/pixi.toml b/pixi.toml index 20ee5d1..6e4d611 100644 --- a/pixi.toml +++ b/pixi.toml @@ -25,7 +25,7 @@ wheel = ">=0.45,<1" pyyaml = ">=6,<7" [feature.test.tasks] -test = { cmd = "pytest -p pytest_cov --cov=dynamic_stack_decider/dynamic_stack_decider --cov-report=term-missing", env = { PYTEST_DISABLE_PLUGIN_AUTOLOAD = "1" } } +test = { cmd = "pytest -p pytest_cov --cov=dynamic_stack_decider/dynamic_stack_decider --cov-report=term-missing", env = { PYTEST_DISABLE_PLUGIN_AUTOLOAD = "1", ROS_AUTOMATIC_DISCOVERY_RANGE = "LOCALHOST" } } package = "python scripts/check_packages.py" typecheck = "mypy dynamic_stack_decider/dynamic_stack_decider scripts examples" demo = { cmd = "python examples/demo.py", env = { PYTHONPATH = "dynamic_stack_decider" } } diff --git a/pyproject.toml b/pyproject.toml index cccfd58..6c4136e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -10,6 +10,7 @@ select = ["F", "E", "B", "W", "I", "N", "UP"] testpaths = ["dynamic_stack_decider/test", "dynamic_stack_decider_visualization/test"] pythonpath = ["dynamic_stack_decider", "dynamic_stack_decider_visualization"] addopts = "--strict-config --strict-markers -ra" +markers = ["ros: integration test using ROS middleware"] [tool.mypy] python_version = "3.10"