From bf0093b36b8982ade4e93e7637fd28a5253a2b8e Mon Sep 17 00:00:00 2001 From: kmakd Date: Mon, 3 Nov 2025 12:49:28 +0000 Subject: [PATCH 1/3] Add support for standard BT plugins --- husarion_ugv_manager/CMakeLists.txt | 1 + .../behavior_tree_utils.hpp | 27 +++++++++++ .../action/call_set_bool_service_node.hpp | 8 ++++ .../call_set_led_animation_service_node.hpp | 8 ++++ .../action/call_trigger_service_node.hpp | 8 ++++ .../plugins/condition/check_bool_msg.hpp | 6 +++ .../plugins/condition/check_joy_msg.hpp | 6 +++ .../plugins/condition/check_string_msg.hpp | 6 +++ .../src/lights_manager_node.cpp | 2 + .../action/call_set_bool_service_node.cpp | 6 +++ .../call_set_led_animation_service_node.cpp | 7 +++ .../action/call_trigger_service_node.cpp | 6 +++ .../src/plugins/condition/check_bool_msg.cpp | 6 +++ .../src/plugins/condition/check_joy_msg.cpp | 6 +++ .../plugins/condition/check_string_msg.cpp | 6 +++ .../test_call_set_bool_service_node.cpp | 14 +++++- ...st_call_set_led_animation_service_node.cpp | 16 ++++++- .../action/test_call_trigger_service_node.cpp | 14 +++++- .../plugins/condition/test_check_bool_msg.cpp | 26 ++++++++++- .../plugins/condition/test_check_joy_msg.cpp | 27 ++++++++++- .../condition/test_check_string_msg.cpp | 31 ++++++++++++- .../test/test_behavior_tree_utils.cpp | 45 +++++++++++++++++++ .../test/utils/plugin_test_utils.hpp | 5 ++- 23 files changed, 276 insertions(+), 11 deletions(-) diff --git a/husarion_ugv_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt index 7d959975f..70f60c4c0 100644 --- a/husarion_ugv_manager/CMakeLists.txt +++ b/husarion_ugv_manager/CMakeLists.txt @@ -315,6 +315,7 @@ if(BUILD_TESTING) safety_manager_parameters OpenSSL::SSL) endif() +ament_export_libraries(${plugin_libs}) ament_export_dependencies(${PACKAGE_DEPENDENCIES}) ament_export_include_directories(include/${PROJECT_NAME}) diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp index 95e6adccc..1e3b52865 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp @@ -77,6 +77,33 @@ inline void RegisterBehaviorTree( RegisterBehaviorTree(factory, bt_project_path, plugin_libs); } +/** + * @brief Creates RosNodeParams from the blackboard contained in the NodeConfig. + * + * @param conf The NodeConfig containing the blackboard. + * @return BT::RosNodeParams The created RosNodeParams. + * + * @throw std::runtime_error Throws when the blackboard is nullptr or the node + * (rclcpp::Node::SharedPtr) is not found or is nullptr. + */ +inline BT::RosNodeParams CreateRosNodeParamsFromBlackboard(const BT::NodeConfig & conf) +{ + auto blackboard = conf.blackboard; + if (!blackboard) { + throw std::runtime_error("Blackboard is nullptr"); + } + + rclcpp::Node::SharedPtr node; + + try { + node = blackboard->get("node"); + } catch (const std::exception & e) { + throw BT::RuntimeError("Failed to get rclcpp::Node from blackboard: ", e.what()); + } + + return BT::RosNodeParams(node); +} + } // namespace husarion_ugv_manager::behavior_tree_utils namespace husarion_ugv_manager diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp index 9ce38f00e..730a7e091 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp @@ -22,12 +22,20 @@ #include "std_srvs/srv/set_bool.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" + namespace husarion_ugv_manager { class CallSetBoolService : public BT::RosServiceNode { public: + CallSetBoolService(const std::string & name, const BT::NodeConfig & conf) + : BT::RosServiceNode( + name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf)) + { + } + CallSetBoolService( const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) : BT::RosServiceNode(name, conf, params) diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp index bd6115fbf..30b76a7ce 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp @@ -22,6 +22,8 @@ #include "husarion_ugv_msgs/srv/set_led_animation.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" + namespace husarion_ugv_manager { @@ -29,6 +31,12 @@ class CallSetLedAnimationService : public BT::RosServiceNode { public: + CallSetLedAnimationService(const std::string & name, const BT::NodeConfig & conf) + : BT::RosServiceNode( + name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf)) + { + } + CallSetLedAnimationService( const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) : BT::RosServiceNode(name, conf, params) diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp index 8dce1582c..0c46a9de8 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_trigger_service_node.hpp @@ -22,12 +22,20 @@ #include "std_srvs/srv/trigger.hpp" +#include "husarion_ugv_manager/behavior_tree_utils.hpp" + namespace husarion_ugv_manager { class CallTriggerService : public BT::RosServiceNode { public: + CallTriggerService(const std::string & name, const BT::NodeConfig & conf) + : BT::RosServiceNode( + name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf)) + { + } + CallTriggerService( const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) : BT::RosServiceNode(name, conf, params) diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp index d99e6dd4c..95c686ffc 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp @@ -35,6 +35,12 @@ class CheckBoolMsg : public BT::RosTopicSubNode using BoolMsg = std_msgs::msg::Bool; public: + CheckBoolMsg(const std::string & name, const BT::NodeConfig & conf) + : BT::RosTopicSubNode( + name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf)) + { + } + CheckBoolMsg( const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) : BT::RosTopicSubNode(name, conf, params) diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp index c288cffa1..4b071a06f 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp @@ -34,6 +34,12 @@ class CheckJoyMsg : public BT::RosTopicSubNode using JoyMsg = sensor_msgs::msg::Joy; public: + CheckJoyMsg(const std::string & name, const BT::NodeConfig & conf) + : BT::RosTopicSubNode( + name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf)) + { + } + CheckJoyMsg( const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) : BT::RosTopicSubNode(name, conf, params) diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp index 074dc0b51..4f9e70cea 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp @@ -34,6 +34,12 @@ class CheckStringMsg : public BT::RosTopicSubNode using StringMsg = std_msgs::msg::String; public: + CheckStringMsg(const std::string & name, const BT::NodeConfig & conf) + : BT::RosTopicSubNode( + name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf)) + { + } + CheckStringMsg( const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) : BT::RosTopicSubNode(name, conf, params) diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp index c5e9d0f1d..82a857d69 100644 --- a/husarion_ugv_manager/src/lights_manager_node.cpp +++ b/husarion_ugv_manager/src/lights_manager_node.cpp @@ -143,6 +143,8 @@ std::map LightsManagerNode::CreateLightsInitialBlackboard {"GOAL_ACHIEVED_ANIM_ID", unsigned(LEDAnimationMsg::GOAL_ACHIEVED)}, {"BLINKER_LEFT_ANIM_ID", unsigned(LEDAnimationMsg::BLINKER_LEFT)}, {"BLINKER_RIGHT_ANIM_ID", unsigned(LEDAnimationMsg::BLINKER_RIGHT)}, + {"GOAL_FAILED_ANIM_ID", unsigned(LEDAnimationMsg::GOAL_FAILED)}, + {"FLOOD_LIGHT_ANIM_ID", unsigned(LEDAnimationMsg::FLOOD_LIGHT)}, // battery status constants {"POWER_SUPPLY_STATUS_UNKNOWN", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN)}, {"POWER_SUPPLY_STATUS_CHARGING", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING)}, diff --git a/husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp b/husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp index b51b58b80..21e2b8148 100644 --- a/husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/call_set_bool_service_node.cpp @@ -46,3 +46,9 @@ BT::NodeStatus CallSetBoolService::onResponseReceived(const typename Response::S #include "behaviortree_ros2/plugins.hpp" CreateRosNodePlugin(husarion_ugv_manager::CallSetBoolService, "CallSetBoolService"); + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("CallSetBoolService"); +} diff --git a/husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp b/husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp index 70c076429..1594cc38e 100644 --- a/husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/call_set_led_animation_service_node.cpp @@ -66,3 +66,10 @@ BT::NodeStatus CallSetLedAnimationService::onResponseReceived( #include "behaviortree_ros2/plugins.hpp" CreateRosNodePlugin(husarion_ugv_manager::CallSetLedAnimationService, "CallSetLedAnimationService"); + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType( + "CallSetLedAnimationService"); +} diff --git a/husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp b/husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp index 73c70856d..4982d75f9 100644 --- a/husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp +++ b/husarion_ugv_manager/src/plugins/action/call_trigger_service_node.cpp @@ -41,3 +41,9 @@ BT::NodeStatus CallTriggerService::onResponseReceived(const typename Response::S #include "behaviortree_ros2/plugins.hpp" CreateRosNodePlugin(husarion_ugv_manager::CallTriggerService, "CallTriggerService"); + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("CallTriggerService"); +} diff --git a/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp index 892d03810..17bebf3ad 100644 --- a/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp +++ b/husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp @@ -32,3 +32,9 @@ BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg) #include "behaviortree_ros2/plugins.hpp" CreateRosNodePlugin(husarion_ugv_manager::CheckBoolMsg, "CheckBoolMsg"); + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("CheckBoolMsg"); +} diff --git a/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp index 8d28cbe1e..a4d4672f6 100644 --- a/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp +++ b/husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp @@ -90,3 +90,9 @@ bool CheckJoyMsg::checkTimeout(const JoyMsg::SharedPtr & last_msg) #include "behaviortree_ros2/plugins.hpp" CreateRosNodePlugin(husarion_ugv_manager::CheckJoyMsg, "CheckJoyMsg"); + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("CheckJoyMsg"); +} diff --git a/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp index 7bafdbdd8..1777d09b3 100644 --- a/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp +++ b/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp @@ -40,3 +40,9 @@ bool CheckStringMsg::latchLastMessage() const { return true; } #include "behaviortree_ros2/plugins.hpp" CreateRosNodePlugin(husarion_ugv_manager::CheckStringMsg, "CheckStringMsg"); + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("CheckStringMsg"); +} diff --git a/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp index d8ec8358f..3d15f3bc5 100644 --- a/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp @@ -43,7 +43,7 @@ void TestCallSetBoolService::ServiceCallback( rclcpp::get_logger("test_set_bool_plugin"), response->message << " data: " << request->data); } -TEST_F(TestCallSetBoolService, GoodLoadingCallSetBoolServicePlugin) +TEST_F(TestCallSetBoolService, GoodLoadingCallSetBoolServiceRosPlugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; @@ -52,6 +52,18 @@ TEST_F(TestCallSetBoolService, GoodLoadingCallSetBoolServicePlugin) ASSERT_NO_THROW({ CreateTree("CallSetBoolService", service); }); } +TEST_F(TestCallSetBoolService, GoodLoadingCallSetBoolServicePlugin) +{ + std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; + + RegisterNodeWithoutParams("CallSetBoolService"); + + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", this->bt_node_); + + ASSERT_NO_THROW({ CreateTree("CallSetBoolService", service, blackboard); }); +} + TEST_F(TestCallSetBoolService, WrongPluginNameLoadingCallSetBoolServicePlugin) { std::map service = {{"service_name", "set_bool"}, {"data", "true"}}; diff --git a/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp index 5c8ed3b4f..fa9b51755 100644 --- a/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp @@ -50,7 +50,7 @@ void TestCallSetLedAnimationService::ServiceCallback( EXPECT_EQ(request->repeating, repeating); } -TEST_F(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServicePlugin) +TEST_F(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServiceRosPlugin) { std::map service = { {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; @@ -61,6 +61,20 @@ TEST_F(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServicePlug ASSERT_NO_THROW({ CreateTree("CallSetLedAnimationService", service); }); } +TEST_F(TestCallSetLedAnimationService, GoodLoadingCallSetLedAnimationServicePlugin) +{ + std::map service = { + {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}}; + + RegisterNodeWithoutParams( + "CallSetLedAnimationService"); + + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", this->bt_node_); + + ASSERT_NO_THROW({ CreateTree("CallSetLedAnimationService", service, blackboard); }); +} + TEST_F(TestCallSetLedAnimationService, WrongPluginNameLoadingCallSetLedAnimationServicePlugin) { std::map service = { diff --git a/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp b/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp index 5acbb291c..16c32ffc3 100644 --- a/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp +++ b/husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp @@ -41,7 +41,7 @@ void TestCallTriggerService::ServiceCallback( RCLCPP_INFO_STREAM(rclcpp::get_logger("test_trigger_plugin"), response->message); } -TEST_F(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin) +TEST_F(TestCallTriggerService, GoodLoadingCallTriggerServiceRosPlugin) { std::map service = {{"service_name", "trigger"}}; @@ -50,6 +50,18 @@ TEST_F(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin) ASSERT_NO_THROW({ CreateTree("CallTriggerService", service); }); } +TEST_F(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin) +{ + std::map service = {{"service_name", "trigger"}}; + + RegisterNodeWithoutParams("CallTriggerService"); + + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", this->bt_node_); + + ASSERT_NO_THROW({ CreateTree("CallTriggerService", service, blackboard); }); +} + TEST_F(TestCallTriggerService, WrongPluginNameLoadingCallTriggerServicePlugin) { std::map service = {{"service_name", "trigger"}}; diff --git a/husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp b/husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp index 13f7d24dc..2a3f8f195 100644 --- a/husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp +++ b/husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp @@ -44,7 +44,9 @@ constexpr auto PLUGIN = "CheckBoolMsg"; class TestCheckBoolMsg : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils { public: - TestCheckBoolMsg(); + TestCheckBoolMsg() {} + + void Initialize(); BoolMsg CreateMsg(bool data); void PublishMsg(BoolMsg msg) { publisher_->publish(msg); } @@ -52,7 +54,7 @@ class TestCheckBoolMsg : public husarion_ugv_manager::plugin_test_utils::PluginT rclcpp::Publisher::SharedPtr publisher_; }; -TestCheckBoolMsg::TestCheckBoolMsg() +void TestCheckBoolMsg::Initialize() { RegisterNodeWithParams(PLUGIN); publisher_ = bt_node_->create_publisher(TOPIC, 10); @@ -65,8 +67,27 @@ BoolMsg TestCheckBoolMsg::CreateMsg(bool data) return msg; } +TEST_F(TestCheckBoolMsg, GoodLoadingCheckBoolMsgRosPlugin) +{ + this->Initialize(); + bt_ports input = {{"topic_name", TOPIC}, {"data", "true"}}; + ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); +} + +TEST_F(TestCheckBoolMsg, GoodLoadingCheckBoolMsgPlugin) +{ + bt_ports input = {{"topic_name", TOPIC}, {"data", "true"}}; + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", this->bt_node_); + + RegisterNodeWithoutParams(PLUGIN); + + ASSERT_NO_THROW({ CreateTree(PLUGIN, input, blackboard); }); +} + TEST_F(TestCheckBoolMsg, NoMessageArrived) { + this->Initialize(); bt_ports input = {{"topic_name", TOPIC}, {"data", "true"}}; ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); @@ -77,6 +98,7 @@ TEST_F(TestCheckBoolMsg, NoMessageArrived) TEST_F(TestCheckBoolMsg, OnTickBehavior) { + this->Initialize(); std::vector test_cases = { {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"data", "true"}}, CreateMsg(true)}, {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"data", "false"}}, CreateMsg(false)}, diff --git a/husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp b/husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp index d2f29b5fd..acbc4fdab 100644 --- a/husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp +++ b/husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp @@ -46,7 +46,9 @@ constexpr auto PLUGIN = "CheckJoyMsg"; class TestCheckJoyMsg : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils { public: - TestCheckJoyMsg(); + TestCheckJoyMsg() {} + + void Initialize(); void PublishMsg(JoyMsg msg) { joy_publisher_->publish(msg); }; JoyMsg CreateMsg( const std::vector & axes = {}, const std::vector & buttons = {}, @@ -57,7 +59,7 @@ class TestCheckJoyMsg : public husarion_ugv_manager::plugin_test_utils::PluginTe rclcpp::Publisher::SharedPtr joy_publisher_; }; -TestCheckJoyMsg::TestCheckJoyMsg() +void TestCheckJoyMsg::Initialize() { RegisterNodeWithParams(PLUGIN); joy_publisher_ = bt_node_->create_publisher(TOPIC, 10); @@ -75,8 +77,27 @@ JoyMsg TestCheckJoyMsg::CreateMsg( void TestCheckJoyMsg::SetCurrentMsgTime(JoyMsg & msg) { msg.header.stamp = bt_node_->now(); } +TEST_F(TestCheckJoyMsg, GoodLoadingCheckJoyMsgRosPlugin) +{ + this->Initialize(); + bt_ports input = {{"topic_name", TOPIC}}; + ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); +} + +TEST_F(TestCheckJoyMsg, GoodLoadingCheckJoyMsgPlugin) +{ + bt_ports input = {{"topic_name", TOPIC}}; + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", this->bt_node_); + + RegisterNodeWithoutParams(PLUGIN); + + ASSERT_NO_THROW({ CreateTree(PLUGIN, input, blackboard); }); +} + TEST_F(TestCheckJoyMsg, NoMessageArrived) { + this->Initialize(); bt_ports input = {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}; ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); @@ -87,6 +108,7 @@ TEST_F(TestCheckJoyMsg, NoMessageArrived) TEST_F(TestCheckJoyMsg, TimeoutTests) { + this->Initialize(); std::vector test_cases = { {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "0.5"}}, @@ -117,6 +139,7 @@ TEST_F(TestCheckJoyMsg, TimeoutTests) TEST_F(TestCheckJoyMsg, OnTickBehavior) { + this->Initialize(); std::vector test_cases = { {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "1.0"}}, diff --git a/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp b/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp index 666252791..2091e42f3 100644 --- a/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp +++ b/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp @@ -44,7 +44,9 @@ constexpr auto PLUGIN = "CheckStringMsg"; class TestCheckStringMsg : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils { public: - TestCheckStringMsg(); + TestCheckStringMsg() {} + + void Initialize(); StringMsg CreateMsg(const std::string & data); void PublishMsg(StringMsg msg) { publisher_->publish(msg); } @@ -52,7 +54,7 @@ class TestCheckStringMsg : public husarion_ugv_manager::plugin_test_utils::Plugi rclcpp::Publisher::SharedPtr publisher_; }; -TestCheckStringMsg::TestCheckStringMsg() +void TestCheckStringMsg::Initialize() { RegisterNodeWithParams(PLUGIN); publisher_ = bt_node_->create_publisher(TOPIC, 10); @@ -65,8 +67,27 @@ StringMsg TestCheckStringMsg::CreateMsg(const std::string & data) return msg; } +TEST_F(TestCheckStringMsg, GoodLoadingCheckStringMsgRosPlugin) +{ + this->Initialize(); + bt_ports input = {{"topic_name", TOPIC}, {"data", "true"}}; + ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); +} + +TEST_F(TestCheckStringMsg, GoodLoadingCheckStringMsgPlugin) +{ + bt_ports input = {{"topic_name", TOPIC}, {"data", "true"}}; + auto blackboard = BT::Blackboard::create(); + blackboard->set("node", this->bt_node_); + + RegisterNodeWithoutParams(PLUGIN); + + ASSERT_NO_THROW({ CreateTree(PLUGIN, input, blackboard); }); +} + TEST_F(TestCheckStringMsg, NoInputData) { + this->Initialize(); bt_ports input = {{"topic_name", TOPIC}}; ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); @@ -82,6 +103,7 @@ TEST_F(TestCheckStringMsg, NoInputData) TEST_F(TestCheckStringMsg, NoMessageArrived) { + this->Initialize(); bt_ports input = {{"topic_name", TOPIC}, {"data", "name"}}; ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); @@ -92,6 +114,7 @@ TEST_F(TestCheckStringMsg, NoMessageArrived) TEST_F(TestCheckStringMsg, SuccessOnExactMatch) { + this->Initialize(); bt_ports input = {{"topic_name", TOPIC}, {"data", "name"}}; CreateTree(PLUGIN, input); auto & tree = GetTree(); @@ -105,6 +128,7 @@ TEST_F(TestCheckStringMsg, SuccessOnExactMatch) TEST_F(TestCheckStringMsg, SuccessOnMatchWithSpaces) { + this->Initialize(); bt_ports input = {{"topic_name", TOPIC}, {"data", "name with spaces"}}; CreateTree(PLUGIN, input); auto & tree = GetTree(); @@ -118,6 +142,7 @@ TEST_F(TestCheckStringMsg, SuccessOnMatchWithSpaces) TEST_F(TestCheckStringMsg, SuccessOnMatchWithDots) { + this->Initialize(); bt_ports input = {{"topic_name", TOPIC}, {"data", "name.with.dots"}}; CreateTree(PLUGIN, input); auto & tree = GetTree(); @@ -131,6 +156,7 @@ TEST_F(TestCheckStringMsg, SuccessOnMatchWithDots) TEST_F(TestCheckStringMsg, SuccessOnMatchWithMixedLetters) { + this->Initialize(); bt_ports input = {{"topic_name", TOPIC}, {"data", "MiXeDlEtTeRs"}}; CreateTree(PLUGIN, input); auto & tree = GetTree(); @@ -144,6 +170,7 @@ TEST_F(TestCheckStringMsg, SuccessOnMatchWithMixedLetters) TEST_F(TestCheckStringMsg, SuccessWhenValueChanges) { + this->Initialize(); bt_ports input = {{"topic_name", TOPIC}, {"data", "name"}}; CreateTree(PLUGIN, input); auto & tree = GetTree(); diff --git a/husarion_ugv_manager/test/test_behavior_tree_utils.cpp b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp index de95d2316..2b299dd4d 100644 --- a/husarion_ugv_manager/test/test_behavior_tree_utils.cpp +++ b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp @@ -123,6 +123,51 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeROS) rclcpp::shutdown(); } +TEST(TestCreateRosNodeParamsFromBlackboard, EmptyBlackboard) +{ + BT::NodeConfiguration config; + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { husarion_ugv_manager::behavior_tree_utils::CreateRosNodeParamsFromBlackboard(config); }, + "Blackboard is nullptr")); +} + +TEST(TestCreateRosNodeParamsFromBlackboard, MissingKeyNode) +{ + BT::NodeConfiguration config; + config.blackboard = BT::Blackboard::create(); + + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { husarion_ugv_manager::behavior_tree_utils::CreateRosNodeParamsFromBlackboard(config); }, + "Failed to get rclcpp::Node")); +} + +TEST(TestCreateRosNodeParamsFromBlackboard, NodeNullPtr) +{ + BT::NodeConfiguration config; + config.blackboard = BT::Blackboard::create(); + config.blackboard->set("node", nullptr); + + EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( + [&]() { husarion_ugv_manager::behavior_tree_utils::CreateRosNodeParamsFromBlackboard(config); }, + "Failed to get rclcpp::Node")); +} + +TEST(TestCreateRosNodeParamsFromBlackboard, GoodInput) +{ + rclcpp::init(0, nullptr); + BT::NodeConfiguration config; + config.blackboard = BT::Blackboard::create(); + auto node = std::make_shared("test_node"); + config.blackboard->set("node", node); + + auto params = + husarion_ugv_manager::behavior_tree_utils::CreateRosNodeParamsFromBlackboard(config); + + EXPECT_FALSE(params.nh.expired()); + + rclcpp::shutdown(); +} + TEST(TestConvertFromStringPoseStamped, GoodInput) { constexpr double time_threshold = 0.1; diff --git a/husarion_ugv_manager/test/utils/plugin_test_utils.hpp b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp index 73fd52ca3..fac41d8bd 100644 --- a/husarion_ugv_manager/test/utils/plugin_test_utils.hpp +++ b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp @@ -181,10 +181,11 @@ class PluginTestUtils : public testing::Test } void CreateTree( - const std::string & plugin_name, const std::map & bb_ports) + const std::string & plugin_name, const std::map & bb_ports, + BT::Blackboard::Ptr blackboard = BT::Blackboard::create()) { auto xml_text = BuildBehaviorTree(plugin_name, bb_ports); - tree_ = factory_.createTreeFromText(xml_text); + tree_ = factory_.createTreeFromText(xml_text, blackboard); } inline BT::Tree & GetTree() { return tree_; } From 0e417da517327527dac4fad33b406fcbfd7d288c Mon Sep 17 00:00:00 2001 From: kmakd Date: Mon, 3 Nov 2025 13:36:24 +0000 Subject: [PATCH 2/3] pre-commit --- husarion_ugv_manager/test/utils/plugin_test_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/husarion_ugv_manager/test/utils/plugin_test_utils.hpp b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp index fac41d8bd..bdc8b0a1f 100644 --- a/husarion_ugv_manager/test/utils/plugin_test_utils.hpp +++ b/husarion_ugv_manager/test/utils/plugin_test_utils.hpp @@ -185,7 +185,7 @@ class PluginTestUtils : public testing::Test BT::Blackboard::Ptr blackboard = BT::Blackboard::create()) { auto xml_text = BuildBehaviorTree(plugin_name, bb_ports); - tree_ = factory_.createTreeFromText(xml_text, blackboard); + tree_ = factory_.createTreeFromText(xml_text, blackboard); } inline BT::Tree & GetTree() { return tree_; } From 69749ef1c4da35042428ae0bd4aff806fcafc719 Mon Sep 17 00:00:00 2001 From: kmakd Date: Tue, 4 Nov 2025 08:29:21 +0000 Subject: [PATCH 3/3] fix nullptr check --- .../include/husarion_ugv_manager/behavior_tree_utils.hpp | 4 ++++ husarion_ugv_manager/test/test_behavior_tree_utils.cpp | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp index 1e3b52865..30c09ab6f 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp @@ -101,6 +101,10 @@ inline BT::RosNodeParams CreateRosNodeParamsFromBlackboard(const BT::NodeConfig throw BT::RuntimeError("Failed to get rclcpp::Node from blackboard: ", e.what()); } + if (!node) { + throw BT::RuntimeError("rclcpp::Node is nullptr"); + } + return BT::RosNodeParams(node); } diff --git a/husarion_ugv_manager/test/test_behavior_tree_utils.cpp b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp index 2b299dd4d..dd41ca336 100644 --- a/husarion_ugv_manager/test/test_behavior_tree_utils.cpp +++ b/husarion_ugv_manager/test/test_behavior_tree_utils.cpp @@ -145,11 +145,11 @@ TEST(TestCreateRosNodeParamsFromBlackboard, NodeNullPtr) { BT::NodeConfiguration config; config.blackboard = BT::Blackboard::create(); - config.blackboard->set("node", nullptr); + config.blackboard->set("node", rclcpp::Node::SharedPtr()); EXPECT_TRUE(husarion_ugv_utils::test_utils::IsMessageThrown( [&]() { husarion_ugv_manager::behavior_tree_utils::CreateRosNodeParamsFromBlackboard(config); }, - "Failed to get rclcpp::Node")); + "rclcpp::Node is nullptr")); } TEST(TestCreateRosNodeParamsFromBlackboard, GoodInput)