Fix ROOT blackboard export in Groot2 publisher

include/behaviortree_cpp/loggers/groot2_publisher.h

@@ -55,7 +55,7 @@ class Groot2Publisher : public StatusChangeLogger
 
   void updateStatusBuffer();
 
-  std::vector<uint8_t> generateBlackboardsDump(const std::string& bb_list);
+  Expected<std::vector<uint8_t>> generateBlackboardsDump(const std::string& bb_list);
 
   bool insertHook(Monitor::Hook::Ptr breakpoint);
 

src/loggers/groot2_publisher.cpp

@@ -34,6 +34,8 @@
 
 namespace
 {
+constexpr const char* kRootBlackboardName = "ROOT";
+
 std::array<char, 16> CreateRandomUUID()
 {
   std::random_device rd;
@@ -332,7 +334,13 @@
           }
           std::string const bb_names_str = requestMsg[1].to_string();
           auto msg = generateBlackboardsDump(bb_names_str);
-          reply_msg.addmem(msg.data(), msg.size());
+          if(!msg)
+          {
+            sendErrorReply(msg.error());
+            continue;
+          }
+          auto const& payload = msg.value();
+          reply_msg.addmem(payload.data(), payload.size());
         }
         break;
 
@@ -601,9 +609,12 @@
   }
 }
 
-std::vector<uint8_t> Groot2Publisher::generateBlackboardsDump(const std::string& bb_list)
+Expected<std::vector<uint8_t>>
+Groot2Publisher::generateBlackboardsDump(const std::string& bb_list)
 {
   auto json = nlohmann::json();
+  const Blackboard* exported_root = nullptr;
+
   auto const bb_names = BT::splitString(bb_list, ';');
   for(auto name : bb_names)
   {
@@ -615,7 +626,40 @@
       // lock the weak pointer
       if(auto subtree = it->second.lock())
       {
-        json[bb_name] = ExportBlackboardToJSON(*subtree->blackboard);
+        auto* local_bb = subtree->blackboard.get();
+        auto* root_bb = subtree->blackboard->rootBlackboard();
+        const bool needs_exported_root = (root_bb != local_bb);
+
+        if(bb_name == kRootBlackboardName &&
+           (needs_exported_root || exported_root != nullptr))
+        {
+          return nonstd::make_unexpected("blackboard dump request uses reserved "
+                                         "name [ROOT] together with an "
+                                         "external root blackboard export");
+        }
+
+        json[bb_name] = ExportBlackboardToJSON(*local_bb);
+
+        if(needs_exported_root)
+        {
+          if(root_bb == exported_root)
+          {
+            continue;
+          }
+          if(exported_root != nullptr)
+          {
+            return nonstd::make_unexpected("blackboard dump request spans "
+                                           "multiple external root blackboards");
+          }
+          if(json.contains(kRootBlackboardName))
+          {
+            return nonstd::make_unexpected("blackboard dump request would "
+                                           "overwrite subtree [ROOT] with an "
+                                           "external root blackboard export");
+          }
+          json[kRootBlackboardName] = ExportBlackboardToJSON(*root_bb);
+          exported_root = root_bb;
+        }
       }
     }
   }

tests/CMakeLists.txt

@@ -108,6 +108,9 @@ endif()
 
 target_include_directories(behaviortree_cpp_test PRIVATE include)
 target_link_libraries(behaviortree_cpp_test ${BTCPP_LIBRARY} bt_sample_nodes)
+if(MSVC)
+  target_compile_options(behaviortree_cpp_test PRIVATE "/utf-8")
+endif()
 target_compile_definitions(behaviortree_cpp_test PRIVATE BT_TEST_FOLDER="${CMAKE_CURRENT_SOURCE_DIR}")
 
 # Ensure plugin is built before tests run, and tests can find it

tests/gtest_groot2_publisher_integration.cpp

@@ -2,6 +2,8 @@
 
 #include <atomic>
 #include <cstdlib>
+#include <stdexcept>
+#include <string>
 
 #include <behaviortree_cpp/bt_factory.h>
 #include <gtest/gtest.h>
