Skip to content

Remove sequence as a distinct element + allow decisions/subtree after action #114

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
MegaIng wants to merge 6 commits into
base: master
Choose a base branch
from
Open

Remove sequence as a distinct element + allow decisions/subtree after action #114

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
f741269
Replace the Sequence tree element with a next_action chain.
MegaIng Jul 9, 2026
640befd
Update dsd_follower to deal with the changes to sequences.
MegaIng Jul 9, 2026
78e1ccd
Fix the incorrect element being highlighted when duplicates names are…
MegaIng Jul 9, 2026
eb3d780
Allow subtrees and decisions after actions.
MegaIng Jul 9, 2026
e8afbd0
Add comprehensive test suite
MegaIng Jul 9, 2026
54fad4c
Ruff format
MegaIng Jul 9, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 22 additions & 27 deletions dynamic_stack_decider/dynamic_stack_decider/dsd.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,10 @@
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.sequence_element import SequenceElement
from dynamic_stack_decider.tree import (
AbstractTreeElement,
ActionTreeElement,
DecisionTreeElement,
SequenceTreeElement,
Tree,
)

Expand Down Expand Up @@ -188,25 +186,22 @@ def _bind_modules(self, element):
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)
elif isinstance(element, SequenceTreeElement):
for action in element.action_elements:
self._bind_modules(action)
else:
raise ValueError(f'Unknown parser tree element type "{type(element)}" for element "{element}"!')

def _init_element(self, element: AbstractTreeElement) -> AbstractStackElement:
"""Initializes the module belonging to the given element."""
if isinstance(element, SequenceTreeElement):
return SequenceElement(self.blackboard, self, element.action_elements, self._init_element)
else:
return element.module(self.blackboard, self, element.parameters)
return element.module(self.blackboard, self, element.parameters)

