fix groot2 publisher

avi9700 · Mar 21, 2023 · b0ffa24 · b0ffa24
1 parent 7b88aea
commit b0ffa24
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 25 deletions.
diff --git a/examples/t12_groot_howto.cpp b/examples/t12_groot_howto.cpp
@@ -12,27 +12,28 @@
 static const char* xml_text = R"(
 <root BTCPP_format="4">
 
-    <BehaviorTree ID="MainTree">
-        <Sequence>
-            <Fallback>
-                <Inverter>
-                    <IsDoorClosed/>
-                </Inverter>
-                <SubTree ID="DoorClosed"/>
-            </Fallback>
-            <PassThroughDoor/>
-        </Sequence>
-    </BehaviorTree>
-
-    <BehaviorTree ID="DoorClosed">
-        <Fallback>
-            <OpenDoor/>
-            <RetryUntilSuccessful num_attempts="5">
-                <PickLock/>
-            </RetryUntilSuccessful>
-            <SmashDoor/>
-        </Fallback>
-    </BehaviorTree>
+  <BehaviorTree ID="MainTree">
+    <Sequence>
+      <Script code="door_open:=false" />
+      <Fallback>
+        <Inverter>
+          <IsDoorClosed/>
+        </Inverter>
+        <SubTree ID="DoorClosed" _autoremap="true" door_open="{door_open}"/>
+      </Fallback>
+      <PassThroughDoor/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="DoorClosed">
+    <Fallback name="tryOpen" _onSuccess="door_open:=true">
+      <OpenDoor/>
+        <RetryUntilSuccessful num_attempts="5">
+          <PickLock/>
+        </RetryUntilSuccessful>
+      <SmashDoor/>
+    </Fallback>
+  </BehaviorTree>
 
 </root>
  )";
@@ -51,13 +52,14 @@ int main()
   // generated using this command and imported.
 
   std::string xml_models = BT::writeTreeNodesModelXML(factory);
-  std::cout << " ---------- XML file containing models ----------\n"
-            << xml_models
-            << "-------------------------------------------------\n";
 
   factory.registerBehaviorTreeFromText(xml_text);
   auto tree = factory.createTree("MainTree");
 
+  std::cout << " ---------- XML file  ----------\n"
+            << BT::WriteTreeToXML(tree, false)
+            << "--------------------------------\n";
+
   // Connect the Groot2Publisher. This will allow Groot2 to
   // get the tree and poll status updates.
   BT::Groot2Publisher publisher(tree);

diff --git a/src/loggers/groot2_publisher.cpp b/src/loggers/groot2_publisher.cpp
@@ -89,7 +89,8 @@ Groot2Publisher::Groot2Publisher(const BT::Tree& tree,
 
   for(const auto& subtree: tree.subtrees)
   {
-    subtrees_.insert( {subtree->instance_name, subtree} );
+    auto name = subtree->instance_name.empty() ? subtree->tree_ID : subtree->instance_name;
+    subtrees_.insert( {name, subtree} );
 
     for(const auto& node: subtree->nodes)
     {

diff --git a/tests/gtest_subtree.cpp b/tests/gtest_subtree.cpp
@@ -254,3 +254,32 @@ TEST(SubTree, SubtreePlusD)
   auto status = tree.tickWhileRunning();
   ASSERT_EQ(status, BT::NodeStatus::SUCCESS);
 }
+
+// TODO: only explicit remapping work, autoremapping fails
+TEST(SubTree, ScriptRemap)
+{
+  static const char* xml_text = R"(
+
+<root BTCPP_format="4" >
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <Script code = "value:=0" />
+            <SubTree ID="mySubtree" value="{value}"  />
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="mySubtree">
+        <Script code = "value:=1" />
+    </BehaviorTree>
+</root> )";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerBehaviorTreeFromText(xml_text);
+
+  Tree tree = factory.createTree("MainTree");
+  tree.tickOnce();
+
+  ASSERT_EQ(tree.subtrees[1]->blackboard->get<int>("value"), 1);
+  ASSERT_EQ(tree.subtrees[0]->blackboard->get<int>("value"), 1);
+}