Standard bt plugins support

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})
 

husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp

@@ -77,6 +77,37 @@ 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<rclcpp::Node::SharedPtr>("node");
+  } catch (const std::exception & e) {
+    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);
+}
+
 }  // namespace husarion_ugv_manager::behavior_tree_utils
 
 namespace husarion_ugv_manager

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<std_srvs::srv::SetBool>
 {
 public:
+  CallSetBoolService(const std::string & name, const BT::NodeConfig & conf)
+  : BT::RosServiceNode<std_srvs::srv::SetBool>(
+      name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf))
+  {
+  }
+
   CallSetBoolService(
     const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
   : BT::RosServiceNode<std_srvs::srv::SetBool>(name, conf, params)

husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp

@@ -22,13 +22,21 @@
 
 #include "husarion_ugv_msgs/srv/set_led_animation.hpp"
 
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
+
 namespace husarion_ugv_manager
 {
 
 class CallSetLedAnimationService
 : public BT::RosServiceNode<husarion_ugv_msgs::srv::SetLEDAnimation>
 {
 public:
+  CallSetLedAnimationService(const std::string & name, const BT::NodeConfig & conf)
+  : BT::RosServiceNode<husarion_ugv_msgs::srv::SetLEDAnimation>(
+      name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf))
+  {
+  }
+
   CallSetLedAnimationService(
     const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
   : BT::RosServiceNode<husarion_ugv_msgs::srv::SetLEDAnimation>(name, conf, params)

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<std_srvs::srv::Trigger>
 {
 public:
+  CallTriggerService(const std::string & name, const BT::NodeConfig & conf)
+  : BT::RosServiceNode<std_srvs::srv::Trigger>(
+      name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf))
+  {
+  }
+
   CallTriggerService(
     const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
   : BT::RosServiceNode<std_srvs::srv::Trigger>(name, conf, params)

husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp

@@ -35,6 +35,12 @@ class CheckBoolMsg : public BT::RosTopicSubNode<std_msgs::msg::Bool>
   using BoolMsg = std_msgs::msg::Bool;
 
 public:
+  CheckBoolMsg(const std::string & name, const BT::NodeConfig & conf)
+  : BT::RosTopicSubNode<BoolMsg>(
+      name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf))
+  {
+  }
+
   CheckBoolMsg(
     const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
   : BT::RosTopicSubNode<BoolMsg>(name, conf, params)

husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp

@@ -34,6 +34,12 @@ class CheckJoyMsg : public BT::RosTopicSubNode<sensor_msgs::msg::Joy>
   using JoyMsg = sensor_msgs::msg::Joy;
 
 public:
+  CheckJoyMsg(const std::string & name, const BT::NodeConfig & conf)
+  : BT::RosTopicSubNode<JoyMsg>(
+      name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf))
+  {
+  }
+
   CheckJoyMsg(
     const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
   : BT::RosTopicSubNode<JoyMsg>(name, conf, params)

husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp

@@ -34,6 +34,12 @@ class CheckStringMsg : public BT::RosTopicSubNode<std_msgs::msg::String>
   using StringMsg = std_msgs::msg::String;
 
 public:
+  CheckStringMsg(const std::string & name, const BT::NodeConfig & conf)
+  : BT::RosTopicSubNode<StringMsg>(
+      name, conf, behavior_tree_utils::CreateRosNodeParamsFromBlackboard(conf))
+  {
+  }
+
   CheckStringMsg(
     const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
   : BT::RosTopicSubNode<StringMsg>(name, conf, params)

husarion_ugv_manager/src/lights_manager_node.cpp

@@ -143,6 +143,8 @@ std::map<std::string, std::any> 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)},

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<husarion_ugv_manager::CallSetBoolService>("CallSetBoolService");
+}

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<husarion_ugv_manager::CallSetLedAnimationService>(
+    "CallSetLedAnimationService");
+}

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<husarion_ugv_manager::CallTriggerService>("CallTriggerService");
+}

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<husarion_ugv_manager::CheckBoolMsg>("CheckBoolMsg");
+}

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<husarion_ugv_manager::CheckJoyMsg>("CheckJoyMsg");
+}

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<husarion_ugv_manager::CheckStringMsg>("CheckStringMsg");
+}

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<std::string, std::string> 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<std::string, std::string> service = {{"service_name", "set_bool"}, {"data", "true"}};
+
+  RegisterNodeWithoutParams<husarion_ugv_manager::CallSetBoolService>("CallSetBoolService");
+
+  auto blackboard = BT::Blackboard::create();
+  blackboard->set<rclcpp::Node::SharedPtr>("node", this->bt_node_);
+
+  ASSERT_NO_THROW({ CreateTree("CallSetBoolService", service, blackboard); });
+}
+
 TEST_F(TestCallSetBoolService, WrongPluginNameLoadingCallSetBoolServicePlugin)
 {
   std::map<std::string, std::string> service = {{"service_name", "set_bool"}, {"data", "true"}};

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<std::string, std::string> 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<std::string, std::string> service = {
+    {"service_name", "set_led_animation"}, {"id", "0"}, {"param", ""}, {"repeating", "true"}};
+
+  RegisterNodeWithoutParams<husarion_ugv_manager::CallSetLedAnimationService>(
+    "CallSetLedAnimationService");
+
+  auto blackboard = BT::Blackboard::create();
+  blackboard->set<rclcpp::Node::SharedPtr>("node", this->bt_node_);
+
+  ASSERT_NO_THROW({ CreateTree("CallSetLedAnimationService", service, blackboard); });
+}
+
 TEST_F(TestCallSetLedAnimationService, WrongPluginNameLoadingCallSetLedAnimationServicePlugin)
 {
   std::map<std::string, std::string> service = {

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<std::string, std::string> service = {{"service_name", "trigger"}};
 
@@ -50,6 +50,18 @@ TEST_F(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin)
   ASSERT_NO_THROW({ CreateTree("CallTriggerService", service); });
 }
 
+TEST_F(TestCallTriggerService, GoodLoadingCallTriggerServicePlugin)
+{
+  std::map<std::string, std::string> service = {{"service_name", "trigger"}};
+
+  RegisterNodeWithoutParams<husarion_ugv_manager::CallTriggerService>("CallTriggerService");
+
+  auto blackboard = BT::Blackboard::create();
+  blackboard->set<rclcpp::Node::SharedPtr>("node", this->bt_node_);
+
+  ASSERT_NO_THROW({ CreateTree("CallTriggerService", service, blackboard); });
+}
+
 TEST_F(TestCallTriggerService, WrongPluginNameLoadingCallTriggerServicePlugin)
 {
   std::map<std::string, std::string> service = {{"service_name", "trigger"}};

husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp

@@ -44,15 +44,17 @@ 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); }
 
 protected:
   rclcpp::Publisher<BoolMsg>::SharedPtr publisher_;
 };
 
-TestCheckBoolMsg::TestCheckBoolMsg()
+void TestCheckBoolMsg::Initialize()
 {
   RegisterNodeWithParams<husarion_ugv_manager::CheckBoolMsg>(PLUGIN);
   publisher_ = bt_node_->create_publisher<BoolMsg>(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<rclcpp::Node::SharedPtr>("node", this->bt_node_);
+
+  RegisterNodeWithoutParams<husarion_ugv_manager::CheckBoolMsg>(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<TestCase> test_cases = {
     {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"data", "true"}}, CreateMsg(true)},
     {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"data", "false"}}, CreateMsg(false)},