Incorrect traversal when a subtree is used multiple times

[Z0rdon]

Hello,
I hope you are having a nice day.
Thank you very much for creating this nice library. I appreciate the work you folks put into this.

Problem Overview

I observed the following issue. When a subtree is used multiple times, the indices do not generate correctly. As a result, the tree execution does not follow the proper evaluation order.

Here is the tree I created in groot2:

<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4"
      main_tree_to_execute="btree_debug">
  <BehaviorTree ID="btree_debug">
    <Sequence>
      <SubTree ID="btree_subtree"
               _autoremap="true"/>
      <A/>
      <SubTree ID="btree_subtree"
               _autoremap="true"/>
      <B/>
    </Sequence>
  </BehaviorTree>

  <BehaviorTree ID="btree_subtree">
    <sub/>
  </BehaviorTree>

  <!-- Description of Node Models (used by Groot) -->
  <TreeNodesModel>
    <Action ID="A"
            editable="true"/>
    <Action ID="B"
            editable="true"/>
    <Action ID="sub"
            editable="true"/>
  </TreeNodesModel>

</root>

Here is the invalid generated output:

static struct zephyrbt_node btree_debug_nodes[] = {
	{
		.function=zephyrbt_action_b,
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INIT, (
			.init=zephyrbt_action_b_init,
		))
		.sibling=-1,
		.child=-1,
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INFO, (
			.name="b [action, 0]",
			.index=0,
		))
	},
	{
		.function=zephyrbt_action_sub,
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INIT, (
			.init=zephyrbt_action_sub_init,
		))
		.sibling=0,
		.child=-1,
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INFO, (
			.name="sub [action, 1]",
			.index=1,
		))
	},
	{
		.function=zephyrbt_action_a,
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INIT, (
			.init=zephyrbt_action_a_init,
		))
		.sibling=1,
		.child=-1,
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INFO, (
			.name="a [action, 2]",
			.index=2,
		))
	},
	{
		.function=zephyrbt_action_sub,
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INIT, (
			.init=zephyrbt_action_sub_init,
		))
		.sibling=0, **<------- Sibling should be 2**
		.child=-1,
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INFO, (
			.name="sub [action, 1]",
			.index=1,  **<------- Index should be 3**
		))
	},
	{
		.function=zephyrbt_control_sequence,
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INIT, (
			.init=NULL,
		))
		.sibling=-1,
		.child=1, **<--------- Child should be 3**
		IF_ENABLED(CONFIG_ZEPHYR_BEHAVIOUR_TREE_NODE_INFO, (
			.name="sequence [control, 4]",
			.index=4,
		))
	},
};

• as you can see, the 'A' action will never be executed.

Thank you for your time.

[nandojve]

Hi @Z0rdon ,

Thank you for your feedback.

We'll investigate.

[Z0rdon]

Hello @nandojve.
I hope you are well.

I had a moment to examine the problem a bit more.
I observed the following in 'getIndexedNode'.

The 'idx_table' correctly contains the nodes with different indices.
From my example above, the 'idx_table' contents are:

[
[138261970755296, 0, 'b', 'action', True, None, []], 
[138261970755456, 1, 'sub', 'action', True, None, []], 
[138261970755136, 2, 'a', 'action', True, None, []], 
[138261970755456, 3, 'sub', 'action', True, None, []], 
[138261970754976, 4, 'sequence', 'control', False, None, []]
]

But using 'id(elem)' to retrieve the index will always just retrieve the lower-index node.

[nandojve]

Hi @Z0rdon ,

Yes, I could reproduce the issue.
The problem is that the id (first column) repeats when a sub-tree exists. I highlight from your example:

[
[138261970755296, 0, 'b', 'action', True, None, []], 
+[138261970755456, 1, 'sub', 'action', True, None, []], 
[138261970755136, 2, 'a', 'action', True, None, []], 
+[138261970755456, 3, 'sub', 'action', True, None, []], 
[138261970754976, 4, 'sequence', 'control', False, None, []]
]

This means that the elements should have a binary or even a ternary key where the second part is the id(tree) and the third the name. For instance, with a new column at position 1:

[
[138261970755296, 0, 0, 'b', 'action', True, None, []], 
[138261970755456, 1, 1, 'sub', 'action', True, None, []], 
[138261970755136, 0, 2, 'a', 'action', True, None, []], 
[138261970755456, 2, 3, 'sub', 'action', True, None, []], 
[138261970754976, 0, 4, 'sequence', 'control', False, None, []]
]

