Ros2 bt manager to hpp

README.md

@@ -133,3 +133,10 @@ This project uses pre-commit to maintain high quality of the source code. Instal
 ```bash
 pre-commit install
 ```
+
+### Unit testing
+
+```bash
+colcon build --symlink-install --packages-up-to husarion_ugv --cmake-args -DCMAKE_BUILD_TYPE=Release -DTEST_INTEGRATION=OFF
+colcon test --packages-up-to husarion_ugv
+```

husarion_ugv_manager/CMakeLists.txt

@@ -61,9 +61,8 @@ foreach(bt_plugin ${plugin_libs})
   ament_target_dependencies(${bt_plugin} ${PACKAGE_DEPENDENCIES})
 endforeach()
 
-add_executable(
-  safety_manager_node src/safety_manager_node_main.cpp
-                      src/safety_manager_node.cpp src/behavior_tree_manager.cpp)
+add_executable(safety_manager_node src/safety_manager_node_main.cpp
+                                   src/safety_manager_node.cpp)
 ament_target_dependencies(
   safety_manager_node
   behaviortree_ros2
@@ -78,9 +77,8 @@ generate_parameter_library(safety_manager_parameters
 target_link_libraries(safety_manager_node ${plugin_libs}
                       safety_manager_parameters)
 
-add_executable(
-  lights_manager_node src/lights_manager_node_main.cpp
-                      src/lights_manager_node.cpp src/behavior_tree_manager.cpp)
+add_executable(lights_manager_node src/lights_manager_node_main.cpp
+                                   src/lights_manager_node.cpp)
 ament_target_dependencies(
   lights_manager_node
   behaviortree_ros2
@@ -133,12 +131,6 @@ if(BUILD_TESTING)
     src/plugins/action/signal_shutdown_node.cpp)
   list(APPEND plugin_tests ${PROJECT_NAME}_test_signal_shutdown_node)
 
-  ament_add_gtest(
-    ${PROJECT_NAME}_test_shutdown_single_host_node
-    test/plugins/action/test_shutdown_single_host_node.cpp
-    src/plugins/action/shutdown_single_host_node.cpp)
-  list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_single_host_node)
-
   ament_add_gtest(
     ${PROJECT_NAME}_test_shutdown_hosts_from_file_node
     test/plugins/action/test_shutdown_hosts_from_file_node.cpp
@@ -159,6 +151,15 @@ if(BUILD_TESTING)
                   test/plugins/test_shutdown_hosts_node.cpp)
   list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_hosts_node)
 
+  option(TEST_INTEGRATION "Run integration tests" ON)
+  if(TEST_INTEGRATION)
+    ament_add_gtest(
+      ${PROJECT_NAME}_test_shutdown_single_host_node
+      test/plugins/action/test_shutdown_single_host_node.cpp
+      src/plugins/action/shutdown_single_host_node.cpp)
+    list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_single_host_node)
+  endif()
+
   foreach(bt_node_test ${plugin_tests})
     target_include_directories(
       ${bt_node_test}
@@ -180,19 +181,17 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_behavior_tree_utils behaviortree_cpp behaviortree_ros2
     husarion_ugv_utils)
 
-  ament_add_gtest(
-    ${PROJECT_NAME}_test_behavior_tree_manager
-    test/test_behavior_tree_manager.cpp src/behavior_tree_manager.cpp)
+  ament_add_gtest(${PROJECT_NAME}_test_behavior_tree_manager
+                  test/test_behavior_tree_manager.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_behavior_tree_manager
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(${PROJECT_NAME}_test_behavior_tree_manager
                             behaviortree_cpp husarion_ugv_utils)
 
-  ament_add_gtest(
-    ${PROJECT_NAME}_test_lights_manager_node test/test_lights_manager_node.cpp
-    src/lights_manager_node.cpp src/behavior_tree_manager.cpp)
+  ament_add_gtest(${PROJECT_NAME}_test_lights_manager_node
+                  test/test_lights_manager_node.cpp src/lights_manager_node.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_lights_manager_node
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -211,8 +210,7 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_lights_behavior_tree
-    test/test_lights_behavior_tree.cpp src/lights_manager_node.cpp
-    src/behavior_tree_manager.cpp)
+    test/test_lights_behavior_tree.cpp src/lights_manager_node.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_lights_behavior_tree
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>
@@ -229,9 +227,8 @@ if(BUILD_TESTING)
   target_link_libraries(${PROJECT_NAME}_test_lights_behavior_tree
                         lights_manager_parameters)
 
-  ament_add_gtest(
-    ${PROJECT_NAME}_test_safety_manager_node test/test_safety_manager_node.cpp
-    src/safety_manager_node.cpp src/behavior_tree_manager.cpp)
+  ament_add_gtest(${PROJECT_NAME}_test_safety_manager_node
+                  test/test_safety_manager_node.cpp src/safety_manager_node.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_safety_manager_node
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -250,8 +247,7 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_safety_behavior_tree
-    test/test_safety_behavior_tree.cpp src/safety_manager_node.cpp
-    src/behavior_tree_manager.cpp)
+    test/test_safety_behavior_tree.cpp src/safety_manager_node.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_safety_behavior_tree
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test>

husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_manager.hpp

@@ -58,7 +58,12 @@ class BehaviorTreeManager
    *
    * @param factory The factory object used to create the tree.
    */
-  void Initialize(BT::BehaviorTreeFactory & factory);
+  inline void Initialize(BT::BehaviorTreeFactory & factory)
+  {
+    config_ = CreateBTConfig(initial_blackboard_);
+    tree_ = factory.createTree(tree_name_, config_.blackboard);
+    groot_publisher_ = std::make_unique<BT::Groot2Publisher>(tree_, groot_port_);
+  }
 
   void TickOnce() { tree_status_ = tree_.tickOnce(); }
   void TickExactlyOnce() { tree_status_ = tree_.tickExactlyOnce(); }
@@ -80,7 +85,36 @@ class BehaviorTreeManager
    * entry type.
    * @return A BehaviorTree configuration object.
    */
-  BT::NodeConfig CreateBTConfig(const std::map<std::string, std::any> & bb_values) const;
+  inline BT::NodeConfig CreateBTConfig(const std::map<std::string, std::any> & bb_values) const
+  {
+    BT::NodeConfig config;
+    config.blackboard = BT::Blackboard::create();
+
+    for (auto & [name, value] : bb_values) {
+      const std::type_info & type = value.type();
+      if (type == typeid(bool)) {
+        config.blackboard->set<bool>(name, std::any_cast<bool>(value));
+      } else if (type == typeid(int)) {
+        config.blackboard->set<int>(name, std::any_cast<int>(value));
+      } else if (type == typeid(unsigned)) {
+        config.blackboard->set<unsigned>(name, std::any_cast<unsigned>(value));
+      } else if (type == typeid(float)) {
+        config.blackboard->set<float>(name, std::any_cast<float>(value));
+      } else if (type == typeid(double)) {
+        config.blackboard->set<double>(name, std::any_cast<double>(value));
+      } else if (type == typeid(const char *)) {
+        config.blackboard->set<std::string>(name, std::any_cast<const char *>(value));
+      } else if (type == typeid(std::string)) {
+        config.blackboard->set<std::string>(name, std::any_cast<std::string>(value));
+      } else {
+        throw std::invalid_argument(
+          "Invalid type for blackboard entry. Valid types are: bool, int, unsigned, float, double, "
+          "const char*, std::string");
+      }
+    }
+
+    return config;
+  }
 
 private:
   const std::string tree_name_;