def set_start_element(self, start_element: AbstractTreeElement):
"""
Expand Down Expand Up @@ -274,12 +269,7 @@ def update(self, reevaluate: bool = True):
if reevaluate:
# reset flag
self.do_not_reevaluate = False
if (
isinstance(current_instance, AbstractActionElement)
and current_instance.never_reevaluate
or isinstance(current_instance, SequenceElement)
and current_instance.current_action.never_reevaluate
):
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
Expand Down Expand Up @@ -316,15 +306,19 @@ def pop(self):
# stop reevaluating
self.stack_reevaluate = False
else:
if isinstance(self.stack[-1][1], SequenceElement):
# If we are in a sequence, only one action should be popped
if not self.stack[-1][1].in_last_element():
# We are still in the sequence, therefore we do not want to pop the SequenceElement,
# only a single element of the sequence
# We also do not want to reset do_not_reevaluate because an action in the sequence
# may control the stack beyond its own lifetime but in the sequence element's lifetime
self.stack[-1][1].pop_one().on_pop()
return
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()

Expand Down Expand Up @@ -352,6 +346,9 @@ def debug_publish_stack(self):
for tree_elem, elem_instance in reversed(self.stack):
elem_data = elem_instance.repr_dict()
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

Expand All @@ -378,11 +375,9 @@ def debug_publish_current_action(self):

# Get the top element
stack_top = self.stack[-1][1]
# Check if it is an action or a sequence element and retrieve the current action
# Check if it is an action and retrieve it
if isinstance(stack_top, AbstractActionElement):
current_action = stack_top
elif isinstance(stack_top, SequenceElement):
current_action = stack_top.current_action
else:
return

Expand Down
192 changes: 125 additions & 67 deletions dynamic_stack_decider/dynamic_stack_decider/parser.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
import yaml
from rclpy.node import Node

from dynamic_stack_decider.tree import ActionTreeElement, DecisionTreeElement, SequenceTreeElement, Tree
from dynamic_stack_decider.tree import ActionTreeElement, DecisionTreeElement, Tree


class DsdParser:
Expand Down Expand Up @@ -101,57 +101,24 @@ def parse(self, file_path: str) -> Tree:

if call.startswith("#"):
# A subtree is called here.
subtree_name = call.strip("#")
name, parameters, unset_parameters = self._extract_parameters(subtree_name, lnr)
if name not in subtrees:
raise ParseError(f"Error parsing line {lnr}: {name} not defined")
subtree = subtrees[name]
if (set(parameters.keys()) | set(unset_parameters.keys())) != set(subtree.parameter_list):
raise ParseError(
"Error parsing line {}: Invalid parameters specified.\n"
"Available parameters are {}, specified parameters are {}.".format(
lnr,
set(parameters.keys()) | set(unset_parameters.keys()),
set(subtree.parameter_list),
)
)
# The root element of the subtree should be placed in this tree position
if current_tree_element is None:
# The current subtree is empty, set the subtree as its root element
subtree, _, _ = self._lookup_subtree(call, lnr, subtrees)
current_subtree.set_root_element(subtree.root_element)
else:
# Copy this subtree
subtree_copy = copy.deepcopy(subtree.root_element)
# Evaluate all unset parameters
to_evaluate = [subtree_copy]
while len(to_evaluate) > 0:
current = to_evaluate.pop(0)
if isinstance(current, SequenceTreeElement):
# For sequence elements, only evaluate the actions
to_evaluate.extend(current.action_elements)
continue
elif isinstance(current, DecisionTreeElement):
to_evaluate.extend(current.children.values())

for name, reference in current.unset_parameters.items():
if reference in parameters:
current.parameters[name] = parameters[reference]
elif reference in unset_parameters:
current.unset_parameters[name] = unset_parameters[reference]
else:
raise ParseError(
f"Error evaluating subtree call in line {lnr}: "
f"Unknown reference to {reference}."
)

# Append this subtree in the current position
# Copy this subtree and place it in the current tree position
subtree_copy = self._instantiate_subtree(call, lnr, subtrees)
current_tree_element.add_child_element(subtree_copy, result)

elif re.search(r"\s*,\s*", call):
# A sequence element
actions = re.split(r"\s*,\s*", call)
element = self._create_sequence_element(actions, current_tree_element, lnr)
current_tree_element.add_child_element(element, result)
# A sequence: a chain of actions, optionally ending in a decision or a subtree
tokens = re.split(r"\s*,\s*", call)
head, tail = self._create_chain(tokens, current_tree_element, lnr, subtrees)
current_tree_element.add_child_element(head, result)
if tokens[-1].startswith("$"):
# A decision at the end of a sequence receives its children from the
# following indented lines
current_tree_element = tail

elif call.startswith("@"):
# An action is called
Expand All @@ -166,18 +133,22 @@ def parse(self, file_path: str) -> Tree:

else:
raise ParseError(
f"Error parsing line {lnr}: " f"Element {call} is neither an action nor a decision"
f"Error parsing line {lnr}: Element {call} is neither an action nor a decision"
)

else:
# No arrow, must be the beginning of a new subtree
if re.search(r"\s*,\s*", line):
actions = re.split(r"\s*,\s*", line)
element = self._create_sequence_element(actions, current_tree_element, lnr)
if re.search(r"\s*,\s*", line_content):
tokens = re.split(r"\s*,\s*", line_content)
head, tail = self._create_chain(tokens, current_tree_element, lnr, subtrees)
current_subtree.set_root_element(head)
# A decision at the end of a sequence receives its children from the
# following indented lines
current_tree_element = tail if tokens[-1].startswith("$") else head
else:
element = self._create_tree_element(line_content, current_tree_element, lnr)
current_subtree.set_root_element(element)
current_tree_element = element
current_subtree.set_root_element(element)
current_tree_element = element

last_indent = indent
return tree
Expand All @@ -203,7 +174,7 @@ def _extract_parameters(self, token, lnr):

if " " in parameter_value:
raise ParseError(
f"Error parsing line {lnr}: Parameter values should not contain spaces. " "Did you forget a comma?"
f"Error parsing line {lnr}: Parameter values should not contain spaces. Did you forget a comma?"
)

if parameter_value.startswith("%"):
Expand All @@ -223,7 +194,7 @@ def _create_tree_element(self, token, parent, lnr):
The method derives the type (Action/Decision) and optional parameters from the token
:param token: the string describing the element in the dsd description
:param parent: the parent element of the new element, None for root
:type parent: Union[DecisionTreeElement, SequenceTreeElement]
:type parent: Union[DecisionTreeElement, ActionTreeElement]
:param lnr: Line number of the current line (used for error messages)
:type lnr: int
:return: a TreeElement containing the information given in token
Expand All @@ -237,25 +208,112 @@ def _create_tree_element(self, token, parent, lnr):
raise ParseError("An element has to start with either $ or @")
return element

def _create_sequence_element(self, actions, parent, lnr):
def _create_chain(self, tokens, parent, lnr, subtrees):
"""
Create a new sequence element
Create a sequence as a chain of tree elements linked via ``next_element``.

