diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index 10e491e..97f3900 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -47,3 +47,4 @@ jobs: cache: true environments: visualization - run: pixi run --locked -e visualization typecheck-gui + - run: pixi run --locked -e visualization test-gui diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 36989a8..aaa5f77 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -10,6 +10,7 @@ pixi run --locked test pixi run --locked package pixi run --locked typecheck pixi run --locked -e visualization typecheck-gui +pixi run --locked -e visualization test-gui pixi run --locked -e format format pixi run --locked -e format lint ``` @@ -26,7 +27,9 @@ environment. ROS package dependencies also belong in the corresponding Both packages retain their ament Python layout and support standard Python distribution builds. `package` builds source distributions and wheels and checks -that ROS resources are present and test fixtures are excluded. +that ROS resources and typing metadata are present. Test fixtures are included +in source distributions and excluded from wheels; isolated imports verify that +the built packages can be imported without using the checkout. Add focused regression tests for parser or runtime changes. Runtime tests use small fake elements and assert observable stack and event behavior. Tests that @@ -43,3 +46,10 @@ ship `py.typed`; blackboards are generic and parameter values use `object` to reflect the existing YAML and ROS inputs. Narrow a parameter with `isinstance` before using it as a particular type. Untyped ROS integrations are confined to third-party imports, and the small RQT stubs document the interfaces used here. + +The core test task includes localhost DDS integration. GUI tests use offscreen Qt +and Graphviz in the visualization environment; the default environment skips +those modules when Qt is absent. The dedicated GUI task requires Qt, so CI cannot +silently skip its tests. All dependency installation can be left to CI when local +bandwidth is limited; use the locked environments already available locally for +focused checks. diff --git a/README.md b/README.md index 701ba45..4228a9e 100644 --- a/README.md +++ b/README.md @@ -1,185 +1,147 @@ -# dynamic_stack_decider +# Dynamic Stack Decider -A lightweight framework for decision making and behavior defining for robots and other agents in changing environments. -It combines the advantages of behavior trees, decision trees and state machines. +DSD selects persistent actions through a hierarchy of decisions that can be +reevaluated as the environment changes. The active stack records the decisions +behind the current action, making it useful for inspecting robot behavior. -## General Structure +Python classes implement decisions and actions. A `.dsd` file defines their +control flow: -In general, the dynamic stack decider consists of actions and decisions. -A decision is a python class extending `dynamic_stack_decider.AbstractDecisionElement`. -It should implement the `perform()` method where, depending on some information, a string is returned. - -An action is a python class extending `dynamic_stack_decider.AbtractActionElement`. -It should also implement the `perform()` method where code can be executed that performs an action. -It should not return anything. - -The DSD follows a tree-like structure that is specified in the DSD domain-specific language. -Let's look at an example: - -``` ---> -$Mode - BALL --> $BallSeen - YES --> @TrackBall - NO --> @SearchBall - PATTERN --> @LookAround +```text +-->HeadBehavior +$BallSeen + YES --> @TrackBall + NO --> @SearchBall ``` -This example is a very simple head behavior for a robot. -The arrow (`-->`) at the beginning of the file indicates the start of the behavior. -Decisions are prefix with `$`, actions with `@`. -Their names correspond to the class names of the python classes implementing the respective action or decision. - -The first decision is the `Mode` decision. -It decides about the mode of the head and returns the string `BALL` or `PATTERN`. -By convention, the returned strings are in capital letters. -The returned strings are indented and followed by an arrow; the element after the arrow is executed when the corresponding string is returned by the decision. +`BallSeen.perform()` returns an outcome such as `YES` or `NO`. +`TrackBall.perform()` and `SearchBall.perform()` perform one tick of work and +return nothing. Actions retain their instance state between updates and complete +with `return self.pop()`. -

-A visualization of the Stack -