@@ -45,6 +47,55 @@ void malformedRequestScenario()
   }
   std::exit(EXIT_SUCCESS);
 }
+
+BT::Tree createTreeWithNamedSubtrees(BT::BehaviorTreeFactory& factory,
+                                     const BT::Blackboard::Ptr& main_blackboard)
+{
+  const std::string xml_text = R"(
+    <root BTCPP_format="4" main_tree_to_execute="MainTree">
+       <BehaviorTree ID="MainTree">
+          <Sequence>
+            <AlwaysSuccess name="MainAction"/>
+            <SubTree ID="ChildA" name="ChildA"/>
+            <SubTree ID="ChildB" name="ChildB"/>
+          </Sequence>
+       </BehaviorTree>
+
+       <BehaviorTree ID="ChildA">
+          <AlwaysSuccess name="ChildActionA"/>
+       </BehaviorTree>
+
+       <BehaviorTree ID="ChildB">
+          <AlwaysSuccess name="ChildActionB"/>
+       </BehaviorTree>
+    </root>)";
+
+  return factory.createTreeFromText(xml_text, main_blackboard);
+}
+
+nlohmann::json requestBlackboardDump(const BT::Tree& tree, const std::string& bb_list)
+{
+  auto [publisher, port] = Groot2Test::makePublisher(tree);
+  Groot2Test::Client client(port);
+  auto reply = client.request(BT::Monitor::RequestType::BLACKBOARD, bb_list);
+  if(reply.size() != 2u)
+  {
+    throw std::runtime_error("Unexpected Groot2 blackboard reply size");
+  }
+  return nlohmann::json::from_msgpack(reply[1].to_string());
+}
+
+std::string requestBlackboardDumpError(const BT::Tree& tree, const std::string& bb_list)
+{
+  auto [publisher, port] = Groot2Test::makePublisher(tree);
+  Groot2Test::Client client(port);
+  auto reply = client.rawRequest(BT::Monitor::RequestType::BLACKBOARD, bb_list);
+  if(reply.size() != 2u || reply[0].to_string() != "error")
+  {
+    throw std::runtime_error("Expected a Groot2 blackboard error reply");
+  }
+  return reply[1].to_string();
+}
 }  // namespace
 
 TEST(Groot2PublisherIntegration, PostHookRunsAfterNodeAndCanBeRemoved)
@@ -87,3 +138,125 @@ TEST(Groot2PublisherIntegration, MalformedRequestDoesNotTerminateServer)
   ::testing::FLAGS_gtest_death_test_style = "threadsafe";
   EXPECT_EXIT(malformedRequestScenario(), ::testing::ExitedWithCode(EXIT_SUCCESS), ".*");
 }
