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

@@ -33,6 +33,8 @@ struct Transition
 
 namespace
 {
+constexpr const char* kRootBlackboardName = "ROOT";
+
 std::array<char, 16> CreateRandomUUID()
 {
   std::random_device rd;
@@ -309,7 +311,13 @@ void Groot2Publisher::serverLoop()
         }
         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;
 
@@ -543,9 +551,12 @@ void Groot2Publisher::heartbeatLoop()
   }
 }
 
-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)
   {
@@ -557,7 +568,40 @@ std::vector<uint8_t> Groot2Publisher::generateBlackboardsDump(const std::string&
       // 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

@@ -97,6 +97,12 @@ 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()
+if(BTCPP_GROOT_INTERFACE)
+  target_compile_definitions(behaviortree_cpp_test PRIVATE BTCPP_GROOT_INTERFACE)
+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_loggers.cpp

@@ -13,17 +13,45 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "behaviortree_cpp/loggers/bt_cout_logger.h"
 #include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
+#ifdef BTCPP_GROOT_INTERFACE
+#include "behaviortree_cpp/loggers/groot2_protocol.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
+#endif
 #include "behaviortree_cpp/loggers/bt_minitrace_logger.h"
 #include "behaviortree_cpp/loggers/bt_sqlite_logger.h"
 
+#ifdef BTCPP_GROOT_INTERFACE
+#include "zmq_addon.hpp"
+#endif
+
 #include <cstdio>
 #include <filesystem>
 #include <fstream>
+#ifdef BTCPP_GROOT_INTERFACE
+#include <atomic>
+#include <chrono>
+#include <stdexcept>
+#include <thread>
+#endif
 
 #include <gtest/gtest.h>
 
 using namespace BT;
 
+#ifdef BTCPP_GROOT_INTERFACE
+using namespace std::chrono_literals;
+
+namespace
+{
+std::atomic_uint g_next_groot2_port{ 17670 };
+
+unsigned nextGroot2Port()
+{
+  return g_next_groot2_port.fetch_add(2);
+}
+}  // namespace
+#endif
+
 class LoggerTest : public testing::Test
 {
 protected:
@@ -56,6 +84,90 @@ class LoggerTest : public testing::Test
       </root>)";
     return factory.createTreeFromText(xml_text);
   }
+
+#ifdef BTCPP_GROOT_INTERFACE
+  BT::Tree createTreeWithNamedSubtrees(
+      const Blackboard::Ptr& main_blackboard = Blackboard::create())
+  {
+    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);
+  }
+
+  struct BlackboardDumpReply
+  {
+    std::string header;
+    std::string payload;
+
+    bool isError() const
+    {
+      return header == "error";
+    }
+  };
+
+  BlackboardDumpReply requestBlackboardDumpReply(const BT::Tree& tree, unsigned port,
+                                                 const std::string& bb_list)
+  {
+    Groot2Publisher publisher(tree, port);
+    std::this_thread::sleep_for(50ms);
+
+    zmq::context_t context(1);
+    zmq::socket_t client(context, ZMQ_REQ);
+    client.set(zmq::sockopt::linger, 0);
+    client.set(zmq::sockopt::rcvtimeo, 1000);
+    client.set(zmq::sockopt::sndtimeo, 1000);
+    client.connect(("tcp://127.0.0.1:" + std::to_string(port)).c_str());
+
+    zmq::multipart_t request;
+    request.addstr(Monitor::SerializeHeader(
+        Monitor::RequestHeader(Monitor::RequestType::BLACKBOARD)));
+    request.addstr(bb_list);
+    if(!request.send(client))
+    {
+      throw std::runtime_error("Failed to send Groot2 blackboard request");
+    }
+
+    zmq::multipart_t reply;
+    if(!reply.recv(client))
+    {
+      throw std::runtime_error("Failed to receive Groot2 blackboard reply");
+    }
+    if(reply.size() != 2u)
+    {
+      throw std::runtime_error("Unexpected Groot2 blackboard reply size");
+    }
+
+    return { reply[0].to_string(), reply[1].to_string() };
+  }
+
+  nlohmann::json requestBlackboardDump(const BT::Tree& tree, unsigned port,
+                                       const std::string& bb_list)
+  {
+    auto reply = requestBlackboardDumpReply(tree, port, bb_list);
+    if(reply.isError())
+    {
+      throw std::runtime_error("Groot2 blackboard request failed: " + reply.payload);
+    }
+    return nlohmann::json::from_msgpack(reply.payload);
+  }
+#endif
 };
 
 // ============ StdCoutLogger tests ============