+## Try the example -The underlying model for the DSD is a stack. -The root element (`Mode` in the example above) is always at the bottom of the stack. -When it makes a decision, the result is pushed on the stack and executed, for example the `BallSeen` decision. -Then, depending on the result, the next decision or action is pushed and executed, for example the `TrackBall` action. -This actions stays on top of the stack until one of the following conditions is met: -1. It calls `self.pop()` to pop itself from the stack. - Then, the decision that is now on top of the stack is executed again and depending on the result, another action or decision is pushed. -2. One of the decisions on the stack is reevaluated (see #Reevaluation below). +Install [Pixi](https://pixi.sh), then run the checked-in example: -The `__init__` method of an element is called when it is pushed on the stack. -It receives the following arguments: -* `blackboard`: an arbitrary object that is shared between all stack elements -* `dsd`: the instance of the dynamic stack decider -* `parameters`: an optional dictionary of parameters +```sh +pixi run --locked demo +``` -The `perform` method is called when it is on top of the stack or getting reevaluated. -It receives the argument `reevaluate` which is set to `True` when the element is getting reevaluated and to `False` when it is executed normally. +The example uses a shared blackboard and switches from searching to tracking. +It requires neither a ROS node nor hardware. The core currently imports ROS +Python libraries; Pixi supplies those dependencies. -## Starting the DSD +## Define elements -To start the DSD, a `dynamic_stack_decider.DSD` object has to be created. -Its arguments are a blackboard, which is passed to all elements, and an optional ROS topic that is used for debugging. -Then, `register_actions` and `register_decisions` have to be called with the name of a folder containing the actions and decisions, respectively. -After registering the elements, `load_behavior` should be used to load the file containing the behavior description in the DSD language. -Finally, to actually execute the stack decider, `update` should be called repeatedly. +The blackboard can be any application object. Use the generic base classes to +keep its interface visible to the type checker: -On each update, all elements on the stack except the top elements are reevaluated from bottom to top, if requested. -Then, the top element is executed. -If it was a decision, the resulting action is pushed on the stack, to be executed in the next call to `update`. +```python +from dataclasses import dataclass -## Reevaluation +from dynamic_stack_decider import AbstractActionElement, AbstractDecisionElement -Normally, only the topmost action or decision of the stack is executed and another decision can only be executed if the topmost element pops itself from the stack. -Sometimes, it is useful to also execute particular decisions when they are not on top of the stack. -For example in the example above, if the mode can be set externally, it would make sense to check on every update of the stack if the decision has changed. -To achieve that behavior, the `get_reevaluate` should be implemented, either to just return `True` or to implement more sophisticated reevaluation criteria. -When the element is reevaluated, `perform` is called on the element. -If the result of `perform` is the same as the last time it was called, nothing happens and the next decision is reevaluated. -If the result of `perform` is different from the last time, everything above the reevaluated decision is discarded and a new element, depending on the result of `perform`, is pushed on the stack and executed. +@dataclass +class Blackboard: + ball_seen: bool = False -An action can call `self.do_not_reevaluate()` to avoid reevaluation of the stack on the next call to `update`. Alternatively the parameters (see #Parameters) `r` or `reevaluate` can be used in the dsd file to enable or disable the reevaluation during a specific action. -## Sequence elements +class BallSeen(AbstractDecisionElement[Blackboard]): + def perform(self, reevaluate: bool = False) -> str: + return "YES" if self.blackboard.ball_seen else "NO" -Sequence elements can be used to perform multiple actions instead of a single one. -To use a sequence element, simply separate the actions with commas, like that: `@FirstAction, @SecondAction, @ThirdAction`. -When the sequence element is pushed on the stack, the first action of the sequence remains on top of the stack until it pops itself from the stack. -Then, the next action in the sequence is pushed to the stack. + def get_reevaluate(self) -> bool: + return True -## Parameters -Sometimes it is helpful to pass parameters to actions or decisions. -The syntax is `@Action + parameter:value`, multiple parameters can be passed using further `+ parameter:value` pairs. -The parameters to an element are passed to the element's constructor as a dictionary. -The value of the parameter is passed through `yaml.safe_load`. -Therefore, you can use integers, floats, booleans or strings as values. -It is also possible to load parameters from the ROS parameter server using `parameter:%rosparam`, where `rosparam` is the name of the parameter on the parameter server. +class SearchBall(AbstractActionElement[Blackboard]): + def perform(self, reevaluate: bool = False) -> None: + # Start or monitor the application's search operation here. + pass -Here is an example: -``` ---> -$Mode - BALL --> $BallSeen - YES --> @TrackBall + time:10 - NO --> @SearchBall - PATTERN --> @LookAround + def on_pop(self) -> None: + # Cancel outstanding work and release resources here. + pass ``` -## Subtrees +The constructor receives the shared `blackboard`, the owning `dsd`, and a +`parameters` dictionary. A subclass overriding it must call `super().__init__`. +Parameter values are YAML- or ROS-provided objects; narrow them with `isinstance` +before treating them as a specific type. Each activation receives its own copy. -To avoid duplication and deep nested structures in the DSD file, subtrees can be used. -Here is an example that is equivalent to the example above: -``` -#BallMode -$BallSeen - YES --> @TrackBall - NO --> @SearchBall +## Run a behavior ---> -$Role - BALL --> #BallMode - PATTERN --> @LookAround -``` +Create a `DSD(blackboard)` instance, register actions and decisions, then load a +behavior. Registries map class names to their implementations; alternatively, +`register_actions(path)` and `register_decisions(path)` discover classes in a +Python file or package. The [runnable example](examples/demo.py) shows explicit +registries. -Subtrees can also receive parameters. -Here is another example: -``` +Call `update()` regularly from one owner. It reevaluates eligible decisions from +the bottom of the active stack, then follows decision outcomes immediately until +an action runs. Actions in a comma-separated chain advance on completion; the +successor runs on the next update. -#BallMode -$BallSeen + tracktime - YES --> @TrackBall + time:*tracktime - NO --> @SearchBall +Use `on_pop()` for cleanup on completion, preemption, interruption, and behavior +replacement. Return immediately after calling `pop()` or `interrupt()`. +`dsd.last_error` exposes the first failure of the latest update; ordinary update +failures are logged. Applications must choose their recovery behavior. ---> -$Role - BALL --> #BallMode + tracktime:10 - PATTERN --> @LookAround -``` +See the [execution contract](docs/runtime.md) for timing, cleanup, suppression, +root behavior, and error semantics. The [language reference](docs/language.md) +covers action chains, `ELSE`, comments, parameters, and reusable subtrees. Parser +errors include the source file and line. -As you can see, the parameters are specified in the same way as for actions or decisions. -In the subtree, the parameter has to be given a name using the `+` syntax and can be referenced with the `*` operator. +## Inspect a running DSD -## ELSE +Pass both a ROS node and a debug topic namespace: -`ELSE` is a catch-all decision result. -It can be used to map multiple results to a single action or decision. -For example, the `Role` decision could return the roles `BALL`, `GOAL`, `ROBOT`, and `PATTERN`, but the latter three should result in the same behavior: +```python +dsd = DSD(blackboard, node=node, debug_topic="behavior/debug") ``` ---> -$Mode - BALL --> $BallSeen - YES --> @TrackBall - NO --> @SearchBall - ELSE --> @LookAround + +The RQT visualization discovers DSD instances through their debug topics and +shows the behavior tree, active path, and debug data. Start the installed +visualization package with: + +```sh +ros2 run dynamic_stack_decider_visualization dsd_gui ``` -## Interrupt +Debug topics use JSON in `std_msgs/msg/String`. The tree and current-action topics +use transient-local durability. Element IDs identify nodes within a loaded tree; +they are not persistent IDs across process restarts. -A stack element can call `self.interrupt()` to create an interrupt which removes all elements except for the start element from the stack (i.e., the DSD is restarted). +Elements can attach debug values with `publish_debug_data(label, value)` and clear +them with `clear_debug_data()`. Unsupported values are represented as strings. +Debug failures cannot prevent behavior execution. -## Debugging +## Install and develop -For debugging, the DSD visualization should be used. -It automatically discovers all running DSDs with a debug topic and visualizes them in RQT. -The visualization shows the DSD tree with the currently active actions and decisions as a tree on the left and the current stack on the right. -More information can be added to the right side by calling `self.publish_debug_data(label, data)` in any action or decision. +Both packages retain their ROS 2 ament Python layout and support source and wheel +builds. In a ROS workspace, use the workspace's `rosdep` and `colcon` workflow. +ROS dependencies are declared in `package.xml`; Python dependencies are declared +in `setup.py`. Installing a wheel alone does not install ROS. -## DSD Development GUI -If you want to use a graphical user interface to define your DSD, we recommend [this repository](https://github.com/bit-bots/dsd-gui) +This repository's locked Pixi environments provide core testing, optional Qt/RQT +checks, and lightweight formatting tools. See [CONTRIBUTING.md](CONTRIBUTING.md) +for commands and dependency updates. The default test environment avoids the GUI +dependencies; the visualization CI job runs offscreen Qt and Graphviz tests. -## VSCode Extension -If you use VSCode, you can use the [DSD extension](https://marketplace.visualstudio.com/items?itemName=Mastermori.dsd) which provides syntax highlighting. +## Background and open decisions -## Examples +The architecture is described in +[DSD — Dynamic Stack Decider](https://link.springer.com/article/10.1007/s12369-021-00768-8). +The [Bit-Bots workspace](https://github.com/bit-bots/bitbots_main) contains body +behavior, humanoid control, and localization examples. -Here are a few projects that use the DSD and can be used for reference: -* The [Bit-Bots Behavior](https://github.com/bit-bots/bitbots_main/tree/main/bitbots_behavior) -* The [Bit-Bots Humanoid Control Module](https://github.com/bit-bots/bitbots_main/tree/main/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd) -* The [Parser Test](https://github.com/bit-bots/dynamic_stack_decider/tree/master/dynamic_stack_decider/test) in this repository +Architectural choices are tracked separately: -## The Paper +- [Failure recovery and action outcomes](https://github.com/bit-bots/dynamic_stack_decider/issues/115) +- [ROS integration and registration boundaries](https://github.com/bit-bots/dynamic_stack_decider/issues/116) +- [Parameter grammar and outcome validation](https://github.com/bit-bots/dynamic_stack_decider/issues/117) +- [Root-chain completion and suppression scope](https://github.com/bit-bots/dynamic_stack_decider/issues/121) -The framework is also described in more depth in [DSD – Dynamic Stack Decider](https://link.springer.com/article/10.1007/s12369-021-00768-8) +The maintenance changes preserve existing chain timing and suppression behavior +while tightening invalid-input handling and lifecycle guarantees. diff --git a/dynamic_stack_decider/LICENSE b/dynamic_stack_decider/LICENSE new file mode 100644 index 0000000..9a25cc9 --- /dev/null +++ b/dynamic_stack_decider/LICENSE @@ -0,0 +1,19 @@ +Copyright (c) Hamburg Bit-Bots + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/dynamic_stack_decider/MANIFEST.in b/dynamic_stack_decider/MANIFEST.in index bb3ec5f..fc3854f 100644 --- a/dynamic_stack_decider/MANIFEST.in +++ b/dynamic_stack_decider/MANIFEST.in @@ -1 +1,2 @@ -include README.md +include README.md LICENSE +recursive-include test *.py *.dsd diff --git a/dynamic_stack_decider/dynamic_stack_decider/tree.py b/dynamic_stack_decider/dynamic_stack_decider/tree.py index b83226a..26c5027 100644 --- a/dynamic_stack_decider/dynamic_stack_decider/tree.py +++ b/dynamic_stack_decider/dynamic_stack_decider/tree.py @@ -9,38 +9,28 @@ class Tree(Generic[BlackboardT]): - """A tree defining a behaviour, parsed from a .dsd file""" + """A parsed behavior or subtree definition.""" def __init__(self) -> None: - # The root element of the tree self.root_element: AbstractTreeElement[BlackboardT] | None = None self.parameter_list: list[str] = [] def set_root_element(self, element: AbstractTreeElement[BlackboardT]) -> None: - """ - Set the root element of the tree - - :param element: The root element - """ + """Set the root blueprint of this definition.""" self.root_element = element def __repr__(self) -> str: return repr(self.root_element) def repr_dict(self) -> TreeData: - """ - Represent this tree as dictionary which is JSON encodable - """ + """Serialize this blueprint for the debug protocol.""" if self.root_element is None: raise ValueError("Tree has no root element") return self.root_element.repr_dict() class AbstractTreeElement(Generic[BlackboardT]): - """ - An element (node) in the tree. Do not use directly, - use one of DecisionTreeElement and ActionTreeElement instead - """ + """A blueprint bound to an element class before activation.""" def __init__( self, @@ -59,27 +49,17 @@ def __init__( self.activation_reason: str | None = None def get_child(self, activating_result: str) -> AbstractTreeElement[BlackboardT] | None: - """ - Get the child that should be activated for the given result. - This makes only sense for DecisionTreeElements; others return None - - :param activating_result: The result that activates the returned child - :return: The corresponding child element or None - """ + """Return the selected child; actions have no decision children.""" return None def set_activation_reason(self, reason: str | None) -> None: - """Set the result that activated this element""" + """Record the selecting branch, propagating it along an action chain.""" self.activation_reason = reason def repr_dict(self) -> TreeData: - """ - Represent this tree element as dictionary which is JSON encodable - """ + """Serialize this blueprint for the debug protocol.""" return { - # A stable identifier for this tree element. It is used to match a stack element to the - # exact tree element it belongs to, which is required because a chain (i.e. a sequence) - # may contain multiple elements with the same name (e.g. @Wait, @SomeAction, @Wait). + # IDs distinguish repeated class names within this loaded tree. "id": id(self), "name": self.name, "parameters": self.parameters, @@ -87,10 +67,7 @@ def repr_dict(self) -> TreeData: class DecisionTreeElement(AbstractTreeElement[BlackboardT]): - """ - A tree element describing a decision. A decision has children that are executed on a certain result. - Children can be added with add_child_element - """ + """Map outcomes to child blueprints, with optional ELSE fallback.""" def __init__( self, @@ -99,33 +76,23 @@ def __init__( parameters: Parameters | None = None, unset_parameters: ParameterReferences | None = None, ) -> None: - """ - Create a new DecisionTreeElement - - :param name: the class name of the corresponding AbstractDecisionElement - :param parent: the parent element, None for the root element - :param parameters: A dictionary of parameters - :param unset_parameters: A dictionary of parameters that must be set later - """ - # Call the constructor of the superclass super().__init__(name, parent, parameters, unset_parameters) - # Dictionary that maps results of the decision to the corresponding child self.children: dict[str, AbstractTreeElement[BlackboardT]] = dict() def add_child_element(self, element: AbstractTreeElement[BlackboardT], activating_result: str) -> None: - """Add a child that will be executed when activating_result is returned""" + """Attach a blueprint to a decision outcome.""" self.children[activating_result] = element element.set_activation_reason(activating_result) def get_child(self, activating_result: str | None) -> AbstractTreeElement[BlackboardT]: - """Get the child for a given result""" + """Return the matching child or ELSE; reject invalid or unmapped outcomes.""" 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(): + if activating_result in self.children: return self.children[activating_result] - elif "ELSE" in self.children.keys(): + elif "ELSE" in self.children: return self.children["ELSE"] else: raise KeyError(f"{activating_result} does not lead to a child of {str(self)} and no ELSE was specified") @@ -140,28 +107,15 @@ def __str__(self) -> str: return self.name def repr_dict(self) -> TreeData: - """ - Represent this decision as dictionary which is JSON encodable - """ - # Get the dict from the superclass + """Serialize this blueprint for the debug protocol.""" result = super().repr_dict() - # Add additional information result["type"] = "decision" result["children"] = {result: child.repr_dict() for result, child in self.children.items()} return result class ActionTreeElement(AbstractTreeElement[BlackboardT]): - """ - A tree element describing an action. An action has optional - parameters that will be passed to the module on creation. - - An action may point to a ``next_element`` which is activated once the action - pops itself off the stack. This is how sequences (``@A, @B, @C``) are - represented: as a chain of actions linked via ``next_element``. The next - element may be any tree element, so a decision is also allowed to follow an - action. - """ + """An action blueprint whose successor is initialized on completion.""" def __init__( self, @@ -170,20 +124,11 @@ def __init__( parameters: Parameters | None = None, unset_parameters: ParameterReferences | None = None, ) -> None: - """ - Create a new ActionTreeElement - :param name: the class name of the corresponding AbstractActionElement - :param parent: the parent element - :param parameters: A dictionary of parameters - :param unset_parameters: A dictionary of parameters that must be set later - """ super().__init__(name, parent, parameters, unset_parameters) - # The element that is activated when this action pops itself. None if this - # action is the last (or only) element in its chain. self.next_element: AbstractTreeElement[BlackboardT] | None = None def set_activation_reason(self, reason: str | None) -> None: - """Set the activation reason for this action and propagate it along the chain""" + """Record the selecting branch, propagating it along an action chain.""" self.activation_reason = reason # All elements of a chain share the activation reason of the head so that a # reevaluated decision below the chain does not consider its result changed @@ -200,12 +145,8 @@ def __str__(self) -> str: return self.name def repr_dict(self) -> TreeData: - """ - Represent this action as dictionary which is JSON encodable - """ - # Get the dict from the superclass + """Serialize this blueprint for the debug protocol.""" result = super().repr_dict() - # Add additional information result["type"] = "action" result["next"] = self.next_element.repr_dict() if self.next_element is not None else None return result diff --git a/dynamic_stack_decider/test/runtime_elements.py b/dynamic_stack_decider/test/runtime_elements.py index 6b4c615..7b2a190 100644 --- a/dynamic_stack_decider/test/runtime_elements.py +++ b/dynamic_stack_decider/test/runtime_elements.py @@ -25,9 +25,6 @@ def __init__(self): self.results: dict[str, str] = {} -# --------------------------------------------------------------------------- -# Actions -# --------------------------------------------------------------------------- class _LoggingAction(AbstractActionElement): """Base action that logs its perform and pop calls but otherwise stays on the stack.""" @@ -129,9 +126,6 @@ def perform(self, reevaluate=False): return self.pop() -# --------------------------------------------------------------------------- -# Decisions -# --------------------------------------------------------------------------- class _LoggingDecision(AbstractDecisionElement): """Base decision that logs its perform calls and returns ``blackboard.results[self.name]``.""" diff --git a/dynamic_stack_decider/test/test_runtime.py b/dynamic_stack_decider/test/test_runtime.py index 2b87fdd..3c478a9 100644 --- a/dynamic_stack_decider/test/test_runtime.py +++ b/dynamic_stack_decider/test/test_runtime.py @@ -2,7 +2,7 @@ Runtime behavior tests for the DSD execution engine (``dsd.py``). These tests drive a DSD through ``update()`` and assert on the resulting stack and execution order. -They aim to cover every runtime code path: pushing/popping, sequences (chains), reevaluation +They cover: pushing/popping, sequences (chains), reevaluation (including ELSE handling), the ``never_reevaluate`` / ``do_not_reevaluate`` flags, interrupts, decisions and subtrees following actions, error handling and the debug publishing. """ @@ -42,9 +42,6 @@ def create_publisher(self, msg_type, topic, qos): return publisher -# --------------------------------------------------------------------------- -# Basic decision / action flow -# --------------------------------------------------------------------------- def test_start_element_on_stack_after_load(): dsd = make_dsd( """ @@ -129,9 +126,6 @@ def test_nested_decisions(): assert stack_names(dsd) == ["Chooser", "Guard", "Idle"] -# --------------------------------------------------------------------------- -# Sequences (chains via next_element) -# --------------------------------------------------------------------------- def test_sequence_advances_one_action_per_tick(): dsd = make_dsd( """ @@ -223,9 +217,6 @@ def test_subtree_after_action(): assert stack_names(dsd) == ["Chooser", "Guard", "Idle"] -# --------------------------------------------------------------------------- -# Reevaluation -# --------------------------------------------------------------------------- def test_reevaluation_rebuilds_stack_on_change(): dsd = make_dsd( """ @@ -305,9 +296,6 @@ def test_reevaluation_else_branch(): assert stack_names(dsd) == ["ElseGuard", "Idle"] -# --------------------------------------------------------------------------- -# never_reevaluate / do_not_reevaluate -# --------------------------------------------------------------------------- def test_never_reevaluate_parameter_suppresses_reevaluation(): dsd = make_dsd( """ @@ -385,9 +373,6 @@ def test_do_not_reevaluate_preserved_across_sequence_advance(): assert dsd.do_not_reevaluate is True -# --------------------------------------------------------------------------- -# Interrupts -# --------------------------------------------------------------------------- def test_external_interrupt_resets_stack(): dsd = make_dsd( """ @@ -441,9 +426,6 @@ def test_interrupt_during_reevaluation(): assert stack_names(dsd) == ["InterruptOnReeval"] -# --------------------------------------------------------------------------- -# pop edge cases -# --------------------------------------------------------------------------- def test_pop_of_only_element_is_noop(): # A behavior whose root is a self-popping action dsd = make_dsd( @@ -504,9 +486,6 @@ def test_reevaluation_pop_in_the_middle_shortens_stack(): assert ("pop", "Idle") in dsd.blackboard.events -# --------------------------------------------------------------------------- -# Error handling -# --------------------------------------------------------------------------- def test_exception_in_perform_is_swallowed(): dsd = make_dsd( """ @@ -552,9 +531,6 @@ def test_decision_returning_none_is_reported(): assert stack_names(dsd) == ["ReturnNone"] -# --------------------------------------------------------------------------- -# _bind_modules error paths -# --------------------------------------------------------------------------- def test_unknown_action_name_raises_on_load(): with pytest.raises(ValueError): make_dsd( @@ -593,9 +569,6 @@ class Bogus: dsd._bind_modules(Bogus()) -# --------------------------------------------------------------------------- -# Misc DSD API -# --------------------------------------------------------------------------- def test_get_stack_returns_the_stack(): dsd = make_dsd( """ @@ -639,9 +612,6 @@ def test_stack_holds_bound_instances(): assert root_instance.blackboard is dsd.blackboard is top_instance.blackboard -# --------------------------------------------------------------------------- -# Debug publishing -# --------------------------------------------------------------------------- def test_debug_tree_published_on_load(): node = FakeNode() make_dsd( diff --git a/dynamic_stack_decider/test/test_tree.py b/dynamic_stack_decider/test/test_tree.py index eb07500..59222fa 100644 --- a/dynamic_stack_decider/test/test_tree.py +++ b/dynamic_stack_decider/test/test_tree.py @@ -1,16 +1,13 @@ """ Unit tests for the tree element classes (``tree.py``): child lookup (including ELSE and error cases), activation-reason propagation along a chain, and the JSON representation (``repr_dict``) including the -stable ``id`` and the ``next`` chain link. +instance ``id`` and the ``next`` chain link. """ import pytest from dynamic_stack_decider.tree import ActionTreeElement, DecisionTreeElement, Tree -# --------------------------------------------------------------------------- -# get_child -# --------------------------------------------------------------------------- def test_get_child_returns_matching_child(): d = DecisionTreeElement("D", None) a = ActionTreeElement("A", d) @@ -44,9 +41,6 @@ def test_action_get_child_is_always_none(): assert a.get_child("anything") is None -# --------------------------------------------------------------------------- -# Activation reason propagation -# --------------------------------------------------------------------------- def test_set_activation_reason_propagates_along_chain(): a = ActionTreeElement("A", None) b = ActionTreeElement("B", a) @@ -87,9 +81,6 @@ def test_add_child_element_propagates_reason_to_chain(): assert tail.activation_reason == "R" -# --------------------------------------------------------------------------- -# repr_dict -# --------------------------------------------------------------------------- def test_action_repr_dict_contains_id_type_and_next(): a = ActionTreeElement("A", None, parameters={"k": "v"}) b = ActionTreeElement("B", a) @@ -127,9 +118,6 @@ def test_repr_dict_id_is_stable_and_unique(): assert a1.repr_dict()["id"] != a2.repr_dict()["id"] -# --------------------------------------------------------------------------- -# Tree and string representations -# --------------------------------------------------------------------------- def test_tree_repr_dict_delegates_to_root(): d = DecisionTreeElement("D", None) d.add_child_element(ActionTreeElement("A", d), "R") diff --git a/dynamic_stack_decider_visualization/LICENSE b/dynamic_stack_decider_visualization/LICENSE new file mode 100644 index 0000000..9a25cc9 --- /dev/null +++ b/dynamic_stack_decider_visualization/LICENSE @@ -0,0 +1,19 @@ +Copyright (c) Hamburg Bit-Bots + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/dynamic_stack_decider_visualization/MANIFEST.in b/dynamic_stack_decider_visualization/MANIFEST.in index bb3ec5f..fc3854f 100644 --- a/dynamic_stack_decider_visualization/MANIFEST.in +++ b/dynamic_stack_decider_visualization/MANIFEST.in @@ -1 +1,2 @@ -include README.md +include README.md LICENSE +recursive-include test *.py *.dsd diff --git a/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/dsd_follower.py b/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/dsd_follower.py index 7d4d03b..a69f828 100644 --- a/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/dsd_follower.py +++ b/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/dsd_follower.py @@ -1,366 +1,270 @@ +"""Receive DSD debug snapshots and render them for RQT.""" + from __future__ import annotations import json -import uuid -from typing import cast +import threading +from typing import Literal, cast import pydot -from dynamic_stack_decider.types import Parameters, StackData, TreeData +from dynamic_stack_decider.types import StackData, TreeData from PyQt5.QtGui import QStandardItem, QStandardItemModel from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile +from rclpy.subscription import Subscription from std_msgs.msg import String -class ParseError(Exception): - pass +class ParseError(ValueError): + """A debug message does not match the DSD snapshot schema.""" + + +def _mapping(value: object, label: str) -> dict[str, object]: + if not isinstance(value, dict) or any(not isinstance(key, str) for key in value): + raise ParseError(f"{label} must be an object with string keys") + return cast(dict[str, object], value) + + +def _identity(data: dict[str, object]) -> tuple[int, str, Literal["action", "decision"]]: + identifier, name, kind = data.get("id"), data.get("name"), data.get("type") + if not isinstance(identifier, int) or isinstance(identifier, bool) or not isinstance(name, str): + raise ParseError("Element id must be an integer and name must be a string") + if kind == "action": + return identifier, name, "action" + if kind == "decision": + return identifier, name, "decision" + raise ParseError(f"Unknown element type {kind!r}") + + +def _parse_tree(value: object) -> TreeData: + data = _mapping(value, "Tree element") + identifier, name, kind = _identity(data) + tree: TreeData = { + "id": identifier, + "name": name, + "type": kind, + "parameters": _mapping(data.get("parameters"), "Parameters"), + } + if kind == "decision": + tree["children"] = { + key: _parse_tree(child) for key, child in _mapping(data.get("children"), "Children").items() + } + else: + successor = data.get("next") + tree["next"] = _parse_tree(successor) if successor is not None else None + return tree + + +def _parse_stack(value: object) -> StackData | None: + if value is None: + return None + data = _mapping(value, "Stack element") + identifier, name, kind = _identity(data) + reason = data.get("activation_reason") + if reason is not None and not isinstance(reason, str): + raise ParseError("Activation reason must be a string or null") + if "next" not in data: + raise ParseError("Stack element is missing its next field") + return { + "id": identifier, + "name": name, + "type": kind, + "activation_reason": reason, + "debug_data": _mapping(data.get("debug_data"), "Debug data"), + "next": _parse_stack(data["next"]), + } class DsdFollower: def __init__(self, node: Node, debug_topic: str) -> None: - """ - Creates a new DSD follower which subscribes to the given debug topics, - fetches it's tree and listens for the current stack. - It provides the rendered tree and the current stack visualization based on the received data. - - :param node: Reference to the used ROS node\ - :param debug_topic: The topic namespace on which the DSD publishes its debug data - """ self._node = node - self.dsd_debug_topic = debug_topic - self.tree: TreeData | None = None self.stack: StackData | None = None - + self._tree_by_id: dict[int, TreeData] = {} + self._lock = threading.RLock() self._cached_dotgraph: pydot.Dot | None = None - - # Subscribe to the DSDs tree (latched) - dsd_tree_topic = f"{debug_topic}/dsd_tree" - self.tree_sub = self._node.create_subscription( - String, - dsd_tree_topic, - self._tree_callback, - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL), + self._cached_full_tree: bool | None = None + self._cached_item_model: QStandardItemModel | None = None + latched = QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self.tree_sub: Subscription[String] | None = node.create_subscription( + String, f"{debug_topic}/dsd_tree", self._tree_callback, latched + ) + self.stack_sub: Subscription[String] | None = node.create_subscription( + String, f"{debug_topic}/dsd_stack", self._stack_callback, 10 ) - self._node.get_logger().info(f"Subscribed to {dsd_tree_topic}") - - # Subscribe to the DSDs stack - dsd_stack_topic = f"{debug_topic}/dsd_stack" - self.stack_sub = self._node.create_subscription(String, dsd_stack_topic, self._stack_callback, 10) - self._node.get_logger().info(f"Subscribed to {dsd_stack_topic}") def _tree_callback(self, msg: String) -> None: - self._node.get_logger().info("Received tree") - # Deserialize the tree message - # This is a json string because the tree is a dynamically nested structure of variable length - self.tree = cast(TreeData, json.loads(msg.data)) + try: + tree = _parse_tree(json.loads(msg.data)) + index: dict[int, TreeData] = {} + pending = [tree] + while pending: + element = pending.pop() + if element["id"] in index: + raise ParseError("Tree element ids must be unique") + index[element["id"]] = element + pending.extend(element.get("children", {}).values()) + successor = element.get("next") + if successor is not None: + pending.append(successor) + except (ValueError, RecursionError) as error: + self._node.get_logger().error(f"Invalid DSD tree: {error}") + return + with self._lock: + if tree != self.tree: + self.tree = tree + self._tree_by_id = index + self.stack = None + self._cached_item_model = None + self.reset_cache() def _stack_callback(self, msg: String) -> None: - # Abort if no tree was received yet - if self.tree is None: + try: + stack = _parse_stack(json.loads(msg.data)) + except (ValueError, RecursionError) as error: + self._node.get_logger().error(f"Invalid DSD stack: {error}") return - - # Deserialize the stack message - stack = cast(StackData | None, json.loads(msg.data)) - - # Check if the stack changed (ignore debug_data log messages) - if DsdFollower.stack_dict_changed(self.stack, stack, ignore_keys=["debug_data"]): - # Reset cache / trigger redraw - self.reset_cache() - - # Update stack - self.stack = stack + with self._lock: + if self.tree is None: + return + frame = stack + while frame is not None: + tree_element = self._tree_by_id.get(frame["id"]) + if tree_element is None: + self._node.get_logger().debug("Ignoring stack from a different behavior tree") + return + if frame["name"] != tree_element["name"] or frame["type"] != tree_element["type"]: + self._node.get_logger().error("DSD stack identity does not match its tree") + return + frame = frame["next"] + if self.stack_dict_changed(self.stack, stack, ignore_keys=["debug_data"]): + self.reset_cache() + if stack != self.stack: + self._cached_item_model = None + self.stack = stack def reset_cache(self) -> None: - """Reset the cached dotgraph""" - self._cached_dotgraph = None + with self._lock: + self._cached_dotgraph = None + self._cached_full_tree = None @staticmethod def stack_dict_changed(old_stack_element: object, new_stack_element: object, ignore_keys: list[str]) -> bool: - """ - Check if two stack dicts are different - """ if type(old_stack_element) is not type(new_stack_element): return True - elif isinstance(old_stack_element, dict) and isinstance(new_stack_element, dict): - # Check if the length of the stack changed - if len(old_stack_element) != len(new_stack_element): - return True - - # Check if the content of the stack changed - for key in old_stack_element.keys(): - if key in ignore_keys: - continue - if key not in new_stack_element.keys(): - return True - if DsdFollower.stack_dict_changed(old_stack_element[key], new_stack_element[key], ignore_keys): - return True - elif isinstance(old_stack_element, list) and isinstance(new_stack_element, list): - # Check if the length of the stack changed - if len(old_stack_element) != len(new_stack_element): - return True - # Check if the content of the stack changed - for i in range(len(old_stack_element)): - if DsdFollower.stack_dict_changed(old_stack_element[i], new_stack_element[i], ignore_keys): - return True - elif isinstance(old_stack_element, (int, float, str, bool)) or old_stack_element is None: - if old_stack_element != new_stack_element: - return True - else: - raise ParseError(f"Unknown type {type(old_stack_element)}") - # The stack did not change - return False + if isinstance(old_stack_element, dict) and isinstance(new_stack_element, dict): + old_keys = old_stack_element.keys() - set(ignore_keys) + new_keys = new_stack_element.keys() - set(ignore_keys) + return old_keys != new_keys or any( + DsdFollower.stack_dict_changed(old_stack_element[key], new_stack_element[key], ignore_keys) + for key in old_keys + ) + if isinstance(old_stack_element, list) and isinstance(new_stack_element, list): + return len(old_stack_element) != len(new_stack_element) or any( + DsdFollower.stack_dict_changed(old, new, ignore_keys) + for old, new in zip(old_stack_element, new_stack_element, strict=True) + ) + if isinstance(old_stack_element, (int, float, str, bool)) or old_stack_element is None: + return old_stack_element != new_stack_element + raise ParseError(f"Unknown debug value type {type(old_stack_element).__name__}") @staticmethod def _error_dotgraph() -> pydot.Dot: dot = pydot.Dot(graph_type="digraph") - - uid1 = str(uuid.uuid4()) - dot.add_node(pydot.Node(uid1, label="I have not received anything from the dsd yet")) - - uid2 = str(uuid.uuid4()) dot.add_node( pydot.Node( - uid2, - label="Please make sure that\n" - "- The appropriate dsd is started\n" - "- You are connected to the same roscore\n" - "- param /debug_active is True", + "waiting", + label="Waiting for a DSD tree and stack.\nCheck the selected topic and ROS 2 discovery configuration.", ) ) - - dot.add_edge(pydot.Edge(uid1, uid2)) - return dot - @staticmethod - def _empty_item_model() -> QStandardItemModel: - return QStandardItemModel() - @staticmethod def _dot_node_from_tree_element(tree_element: TreeData, stack_element: StackData | None = None) -> pydot.Node: - """ - :param tree_element: The node from the dsd tree which should be converted to a dot node - :param stack_element: The corresponding element from the stack - :return: The corresponding dot node - """ - - def param_string(params: Parameters) -> str: - """ - :param params: A dict of parameters - :return: A string representation of the parameters - """ - # Return empty string if no parameters are given - if not params: - return "" - - output = [] - for param_name, param_value in params.items(): - output.append(f"{param_name}: {str(param_value)}") - return " (" + ", ".join(output) + ")" - - # Sanity check - if stack_element is not None: - assert stack_element["type"] == tree_element["type"], ( - f"The stack and the tree do not match (element type mismatch: {stack_element['type']} vs. {tree_element['type']})" - ) - assert stack_element["name"] == tree_element["name"], ( - f"The stack and the tree do not match (element name mismatch: {stack_element['name']} vs. {tree_element['name']})" - ) - - # Initialize parameters of the dot node we are going to create - dot_node_params = { - "name": str(uuid.uuid4()), - } - - # Go through all possible element types and create the corresponding label and shape - if tree_element["type"] == "decision": - dot_node_params["shape"] = "ellipse" - dot_node_params["label"] = tree_element["name"] + param_string(tree_element["parameters"]) - elif tree_element["type"] == "action": - dot_node_params["shape"] = "box" - dot_node_params["label"] = tree_element["name"] + param_string(tree_element["parameters"]) - else: - raise ParseError(f"Unknown element type {tree_element['type']}") - - # Set color if this element is on the stack - dot_node_params["color"] = "gray" if stack_element is None else "orangered" - - # Create node in graph + parameters = tree_element["parameters"] + suffix = " (" + ", ".join(f"{key}: {value}" for key, value in parameters.items()) + ")" if parameters else "" return pydot.Node( - name=dot_node_params["name"], - shape=dot_node_params["shape"], - label=dot_node_params["label"], - color=dot_node_params["color"], + str(tree_element["id"]), + shape="ellipse" if tree_element["type"] == "decision" else "box", + label=tree_element["name"] + suffix, + color="orangered" if stack_element is not None else "gray", ) @staticmethod def _stack_matches_tree(stack_element: StackData | None, tree_element: TreeData) -> bool: - """ - Check whether a stack element corresponds to a given tree element. - - Matching is done via the stable ``id`` of the tree element rather than the name, because a - chain (i.e. a sequence) may contain multiple elements with the same name (e.g. - ``@Wait, @SomeAction, @Wait``) and we need to know exactly which one is active. - """ return stack_element is not None and stack_element["id"] == tree_element["id"] def _stack_to_dotgraph( self, dot: pydot.Dot, subtree_root: TreeData, stack_root: StackData | None = None, full_tree: bool = False ) -> tuple[pydot.Dot, str]: - """ - Recursively modify dot to include every element of the stack - - :param dot: The dotgraph to modify - :param subtree_root: The root node of the subtree which should be added to the graph - :param stack_root: The stack element which is currently active in this position of the tree, or None if - this branch of the tree is not on the stack. For a chain of actions (i.e. a sequence) linked via - ``next`` this is the currently active element of the chain, which is not necessarily ``subtree_root``. - :param full_tree: If true the whole tree is rendered, otherwise only the current stack and its direct neighbors are rendered - :return: The modified dotgraph and the uid of the root node - """ - # The passed stack element only corresponds to this tree node if their type and name match. For a chain of - # actions (a sequence) the active element might be a later element of the chain, so it does not match here. node_stack = stack_root if self._stack_matches_tree(stack_root, subtree_root) else None - - # Generate dot node for the root and mark it as active if it is on the stack - dot_node = DsdFollower._dot_node_from_tree_element(subtree_root, node_stack) - # Append this element to graph - dot.add_node(dot_node) - - # Append children to graph if this element is on the stack or if we want to render the whole tree - if "children" in subtree_root and (node_stack is not None or full_tree): - # Go through all children - for activating_result, child in subtree_root["children"].items(): - # Get the root of the sub stack if this branch of the tree is the one which is currently on the stack - sub_stack_root = None - if ( - node_stack is not None - and node_stack["next"] is not None - and node_stack["next"]["activation_reason"] == activating_result - ): - sub_stack_root = node_stack["next"] - - # Recursively generate dot nodes for the children - dot, child_uid = self._stack_to_dotgraph(dot, child, sub_stack_root, full_tree) - # Connect the child to the parent element - edge = pydot.Edge(src=dot_node.get_name(), dst=child_uid, label=activating_result) - dot.add_edge(edge) - - # Append the successor of a chain (e.g. the next action of a sequence) if it exists. - # We render the whole chain if any element of it is on the stack or if the whole tree is requested. + node = self._dot_node_from_tree_element(subtree_root, node_stack) + dot.add_node(node) + if node_stack is not None or full_tree: + for outcome, child in subtree_root.get("children", {}).items(): + child_stack = None + if node_stack is not None and node_stack["next"] is not None: + if node_stack["next"]["activation_reason"] == outcome: + child_stack = node_stack["next"] + dot, child_id = self._stack_to_dotgraph(dot, child, child_stack, full_tree) + dot.add_edge(pydot.Edge(node.get_name(), child_id, label=outcome)) successor = subtree_root.get("next") if successor is not None and (stack_root is not None or full_tree): - # We pass the same active stack element down the whole chain. Only the element it actually - # corresponds to (matched via its id) is rendered as active; all others are rendered inactive. - dot, next_uid = self._stack_to_dotgraph(dot, successor, stack_root, full_tree) - # Connect the successor to this element - dot.add_edge(pydot.Edge(src=dot_node.get_name(), dst=next_uid)) - return dot, dot_node.get_name() - - def _append_debug_data_to_item(self, parent_item: QStandardItem, debug_data: object) -> None: - """ - Appends debug_data of a given element and its children to a QStandardItem. - """ - if isinstance(debug_data, list): - for i, data in enumerate(debug_data): - child_item = QStandardItem() - child_item.setText(str(i) + ": ") - child_item.setEditable(False) - child_item.setSelectable(False) - self._append_debug_data_to_item(child_item, data) - parent_item.appendRow(child_item) - elif isinstance(debug_data, dict): - for label, data in debug_data.items(): - child_item = QStandardItem() - child_item.setText(str(label) + ": ") - child_item.setEditable(False) - child_item.setSelectable(False) - self._append_debug_data_to_item(child_item, data) - parent_item.appendRow(child_item) - elif isinstance(debug_data, (bool, float, int, str, bytes)): - parent_item.setText(parent_item.text() + str(debug_data)) + dot, next_id = self._stack_to_dotgraph(dot, successor, stack_root, full_tree) + dot.add_edge(pydot.Edge(node.get_name(), next_id)) + return dot, node.get_name() def to_dotgraph(self, full_tree: bool) -> pydot.Dot: - """ - Represent the current stack as dotgraph - - :param full_tree: If true the whole tree is rendered, otherwise only the current stack is rendered - """ - # Return cached result if available - if self._cached_dotgraph is not None: - return self._cached_dotgraph - - self._node.get_logger().debug("Generating dotgraph") + with self._lock: + if self._cached_dotgraph is not None and self._cached_full_tree == full_tree: + return self._cached_dotgraph + if self.tree is None or self.stack is None: + dot = self._error_dotgraph() + else: + dot, _ = self._stack_to_dotgraph(pydot.Dot(graph_type="digraph"), self.tree, self.stack, full_tree) + self._cached_dotgraph = dot + self._cached_full_tree = full_tree + return dot - # Check if we received any data yet - if self.stack is None or self.tree is None: - self._node.get_logger().info("No data received yet") - return self._error_dotgraph() - - # Create dot graph - self._cached_dotgraph, _ = self._stack_to_dotgraph( - pydot.Dot(graph_type="digraph"), self.tree, self.stack, full_tree - ) - - return self._cached_dotgraph + def _append_debug_data_to_item(self, parent_item: QStandardItem, debug_data: object) -> None: + if isinstance(debug_data, (dict, list)): + entries = debug_data.items() if isinstance(debug_data, dict) else enumerate(debug_data) + for label, data in entries: + child = QStandardItem(f"{label}: ") + child.setEditable(False) + child.setSelectable(False) + self._append_debug_data_to_item(child, data) + parent_item.appendRow(child) + else: + parent_item.setText(parent_item.text() + str(debug_data)) def to_q_item_model(self) -> QStandardItemModel: - """ - Represent the DSDs debug data as QITemModel - """ - # Return if we received no data yet - if self.stack is None or self.tree is None: - return self._empty_item_model() - - # Construct a new item-model - model = QStandardItemModel() - - # Start with the root/bottom of the stack - stack_element: StackData | None = self.stack - - # Go through all stack elements - while stack_element is not None: - # Sanity check - assert "next" in stack_element, "Stack element has no next element" - # Check if this is the last element - last_element: bool = stack_element["next"] is None - - # Create a new item for this element - elem_item = QStandardItem() - elem_item.setEditable(False) - elem_item.setSelectable(False) - - # Set the text of the item - elem_item.setText(stack_element["name"]) - - # Add debug data to the item - self._append_debug_data_to_item(elem_item, stack_element["debug_data"]) - - model.invisibleRootItem().appendRow(elem_item) - - # Add a spacer - if not last_element: - # Create a new horizontal line covering the full with as a spacer of the stack elements - spacer = QStandardItem() - spacer.setEditable(False) - # Disable the selection of the spacer - spacer.setSelectable(False) - spacer.setText("──────────────────────────────") - - model.invisibleRootItem().appendRow(spacer) - - # Go to next element - stack_element = stack_element["next"] - - return model + with self._lock: + if self._cached_item_model is not None: + return self._cached_item_model + model = QStandardItemModel() + frame = self.stack + while frame is not None: + item = QStandardItem(frame["name"]) + item.setEditable(False) + item.setSelectable(False) + self._append_debug_data_to_item(item, frame["debug_data"]) + model.appendRow(item) + frame = frame["next"] + if frame is not None: + spacer = QStandardItem("──────────────────────────────") + spacer.setEditable(False) + spacer.setSelectable(False) + model.appendRow(spacer) + self._cached_item_model = model + return model def destroy(self) -> None: - """ - Cleanup the subscriptions, so we don't receive any more data that we don't need. - This improves the performance. - Also this allows the garbage collector to delete this object and prevent memory leaks. - """ - self._node.destroy_subscription(self.tree_sub) - self._node.destroy_subscription(self.stack_sub) + """Release both ROS subscriptions. Repeated calls are harmless.""" + with self._lock: + subscriptions = self.tree_sub, self.stack_sub + self.tree_sub = self.stack_sub = None + for subscription in subscriptions: + if subscription is not None: + self._node.destroy_subscription(subscription) diff --git a/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/dsd_visualization_plugin.py b/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/dsd_visualization_plugin.py index 79acda8..cdc5902 100644 --- a/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/dsd_visualization_plugin.py +++ b/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/dsd_visualization_plugin.py @@ -30,9 +30,8 @@ from __future__ import annotations -import os import sys -import uuid +from pathlib import Path from typing import Protocol import pydot @@ -80,111 +79,78 @@ class DsdWidget(QWidget): class DsdVizPlugin(Plugin): + """Inspect a DSD discovered through its ROS 2 debug topics.""" + def __init__(self, context: PluginContext) -> None: super().__init__(context) - self._node: Node = context.node - self._initialized = False # This gets set to true once the plugin hast completely finished loading - - # Ensure startup state - self.freeze = False # Controls whether the state should be updated from remote + self._node = context.node + self._initialized = False + self.freeze = False self.dsd_follower: DsdFollower | None = None self.running_dsd_instances: dict[str, str] = {} - self._init_plugin(context) - - # Performance optimization variables self._prev_dotgraph: pydot.Dot | None = None - self._prev_QItemModel: QStandardItemModel | None = None + self._prev_item_model: QStandardItemModel | None = None + self._messages: tuple[str, ...] | None = None + self._timer_id: int | None = None + self._init_plugin(context) def _init_plugin(self, context: PluginContext) -> None: self.setObjectName("DSD-Visualization") - self._widget = DsdWidget() self._widget.setObjectName(self.objectName()) - - # load qt ui definition from file - ui_file = os.path.join( - get_package_share_directory("dynamic_stack_decider_visualization"), "resource", "StackmachineViz.ui" + ui_file = ( + Path(get_package_share_directory("dynamic_stack_decider_visualization")) / "resource/StackmachineViz.ui" ) - loadUi(ui_file, self._widget, {"InteractiveGraphicsView": InteractiveGraphicsView}) - - # initialize qt scene - self._scene = QGraphicsScene() + loadUi(str(ui_file), self._widget) + self._scene = QGraphicsScene(self._widget) self._scene.setBackgroundBrush(Qt.white) self._widget.graphics_view.setScene(self._scene) - - # Bind fit-in-view button self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme("view-fullscreen")) self._widget.fit_in_view_push_button.pressed.connect(self.fit_in_view) + self._widget.auto_fit_graph_check_box.toggled.connect(lambda enabled: self.fit_in_view() if enabled else None) - # Fit-in-view on checkbox toggle - self._widget.auto_fit_graph_check_box.toggled.connect(self.fit_in_view) - - # Freezing def toggle_freeze() -> None: - self.freeze = not self.freeze + self.freeze = self._widget.freeze_push_button.isChecked() self.refresh() - self._widget.freeze_push_button.toggled.connect(toggle_freeze) + def reset_dot_cache() -> None: + if self.dsd_follower is not None: + self.dsd_follower.reset_cache() - # Exporting and importing + self._widget.freeze_push_button.toggled.connect(toggle_freeze) self._widget.save_as_svg_push_button.pressed.connect(self.save_svg_to_file) - - # Discover dsd instances - self.discover_dsd_instances() - - # Connect the combo box to the set_dsd method self._widget.dsd_selector_combo_box.currentTextChanged.connect(self.set_dsd) - - # Bind refresh button and add icon self._widget.refresh_combobox_push_button.setIcon(QIcon.fromTheme("view-refresh")) self._widget.refresh_combobox_push_button.pressed.connect(self.discover_dsd_instances) - - # Hook that resets the dot cache and thus forces a redraw of the dotgraph - def reset_dot_cache() -> None: - if self.dsd_follower is not None: - self.dsd_follower.reset_cache() - - # Add hook that triggers redraw to ui elements self._widget.highlight_connections_check_box.toggled.connect(reset_dot_cache) self._widget.show_full_tree.toggled.connect(reset_dot_cache) - self._widget.auto_fit_graph_check_box.toggled.connect(reset_dot_cache) - self._widget.fit_in_view_push_button.pressed.connect(reset_dot_cache) - self._widget.freeze_push_button.toggled.connect(reset_dot_cache) - self._widget.graphics_view.zoomed.connect(lambda: self._widget.auto_fit_graph_check_box.setChecked(False)) - - # Add widget to the user interface + self.discover_dsd_instances() context.add_widget(self._widget) - - # Start a timer that calls back every 100 ms self._timer_id = self.startTimer(100) def discover_dsd_instances(self) -> None: - """Updates the dict of known dsd instances""" - # Store all DSDs in a dict - dsd_instances = {} - - # List all known topics - topics = self._node.get_topic_names_and_types() - for topic_name, topic_types in topics: - if topic_name.endswith("/dsd_tree") and "std_msgs/msg/String" in topic_types: - # Extract the dsd name from the topic name - dsd_name = "/".join(topic_name.split("/")[:-1]).upper() - # Store the dsd name and the debug topic namespace (not just the tree topic) - dsd_instances[dsd_name] = topic_name.replace("/dsd_tree", "") - - # Update the list of known dsd instances - self.running_dsd_instances = dsd_instances - - # Update the list of dsd instances in the combo box - self._widget.dsd_selector_combo_box.clear() - self._widget.dsd_selector_combo_box.addItem("Select DSD...") - for name in self.running_dsd_instances.keys(): - self._widget.dsd_selector_combo_box.addItem(name) + instances: dict[str, str] = {} + for topic, types in self._node.get_topic_names_and_types(): + if topic.endswith("/dsd_tree") and "std_msgs/msg/String" in types: + namespace = topic.removesuffix("/dsd_tree") + instances[namespace] = namespace + self.running_dsd_instances = instances + selector = self._widget.dsd_selector_combo_box + previous = selector.currentText() + selector.blockSignals(True) + try: + selector.clear() + selector.addItem("Select DSD...") + selector.addItems(sorted(instances)) + if previous in instances: + selector.setCurrentText(previous) + finally: + selector.blockSignals(False) + self.set_dsd(selector.currentText()) def save_settings(self, plugin_settings: object, instance_settings: Settings) -> None: super().save_settings(plugin_settings, instance_settings) - instance_settings.set_value("auto_fit_graph_check_box_state", self._widget.auto_fit_graph_check_box.isChecked()) instance_settings.set_value( "highlight_connections_check_box_state", self._widget.highlight_connections_check_box.isChecked() @@ -193,17 +159,15 @@ def save_settings(self, plugin_settings: object, instance_settings: Settings) -> def restore_settings(self, plugin_settings: object, instance_settings: Settings) -> None: super().restore_settings(plugin_settings, instance_settings) - self._widget.auto_fit_graph_check_box.setChecked( - instance_settings.value("auto_fit_graph_check_box_state", True) in [True, "true"] + instance_settings.value("auto_fit_graph_check_box_state", True) in (True, "true") ) self._widget.highlight_connections_check_box.setChecked( - instance_settings.value("highlight_connections_check_box_state", True) in [True, "true"] + instance_settings.value("highlight_connections_check_box_state", True) in (True, "true") ) self._widget.show_full_tree.setChecked( - instance_settings.value("show_full_tree_check_box_state", False) in [True, "true"] + instance_settings.value("show_full_tree_check_box_state", False) in (True, "true") ) - self._initialized = True self.refresh() @@ -211,129 +175,92 @@ def save_svg_to_file(self) -> None: file_name, _ = QFileDialog.getSaveFileName( self._widget, self.tr("Save as SVG"), "stackmachine.svg", self.tr("Scalable Vector Graphic (*.svg)") ) - - if file_name is not None and file_name != "": - generator = QSvgGenerator() - generator.setFileName(file_name) - generator.setSize((self._scene.sceneRect().size() * 2.0).toSize()) - - painter = QPainter(generator) + if not file_name: + return + generator = QSvgGenerator() + generator.setFileName(file_name) + generator.setSize((self._scene.sceneRect().size() * 2.0).toSize()) + painter = QPainter(generator) + try: painter.setRenderHint(QPainter.Antialiasing) self._scene.render(painter) + finally: painter.end() - # fmt: off - def timerEvent(self, timer_event: QTimerEvent | None) -> None: #noqa: N802 - # fmt: on - """This gets called by QT whenever the timer ticks""" - # Refresh the viz if the freeze button is not pressed + def timerEvent(self, timer_event: QTimerEvent | None) -> None: if not self.freeze: self.refresh() - - # Automatically fit in view if the checkbox is checked if self._widget.auto_fit_graph_check_box.isChecked(): self.fit_in_view() def fit_in_view(self) -> None: - """Rescales the tree to fit the window""" self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) def refresh(self) -> None: - """Refresh the complete drawn representation""" - - # Show message if the plugin is not yet completely initialized if not self._initialized: - self._render_messages("The plugin is not yet completely initialized. Please wait...") - - # Show message if no dsd is selected + self._render_messages("The plugin is initializing.") elif self.dsd_follower is None: self._render_messages("No DSD selected") - else: - # Render the dotgraph and the debug data - self._render_dotgraph(self.dsd_follower.to_dotgraph( - full_tree=self._widget.show_full_tree.isChecked(), - )) + self._render_dotgraph(self.dsd_follower.to_dotgraph(self._widget.show_full_tree.isChecked())) self._render_debug_data(self.dsd_follower.to_q_item_model()) def _render_messages(self, *messages: str) -> None: - """Render simple messages on the canvas""" - - msg_dot = pydot.Dot() - - for msg in messages: - msg_dot.add_node(pydot.Node(str(uuid.uuid4()), label=str(msg))) - - self._render_dotgraph(msg_dot) - self._render_debug_data(QStandardItemModel(self._scene)) + if messages == self._messages: + return + dot = pydot.Dot(graph_type="digraph") + for index, message in enumerate(messages): + dot.add_node(pydot.Node(f"message_{index}", label=message)) + self._render_dotgraph(dot) + self._render_debug_data(QStandardItemModel(self._widget)) + self._messages = messages def _render_dotgraph(self, dotgraph: pydot.Dot) -> None: - """Render the specified dotgraph on canvas""" - - # Only redraw when the graph differs from the previous one - if self._prev_dotgraph == dotgraph: + if self._prev_dotgraph is dotgraph: return - else: - self._prev_dotgraph = dotgraph - + self._prev_dotgraph = dotgraph + self._messages = None self._scene.clear() - - if self._widget.highlight_connections_check_box.isChecked(): - highlight_level = 3 - else: - highlight_level = 1 - - # Generate qt items from dotcode - dotcode = PydotFactory().create_dot(dotgraph) - nodes, edges = DotToQtGenerator().dotcode_to_qt_items(dotcode, highlight_level, same_label_siblings=False) - - # Add generated items to scene - for node_item in nodes.values(): - self._scene.addItem(node_item) - for edge_items in edges.values(): - for edge_item in edge_items: - edge_item.add_to_scene(self._scene) - + highlight = 3 if self._widget.highlight_connections_check_box.isChecked() else 1 + nodes, edges = DotToQtGenerator().dotcode_to_qt_items( + PydotFactory().create_dot(dotgraph), highlight, same_label_siblings=False + ) + for node in nodes.values(): + self._scene.addItem(node) + for group in edges.values(): + for edge in group: + edge.add_to_scene(self._scene) self._scene.setSceneRect(self._scene.itemsBoundingRect()) - def _render_debug_data(self, qitem_model: QStandardItemModel) -> None: - """Render debug data in the tree view on the right side of the scene""" - - # Only redraw when the item-model differs from the previous one - if self._prev_QItemModel == qitem_model: + def _render_debug_data(self, model: QStandardItemModel) -> None: + if self._prev_item_model is model: return - else: - self._prev_QItemModel = qitem_model - self._widget.stack_prop_tree_view.setHeaderHidden(True) - self._widget.stack_prop_tree_view.setModel(qitem_model) - self._widget.stack_prop_tree_view.expandAll() + self._prev_item_model = model + self._widget.stack_prop_tree_view.setHeaderHidden(True) + self._widget.stack_prop_tree_view.setModel(model) + self._widget.stack_prop_tree_view.expandAll() def set_dsd(self, name: str) -> None: - """ - Set the target dsd - - :param name: display_name of any dsd in the locations.yaml - """ - if name == "Select DSD...": - self.dsd_follower = None - return - - # Check if the selected dsd is known - if name not in self.running_dsd_instances.keys(): + topic = self.running_dsd_instances.get(name) + if self.dsd_follower is not None and self.dsd_follower.dsd_debug_topic == topic: return - - # Destroy the old dsd follower, so that we don't keep old subscriptions that take up resources if self.dsd_follower is not None: self.dsd_follower.destroy() + self.dsd_follower = DsdFollower(self._node, topic) if topic is not None else None + self._messages = None - # Initialize dsd follower - self.dsd_follower = DsdFollower(self._node, self.running_dsd_instances[name]) + def shutdown_plugin(self) -> None: + if self._timer_id is not None: + self.killTimer(self._timer_id) + self._timer_id = None + if self.dsd_follower is not None: + self.dsd_follower.destroy() + self.dsd_follower = None def main() -> None: plugin = "dynamic_stack_decider_visualization.dsd_visualization_plugin.DsdVizPlugin" - main = Main(filename=plugin) - sys.exit(main.main(standalone=plugin)) + sys.exit(Main(filename=plugin).main(standalone=plugin)) if __name__ == "__main__": diff --git a/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/interactive_graphics_view.py b/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/interactive_graphics_view.py index e4a035b..a1a7e01 100644 --- a/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/interactive_graphics_view.py +++ b/dynamic_stack_decider_visualization/dynamic_stack_decider_visualization/interactive_graphics_view.py @@ -36,7 +36,6 @@ from PyQt5.QtWidgets import QGraphicsView, QWidget -# ruff: noqa: N802 class InteractiveGraphicsView(QGraphicsView): zoomed = pyqtSignal() diff --git a/dynamic_stack_decider_visualization/resource/StackmachineViz.ui b/dynamic_stack_decider_visualization/resource/StackmachineViz.ui index d20a8e3..98a67d2 100644 --- a/dynamic_stack_decider_visualization/resource/StackmachineViz.ui +++ b/dynamic_stack_decider_visualization/resource/StackmachineViz.ui @@ -203,7 +203,7 @@ InteractiveGraphicsView QGraphicsView -
rqt_graph.interactive_graphics_view
+
dynamic_stack_decider_visualization.interactive_graphics_view
 diff --git a/dynamic_stack_decider_visualization/setup.py b/dynamic_stack_decider_visualization/setup.py index aa8ea8b..b4e09dd 100644 --- a/dynamic_stack_decider_visualization/setup.py +++ b/dynamic_stack_decider_visualization/setup.py @@ -6,6 +6,7 @@ name=package_name, version="0.2.1", description="ROS 2 RQT visualization for the Dynamic Stack Decider", + license="MIT", python_requires=">=3.10", packages=find_packages(exclude=["test", "test.*", "tests", "tests.*"]), package_data={package_name: ["py.typed"]}, @@ -15,7 +16,7 @@ ("share/" + package_name, ["package.xml"]), ("share/" + package_name, ["plugin.xml"]), ], - install_requires=["pydot>=3", "PyQt5>=5.15"], + install_requires=["dynamic_stack_decider>=0.5.3", "pydot>=3", "PyQt5>=5.15"], zip_safe=False, entry_points={ "console_scripts": [ diff --git a/dynamic_stack_decider_visualization/test/conftest.py b/dynamic_stack_decider_visualization/test/conftest.py new file mode 100644 index 0000000..d129ab7 --- /dev/null +++ b/dynamic_stack_decider_visualization/test/conftest.py @@ -0,0 +1,41 @@ +"""Shared ROS subscription stand-ins and an offscreen Qt application.""" + +from unittest.mock import Mock + +import pytest + + +class FakeNode: + def __init__(self): + self.logger = Mock() + self.subscriptions = [] + self.destroyed = [] + self.topics = [("/robot/behavior/dsd_tree", ["std_msgs/msg/String"])] + + def get_logger(self): + return self.logger + + def create_subscription(self, message_type, topic, callback, qos): + handle = object() + self.subscriptions.append((handle, topic, callback)) + return handle + + def destroy_subscription(self, subscription): + self.destroyed.append(subscription) + + def get_topic_names_and_types(self): + return self.topics + + +@pytest.fixture +def node(): + return FakeNode() + + +@pytest.fixture(scope="session") +def qapp(): + from PyQt5.QtWidgets import QApplication + + app = QApplication.instance() or QApplication([]) + yield app + app.processEvents() diff --git a/dynamic_stack_decider_visualization/test/test_follower.py b/dynamic_stack_decider_visualization/test/test_follower.py new file mode 100644 index 0000000..e52d146 --- /dev/null +++ b/dynamic_stack_decider_visualization/test/test_follower.py @@ -0,0 +1,172 @@ +"""Snapshot validation, rendering, and cache invalidation regressions.""" + +import copy +import importlib +import json +import os + +import pytest + +if os.environ.get("DSD_REQUIRE_GUI"): + importlib.import_module("PyQt5.QtWidgets") +else: + pytest.importorskip("PyQt5.QtWidgets") + +from dynamic_stack_decider.tree import ActionTreeElement, DecisionTreeElement +from dynamic_stack_decider_visualization.dsd_follower import DsdFollower +from qt_dotgraph.pydotfactory import PydotFactory +from std_msgs.msg import String + + +def snapshots(): + root = DecisionTreeElement("Mode", None) + first = ActionTreeElement("Wait", root) + middle = ActionTreeElement("Work", root) + last = ActionTreeElement("Wait", root) + first.next_element = middle + middle.next_element = last + root.add_child_element(first, "GO") + other = DecisionTreeElement("Other", root) + other.add_child_element(ActionTreeElement("ElseAction", other), "ELSE") + root.add_child_element(other, "ELSE") + stack = { + "id": id(root), + "type": "decision", + "name": "Mode", + "debug_data": {}, + "activation_reason": None, + "next": { + "id": id(last), + "type": "action", + "name": "Wait", + "debug_data": {"value": 1}, + "activation_reason": "GO", + "next": None, + }, + } + return root.repr_dict(), stack + + +@pytest.fixture +def follower(node, qapp): + receiver = DsdFollower(node, "/debug") + yield receiver + receiver.destroy() + + +def feed(follower, tree, stack): + follower._tree_callback(String(data=json.dumps(tree))) + follower._stack_callback(String(data=json.dumps(stack))) + + +def test_repeated_names_highlight_only_the_active_chain_element(follower): + tree, stack = snapshots() + feed(follower, tree, stack) + graph = follower.to_dotgraph(False) + active = [node.get_name() for node in graph.get_nodes() if node.get_color() == "orangered"] + assert set(active) == {str(stack["id"]), str(stack["next"]["id"])} + first_id = str(tree["children"]["GO"]["id"]) + assert graph.get_node(first_id)[0].get_color() == "gray" + assert "digraph" in PydotFactory().create_dot(graph) + + +def test_full_tree_flag_is_part_of_cache_key(follower): + tree, stack = snapshots() + feed(follower, tree, stack) + partial = follower.to_dotgraph(False) + complete = follower.to_dotgraph(True) + assert partial is not complete + assert len(complete.get_nodes()) > len(partial.get_nodes()) + assert follower.to_dotgraph(True) is complete + + +def test_debug_changes_reuse_graph_but_refresh_item_model(follower): + tree, stack = snapshots() + feed(follower, tree, stack) + graph = follower.to_dotgraph(False) + model = follower.to_q_item_model() + assert follower.to_q_item_model() is model + stack["next"]["debug_data"]["value"] = 2 + follower._stack_callback(String(data=json.dumps(stack))) + assert follower.to_dotgraph(False) is graph + new_model = follower.to_q_item_model() + assert new_model is not model + assert new_model.item(2).child(0).text() == "value: 2" + + +def test_tree_reload_clears_old_stack_and_render_caches(follower): + tree, stack = snapshots() + feed(follower, tree, stack) + old_graph = follower.to_dotgraph(False) + replacement = copy.deepcopy(tree) + replacement["id"] += 1 + follower._tree_callback(String(data=json.dumps(replacement))) + assert follower.stack is None + assert follower.to_dotgraph(False) is not old_graph + follower._stack_callback(String(data=json.dumps(stack))) + assert follower.stack is None + stack["id"] = replacement["id"] + follower._stack_callback(String(data=json.dumps(stack))) + assert follower.stack is not None + + +def test_unchanged_tree_does_not_discard_stack(follower): + tree, stack = snapshots() + feed(follower, tree, stack) + graph = follower.to_dotgraph(False) + follower._tree_callback(String(data=json.dumps(tree))) + assert follower.stack is not None + assert follower.to_dotgraph(False) is graph + + +@pytest.mark.parametrize("payload", ["{", "null", "[]", '{"id": true}', '{"id":1,"name":"A","type":"invalid"}']) +def test_invalid_tree_messages_keep_last_valid_state(follower, node, payload): + tree, stack = snapshots() + feed(follower, tree, stack) + follower._tree_callback(String(data=payload)) + assert follower.tree == tree + assert follower.stack == stack + node.logger.error.assert_called() + + +@pytest.mark.parametrize("payload", ["{", "[]", '{"id":1,"name":"A","type":"action","debug_data":{}}']) +def test_invalid_stack_messages_keep_last_valid_state(follower, node, payload): + tree, stack = snapshots() + feed(follower, tree, stack) + follower._stack_callback(String(data=payload)) + assert follower.stack == stack + node.logger.error.assert_called() + + +def test_inconsistent_stack_identity_is_rejected(follower, node): + tree, stack = snapshots() + feed(follower, tree, stack) + invalid = copy.deepcopy(stack) + invalid["next"]["name"] = "WrongName" + follower._stack_callback(String(data=json.dumps(invalid))) + assert follower.stack == stack + node.logger.error.assert_called() + + +def test_nested_and_null_debug_values_are_shown(follower): + tree, stack = snapshots() + stack["next"]["debug_data"] = {"items": [None, {"enabled": False}]} + feed(follower, tree, stack) + debug = follower.to_q_item_model().item(2).child(0) + assert debug.child(0).text() == "0: None" + assert debug.child(1).child(0).text() == "enabled: False" + + +def test_destroy_releases_both_subscriptions_once(follower, node): + follower.destroy() + follower.destroy() + assert len(node.destroyed) == 2 + assert set(node.destroyed) == {handle for handle, _, _ in node.subscriptions} + + +def test_ignored_keys_do_not_trigger_structural_changes(): + assert not DsdFollower.stack_dict_changed({"name": "A"}, {"name": "A", "debug_data": {}}, ["debug_data"]) + + +def test_waiting_graph_is_cached(follower): + assert follower.to_dotgraph(False) is follower.to_dotgraph(False) diff --git a/dynamic_stack_decider_visualization/test/test_plugin.py b/dynamic_stack_decider_visualization/test/test_plugin.py new file mode 100644 index 0000000..1e1d799 --- /dev/null +++ b/dynamic_stack_decider_visualization/test/test_plugin.py @@ -0,0 +1,163 @@ +"""Load the real Qt UI and Graphviz renderer without a running robot.""" + +import importlib +import os +from pathlib import Path + +import pytest + +if os.environ.get("DSD_REQUIRE_GUI"): + importlib.import_module("PyQt5.QtWidgets") +else: + pytest.importorskip("PyQt5.QtWidgets") + +import dynamic_stack_decider_visualization.dsd_visualization_plugin as plugin_module +from PyQt5.QtCore import QObject, QPoint, QPointF, Qt +from PyQt5.QtGui import QWheelEvent + + +class Context(QObject): + def __init__(self, node): + super().__init__() + self.node = node + self.widgets = [] + + def add_widget(self, widget): + self.widgets.append(widget) + + +class Settings: + def value(self, key, default): + return default + + +@pytest.fixture +def plugin(qapp, node, monkeypatch): + package = Path(__file__).resolve().parents[1] + monkeypatch.setattr(plugin_module, "get_package_share_directory", lambda name: str(package)) + context = Context(node) + instance = plugin_module.DsdVizPlugin(context) + instance.restore_settings(object(), Settings()) + yield instance + instance.shutdown_plugin() + for widget in context.widgets: + widget.close() + qapp.processEvents() + + +def test_ui_loads_and_waiting_message_renders(plugin): + assert plugin._scene.items() + model = plugin._prev_item_model + graph = plugin._prev_dotgraph + plugin.refresh() + assert plugin._prev_item_model is model + assert plugin._prev_dotgraph is graph + + +def test_deselecting_releases_subscriptions(plugin, node): + plugin.set_dsd("/robot/behavior") + assert plugin.dsd_follower is not None + plugin.set_dsd("Select DSD...") + assert plugin.dsd_follower is None + assert len(node.destroyed) == 2 + + +def test_refresh_preserves_selection_without_resubscribing(plugin, node): + plugin._widget.dsd_selector_combo_box.setCurrentText("/robot/behavior") + original = plugin.dsd_follower + plugin.discover_dsd_instances() + assert plugin.dsd_follower is original + assert len(node.subscriptions) == 2 + assert node.destroyed == [] + + +def test_disappearing_topic_releases_subscriptions(plugin, node): + plugin._widget.dsd_selector_combo_box.setCurrentText("/robot/behavior") + node.topics = [] + plugin.discover_dsd_instances() + assert plugin.dsd_follower is None + assert len(node.destroyed) == 2 + + +def test_shutdown_stops_timer_and_releases_subscriptions(plugin, node): + plugin.set_dsd("/robot/behavior") + plugin.shutdown_plugin() + plugin.shutdown_plugin() + assert plugin._timer_id is None + assert plugin.dsd_follower is None + assert len(node.destroyed) == 2 + + +def test_zoom_signal_disables_auto_fit(plugin): + view = plugin._widget.graphics_view + plugin._widget.auto_fit_graph_check_box.setChecked(True) + before = view.transform().m11() + event = QWheelEvent( + QPointF(10, 10), + QPointF(10, 10), + QPoint(0, 0), + QPoint(0, 120), + Qt.NoButton, + Qt.NoModifier, + Qt.NoScrollPhase, + False, + ) + view.wheelEvent(event) + assert not plugin._widget.auto_fit_graph_check_box.isChecked() + assert view.transform().m11() > before + + +def test_live_tree_and_stack_render_in_the_actual_scene(plugin): + import json + + from std_msgs.msg import String + + plugin.set_dsd("/robot/behavior") + follower = plugin.dsd_follower + tree = { + "id": 1, + "type": "decision", + "name": "Mode", + "parameters": {}, + "children": {"GO": {"id": 2, "type": "action", "name": "Idle", "parameters": {}, "next": None}}, + } + stack = { + "id": 1, + "type": "decision", + "name": "Mode", + "debug_data": {}, + "activation_reason": None, + "next": { + "id": 2, + "type": "action", + "name": "Idle", + "debug_data": {"status": "active"}, + "activation_reason": "GO", + "next": None, + }, + } + follower._tree_callback(String(data=json.dumps(tree))) + follower._stack_callback(String(data=json.dumps(stack))) + plugin.refresh() + assert len(plugin._scene.items()) > 2 + assert plugin._prev_item_model.item(2).child(0).text() == "status: active" + + +def test_svg_export_writes_rendered_scene(plugin, tmp_path, monkeypatch): + path = tmp_path / "scene.svg" + monkeypatch.setattr(plugin_module.QFileDialog, "getSaveFileName", lambda *args: (str(path), "")) + plugin.save_svg_to_file() + assert " None: assert f"{name}/__init__.py" in files assert f"{name}/py.typed" in files assert not any(path.startswith(("test/", "tests/")) for path in files) - assert any(path.endswith("/package.xml") for path in files) + package_xml = next(path for path in files if path.endswith("/package.xml")) + manifest = ET.fromstring(archive.read(package_xml)) + metadata_file = next(path for path in files if path.endswith(".dist-info/METADATA")) + metadata = email.message_from_bytes(archive.read(metadata_file)) + assert metadata["Version"] == manifest.findtext("version") + assert any(path.endswith("/LICENSE") for path in files) + imported = Path(directory) / "imported" + archive.extractall(imported) if name.endswith("visualization"): assert any(path.endswith("/resource/StackmachineViz.ui") for path in files) assert any(path.endswith("/plugin.xml") for path in files) + subprocess.run( + [ + sys.executable, + "-I", + "-c", + "import importlib, pathlib, sys; sys.path.insert(0, sys.argv[1]); " + "module = importlib.import_module(sys.argv[2]); " + "assert pathlib.Path(module.__file__).is_relative_to(sys.argv[1])", + str(imported), + name, + ], + check=True, + ) + with tarfile.open(next(Path(directory).glob("*.tar.gz"))) as archive: + sources = archive.getnames() + assert any(path.endswith("/README.md") for path in sources) + assert any(path.endswith("/LICENSE") for path in sources) + if not name.endswith("visualization"): + assert any(path.endswith("/test/test.dsd") for path in sources) + assert any(path.endswith("/test/fixture_elements/actions.py") for path in sources) if __name__ == "__main__":