where 0 is the main tree, 1 first sub-tree, 2 second sub-tree.

So, the search algorithm could not only rely on a name, because you can have multiple instances of that name. The same way it can not only rely in the node name + id(node) because a sub-tree will repeat these pattern in the collection when parsing (problem that you found). Then we should associate each node based in the name + id(node) + id(tree). I believe with this we can fix the issue.

The change is not so trivial and will take sometime. The good thing is that the problem is understood and there is a solution.

[Z0rdon]

Hello @nandojve,
That sounds like a very nice approach.

I was experimenting as well with some other possibilities.
Below is a draft of a different approach where the subtrees are 'replaced inline' using a deep copy.
This piece of code has not been thoroughly tested, but is an interesting proof of concept.
I also took the liberty of adding some mypy types to the functions I added.

modified   scripts/generate-zephyrbt-from-behaviourtreecpp-xml
@@ -17,6 +17,7 @@ from pathlib import Path
 from functools import reduce
 import re
 import struct
+from copy import deepcopy
 
 from natsort import natsorted
 import xml.etree.ElementTree as ET
@@ -450,6 +451,52 @@ def addBlackboardVariables(idx_nodes_set, blackboard_set):
                 index, str(blackboard[0][1]), blackboard_set)
 
 
+def collect_subtrees(root: ET.Element) -> dict[str, ET.Element]:
+    """
+    Create a dict that maps an ID string to a particular subtree
+    """
+    subtrees = {}
+    for bt in root.findall('BehaviorTree'):
+        id_value = bt.attrib.get('ID')
+        assert id_value is not None, 'ID cannot be None'
+        # store the first (root) child of the BehaviorTree
+        subtrees[id_value] = bt[0]
+    return subtrees
+
+
+def inline_subtrees(elem: ET.Element, subtrees: dict[str, ET.Element]) -> None:
+    """
+    Replace SubTree elements in the main tree with a deep copy of the referenced subtree's root node.
+    This modifies the XML tree in-place so subsequent processing doesn't need to resolve SubTree nodes.
+    """
+    # Build a new list of children
+    # (with the SubTree references replaced by deep copies of referenced nodes)
+    new_children = []
+    for child in list(elem):
+        if child.tag == 'SubTree':
+            # retrieve a reference to the underlying subtree
+            ref_id = child.attrib.get('ID')
+            if ref_id is None:
+                raise Exception("SubTree element missing ID attribute")
+            referenced = subtrees.get(ref_id)
+            if referenced is None:
+                raise Exception(
+                    f"Referenced SubTree ID '{ref_id}' not found")
+
+            # deepcopy referenced subtree root node
+            copied = deepcopy(referenced)
+            new_children.append(copied)
+        else:
+            new_children.append(child)
+
+    # replace element's children
+    elem[:] = new_children
+
+    # recurse into children
+    for c in elem:
+        inline_subtrees(c, subtrees)
+
+
 def build_bt_set(builtin, root):
     main = root.attrib.get('main_tree_to_execute')
 
@@ -462,13 +509,19 @@ def build_bt_set(builtin, root):
         for e in elem:
             parseCustomNodes(custom_set, e)
 
-    for elem in root.iter('BehaviorTree'):
-        if elem.attrib.get('ID') != main:
-            continue
+    subtrees = collect_subtrees(root)
+    assert subtrees is not None
+    main_id = root.attrib.get('main_tree_to_execute')
+
+    main_elem = subtrees.get(main_id)
+    assert main_elem is not None
+
+    # Inline subtrees into the main tree so later processing does not need to handle SubTree nodes
+    inline_subtrees(main_elem, subtrees)
 
-        parseNodes(builtin, idx_nodes_set, custom_set, 0, elem[0], root)
-        assemblyBT(builtin, idx_nodes_set, bt_set, 0, elem[0], elem[0], root)
-        break
+    parseNodes(builtin, idx_nodes_set, custom_set, 0, main_elem, root)
+    assemblyBT(builtin, idx_nodes_set, bt_set, 0,
+               main_elem, main_elem, root)
 
     addBuiltinBlackboard(idx_nodes_set, blackboard_set, builtin)
     for elem in root.iter('TreeNodesModel'):

Here is the full modified script:
temp.py