@@ -494,3 +606,130 @@ TEST_F(LoggerTest, Logger_DisabledDuringExecution)
 
   ASSERT_TRUE(std::filesystem::exists(filepath));
 }
+
+#ifdef BTCPP_GROOT_INTERFACE
+TEST_F(LoggerTest, Groot2Publisher_DoesNotExportRootWithoutExternalBlackboard)
+{
+  const std::string xml_text = R"(
+    <root BTCPP_format="4">
+      <BehaviorTree ID="MainTree">
+        <AlwaysSuccess/>
+      </BehaviorTree>
+    </root>)";
+
+  auto main_blackboard = Blackboard::create();
+  main_blackboard->set("local_value", 7);
+
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("MainTree", main_blackboard);
+
+  auto json = requestBlackboardDump(tree, nextGroot2Port(), "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_F(LoggerTest, Groot2Publisher_ExportsExternalRootBlackboard)
+{
+  const std::string xml_text = R"(
+    <root BTCPP_format="4">
+      <BehaviorTree ID="MainTree">
+        <AlwaysSuccess/>
+      </BehaviorTree>
+    </root>)";
+
+  auto external_root = Blackboard::create();
+  external_root->set("shared_value", 42);
+
+  auto main_blackboard = Blackboard::create(external_root);
+  main_blackboard->set("local_value", 7);
+
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("MainTree", main_blackboard);
+
+  auto json = requestBlackboardDump(tree, nextGroot2Port(), "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_F(LoggerTest, Groot2Publisher_DeduplicatesSharedExternalRootBlackboard)
+{
+  auto external_root = Blackboard::create();
+  external_root->set("shared_value", 99);
+
+  auto main_blackboard = Blackboard::create(external_root);
+  auto tree = createTreeWithNamedSubtrees(main_blackboard);
+  ASSERT_EQ(tree.subtrees.size(), 3u);
+
+  auto json = requestBlackboardDump(tree, nextGroot2Port(), "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_F(LoggerTest, Groot2Publisher_RejectsReservedRootNameCollision)
+{
+  const std::string xml_text = R"(
+    <root BTCPP_format="4">
+      <BehaviorTree ID="ROOT">
+        <AlwaysSuccess/>
+      </BehaviorTree>
+    </root>)";
+
+  auto external_root = Blackboard::create();
+  external_root->set("shared_value", 42);
+
+  auto main_blackboard = Blackboard::create(external_root);
+  main_blackboard->set("local_value", 7);
+
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("ROOT", main_blackboard);
+
+  auto reply = requestBlackboardDumpReply(tree, nextGroot2Port(), "ROOT");
+  ASSERT_TRUE(reply.isError());
+  EXPECT_NE(reply.payload.find("reserved name [ROOT]"), std::string::npos);
+}
+
+TEST_F(LoggerTest, Groot2Publisher_RejectsConflictingExternalRootBlackboards)
+{
+  auto first_root = Blackboard::create();
+  first_root->set("shared_value", 99);
+
+  auto main_blackboard = Blackboard::create(first_root);
+  auto tree = createTreeWithNamedSubtrees(main_blackboard);
+
+  auto second_root = 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 = Blackboard::create(second_root);
+      replaced_child_blackboard = true;
+      break;
+    }
+  }
+  ASSERT_TRUE(replaced_child_blackboard);
+
+  auto reply = requestBlackboardDumpReply(tree, nextGroot2Port(), "MainTree;ChildB");
+  ASSERT_TRUE(reply.isError());
+  EXPECT_NE(reply.payload.find("multiple external root blackboards"), std::string::npos);
+}
+#endif