diff --git a/include/behaviortree_cpp/loggers/groot2_publisher.h b/include/behaviortree_cpp/loggers/groot2_publisher.h index 675d12e0e..c87ae9444 100644 --- a/include/behaviortree_cpp/loggers/groot2_publisher.h +++ b/include/behaviortree_cpp/loggers/groot2_publisher.h @@ -55,7 +55,7 @@ class Groot2Publisher : public StatusChangeLogger void updateStatusBuffer(); - std::vector generateBlackboardsDump(const std::string& bb_list); + Expected> generateBlackboardsDump(const std::string& bb_list); bool insertHook(Monitor::Hook::Ptr breakpoint); diff --git a/src/loggers/groot2_publisher.cpp b/src/loggers/groot2_publisher.cpp index 4726c1a7b..e2a13198e 100644 --- a/src/loggers/groot2_publisher.cpp +++ b/src/loggers/groot2_publisher.cpp @@ -33,6 +33,8 @@ struct Transition namespace { +constexpr const char* kRootBlackboardName = "ROOT"; + std::array 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 Groot2Publisher::generateBlackboardsDump(const std::string& bb_list) +Expected> +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 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; + } } } } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index a009ee5ea..0294bc26c 100644 --- a/tests/CMakeLists.txt +++ b/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 diff --git a/tests/gtest_loggers.cpp b/tests/gtest_loggers.cpp index 1daae3f3b..82f2c7ba4 100644 --- a/tests/gtest_loggers.cpp +++ b/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 #include #include +#ifdef BTCPP_GROOT_INTERFACE +#include +#include +#include +#include +#endif #include 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 )"; 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"( + + + + + + + + + + + + + + + + + )"; + + 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"( + + + + + )"; + + 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(), 7); +} + +TEST_F(LoggerTest, Groot2Publisher_ExportsExternalRootBlackboard) +{ + const std::string xml_text = R"( + + + + + )"; + + 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(), 7); + EXPECT_FALSE(json["MainTree"].contains("shared_value")); + + ASSERT_TRUE(json["ROOT"].contains("shared_value")); + EXPECT_EQ(json["ROOT"]["shared_value"].get(), 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(), 99); +} + +TEST_F(LoggerTest, Groot2Publisher_RejectsReservedRootNameCollision) +{ + const std::string xml_text = R"( + + + + + )"; + + 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