:param actions: The names of actions in the sequence
:type actions: list[str]
:param parent: The parent element of the sequence
:type parent: AbstractDecisionElement
Every element except the last must be an action, as only actions advance to their successor
by popping themselves. The last element may be an action, a decision, or a subtree call, which
allows a decision or a subtree to follow a sequence of actions.

:param tokens: The call tokens of the sequence (e.g. ``["@A", "@B", "$D"]``)
:type tokens: list[str]
:param parent: The element the chain is attached to
:param lnr: Line number of the current line (used for error messages)
:type lnr: int
:return: The sequence element
:param subtrees: The already parsed subtrees, needed to resolve a trailing subtree call
:type subtrees: dict
:return: A tuple of the head element (the child of ``parent``) and the tail element of the chain
"""
head = None
previous = None
for i, token in enumerate(tokens):
is_last = i == len(tokens) - 1
if is_last and token.startswith("#"):
# A subtree may terminate a sequence
element = self._instantiate_subtree(token, lnr, subtrees)
elif is_last:
# The last element may be an action or a decision
element = self._create_tree_element(token, parent, lnr)
else:
# Only actions may be followed by another element in a sequence
if not token.startswith("@"):
raise ParseError(
f"Error parsing line {lnr}: Only actions may be followed by another element "
f"in a sequence, but got '{token}'"
)
element = self._create_tree_element(token, parent, lnr)

if previous is None:
head = element
else:
previous.next_element = element
previous = element
return head, previous

def _lookup_subtree(self, call, lnr, subtrees):
"""
Look up the subtree referenced by a ``#``-call and validate that its parameters match.

:param call: The call token, e.g. ``#SubBehavior + key:value``
:param lnr: Line number of the current line (used for error messages)
:param subtrees: The already parsed subtrees
:return: A tuple of the referenced subtree and the set and unset parameters of the call
"""
name, parameters, unset_parameters = self._extract_parameters(call.strip("#"), lnr)
if name not in subtrees:
raise ParseError(f"Error parsing line {lnr}: {name} not defined")
subtree = subtrees[name]
if (set(parameters.keys()) | set(unset_parameters.keys())) != set(subtree.parameter_list):
raise ParseError(
"Error parsing line {}: Invalid parameters specified.\n"
"Available parameters are {}, specified parameters are {}.".format(
lnr,
set(parameters.keys()) | set(unset_parameters.keys()),
set(subtree.parameter_list),
)
)
return subtree, parameters, unset_parameters

def _instantiate_subtree(self, call, lnr, subtrees):
"""
Create a deep copy of a referenced subtree with its parameters evaluated.

:param call: The call token, e.g. ``#SubBehavior + key:value``
:param lnr: Line number of the current line (used for error messages)
:param subtrees: The already parsed subtrees
:return: The root element of the instantiated subtree
"""
subtree, parameters, unset_parameters = self._lookup_subtree(call, lnr, subtrees)
subtree_copy = copy.deepcopy(subtree.root_element)
self._evaluate_subtree_parameters(subtree_copy, parameters, unset_parameters, lnr)
return subtree_copy

def _evaluate_subtree_parameters(self, root, parameters, unset_parameters, lnr):
"""
Resolve the unset parameters of a copied subtree against the parameters given in the call.

:param root: The root element of the copied subtree
:param parameters: The set parameters of the subtree call
:param unset_parameters: The unset (referenced) parameters of the subtree call
:param lnr: Line number of the current line (used for error messages)
"""
sequence_element = SequenceTreeElement(parent)
for action in actions:
element = self._create_tree_element(action, sequence_element, lnr)
if not isinstance(element, ActionTreeElement):
raise ParseError("In a sequence, only actions are allowed!")
sequence_element.add_action_element(element)
return sequence_element
to_evaluate = [root]
while len(to_evaluate) > 0:
current = to_evaluate.pop(0)
if isinstance(current, DecisionTreeElement):
to_evaluate.extend(current.children.values())
elif isinstance(current, ActionTreeElement) and current.next_element is not None:
# Follow the chain of actions (i.e. a sequence)
to_evaluate.append(current.next_element)

for name, reference in current.unset_parameters.items():
if reference in parameters:
current.parameters[name] = parameters[reference]
elif reference in unset_parameters:
current.unset_parameters[name] = unset_parameters[reference]
else:
raise ParseError(f"Error evaluating subtree call in line {lnr}: Unknown reference to {reference}.")


class ParseError(AssertionError):
Expand Down
Loading
Loading