+
+TEST(Groot2PublisherIntegration, BlackboardDump_NoRootWithoutExternalBlackboard)
+{
+  BT::BehaviorTreeFactory factory;
+  factory.registerBehaviorTreeFromText(R"(
+    <root BTCPP_format="4">
+      <BehaviorTree ID="MainTree">
+        <AlwaysSuccess/>
+      </BehaviorTree>
+    </root>)");
+
+  auto main_blackboard = BT::Blackboard::create();
+  main_blackboard->set("local_value", 7);
+  auto tree = factory.createTree("MainTree", main_blackboard);
+
+  auto json = requestBlackboardDump(tree, "MainTree");
+  ASSERT_TRUE(json.contains("MainTree"));
+  EXPECT_FALSE(json.contains("ROOT"));
+
+  ASSERT_TRUE(json["MainTree"].contains("local_value"));
+  EXPECT_EQ(json["MainTree"]["local_value"].get<int>(), 7);
+}
+
+TEST(Groot2PublisherIntegration, BlackboardDump_ExportsExternalRootBlackboard)
+{
+  BT::BehaviorTreeFactory factory;
+  factory.registerBehaviorTreeFromText(R"(
+    <root BTCPP_format="4">
+      <BehaviorTree ID="MainTree">
+        <AlwaysSuccess/>
+      </BehaviorTree>
+    </root>)");
+
+  auto external_root = BT::Blackboard::create();
+  external_root->set("shared_value", 42);
+
+  auto main_blackboard = BT::Blackboard::create(external_root);
+  main_blackboard->set("local_value", 7);
+  auto tree = factory.createTree("MainTree", main_blackboard);
+
+  auto json = requestBlackboardDump(tree, "MainTree");
+  ASSERT_TRUE(json.contains("MainTree"));
+  ASSERT_TRUE(json.contains("ROOT"));
+
+  ASSERT_TRUE(json["MainTree"].contains("local_value"));
+  EXPECT_EQ(json["MainTree"]["local_value"].get<int>(), 7);
+  EXPECT_FALSE(json["MainTree"].contains("shared_value"));
+
+  ASSERT_TRUE(json["ROOT"].contains("shared_value"));
+  EXPECT_EQ(json["ROOT"]["shared_value"].get<int>(), 42);
+  EXPECT_FALSE(json["ROOT"].contains("local_value"));
+}
+
+TEST(Groot2PublisherIntegration, BlackboardDump_DeduplicatesSharedExternalRoot)
+{
+  BT::BehaviorTreeFactory factory;
+  auto external_root = BT::Blackboard::create();
+  external_root->set("shared_value", 99);
+
+  auto main_blackboard = BT::Blackboard::create(external_root);
+  auto tree = createTreeWithNamedSubtrees(factory, main_blackboard);
+  ASSERT_EQ(tree.subtrees.size(), 3u);
+
+  auto json = requestBlackboardDump(tree, "MainTree;ChildA;ChildB");
+  ASSERT_TRUE(json.contains("MainTree"));
+  ASSERT_TRUE(json.contains("ChildA"));
+  ASSERT_TRUE(json.contains("ChildB"));
+  ASSERT_TRUE(json.contains("ROOT"));
+  EXPECT_EQ(json.size(), 4u);
+
+  ASSERT_TRUE(json["ROOT"].contains("shared_value"));
+  EXPECT_EQ(json["ROOT"]["shared_value"].get<int>(), 99);
+}
+
+TEST(Groot2PublisherIntegration, BlackboardDump_RejectsReservedRootNameCollision)
+{
+  BT::BehaviorTreeFactory factory;
+  factory.registerBehaviorTreeFromText(R"(
+    <root BTCPP_format="4">
+      <BehaviorTree ID="ROOT">
+        <AlwaysSuccess/>
+      </BehaviorTree>
+    </root>)");
+
+  auto external_root = BT::Blackboard::create();
+  external_root->set("shared_value", 42);
+
+  auto main_blackboard = BT::Blackboard::create(external_root);
+  main_blackboard->set("local_value", 7);
+  auto tree = factory.createTree("ROOT", main_blackboard);
+
+  auto error = requestBlackboardDumpError(tree, "ROOT");
+  EXPECT_NE(error.find("reserved name [ROOT]"), std::string::npos);
+}
+
+TEST(Groot2PublisherIntegration, BlackboardDump_RejectsConflictingExternalRoots)
+{
+  BT::BehaviorTreeFactory factory;
+  auto first_root = BT::Blackboard::create();
+  first_root->set("shared_value", 99);
+
+  auto main_blackboard = BT::Blackboard::create(first_root);
+  auto tree = createTreeWithNamedSubtrees(factory, main_blackboard);
+
+  auto second_root = BT::Blackboard::create();
+  second_root->set("other_shared_value", 123);
+
+  bool replaced_child_blackboard = false;
+  for(auto& subtree : tree.subtrees)
+  {
+    if(subtree->instance_name == "ChildB")
+    {
+      subtree->blackboard = BT::Blackboard::create(second_root);
+      replaced_child_blackboard = true;
+      break;
+    }
+  }
+  ASSERT_TRUE(replaced_child_blackboard);
+
+  auto error = requestBlackboardDumpError(tree, "MainTree;ChildB");
+  EXPECT_NE(error.find("multiple external root blackboards"), std::string::npos);
+}