From 9342626e9c5955a05cf8a9041732076cbfa9c7fe Mon Sep 17 00:00:00 2001
From: Jonas Otto <jonas@jonasotto.com>
Date: Mon, 22 Jul 2024 21:49:13 +0200
Subject: [PATCH] wip tests

---
 battery_state_broadcaster/CMakeLists.txt      | 29 +++++++++++++++++
 battery_state_broadcaster/package.xml         |  7 ++++-
 .../test/test_battery_state_broadcaster.cpp   |  0
 .../test_load_battery_state_broadcaster.cpp   | 31 +++++++++++++++++++
 4 files changed, 66 insertions(+), 1 deletion(-)
 create mode 100644 battery_state_broadcaster/test/test_battery_state_broadcaster.cpp
 create mode 100644 battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp

diff --git a/battery_state_broadcaster/CMakeLists.txt b/battery_state_broadcaster/CMakeLists.txt
index 90c8f1a..302ef95 100644
--- a/battery_state_broadcaster/CMakeLists.txt
+++ b/battery_state_broadcaster/CMakeLists.txt
@@ -27,6 +27,35 @@ ament_target_dependencies(battery_state_broadcaster PUBLIC ${THIS_PACKAGE_INCLUD
 
 pluginlib_export_plugin_description_file(controller_interface battery_state_broadcaster.xml)
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(test_load_battery_state_broadcaster
+    test/test_load_battery_state_broadcaster.cpp)
+  target_include_directories(test_load_battery_state_broadcaster PRIVATE include)
+  target_link_libraries(test_load_battery_state_broadcaster
+    battery_state_broadcaster
+  )
+  ament_target_dependencies(test_load_battery_state_broadcaster
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  #[[ament_add_gmock(test_battery_state_broadcaster
+    test/test_battery_state_broadcaster.cpp)
+  target_include_directories(test_battery_state_broadcaster PRIVATE include)
+  target_link_libraries(test_battery_state_broadcaster
+    battery_state_broadcaster
+  )
+  ament_target_dependencies(test_battery_state_broadcaster
+    hardware_interface
+  )]]#
+endif()
+
 install(
   DIRECTORY include/
   DESTINATION include/battery_state_broadcaster
diff --git a/battery_state_broadcaster/package.xml b/battery_state_broadcaster/package.xml
index dfbeaa4..2b46902 100644
--- a/battery_state_broadcaster/package.xml
+++ b/battery_state_broadcaster/package.xml
@@ -14,7 +14,12 @@
   <depend>realtime_tools</depend>
   <depend>sensor_msgs</depend>
 
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
\ No newline at end of file
diff --git a/battery_state_broadcaster/test/test_battery_state_broadcaster.cpp b/battery_state_broadcaster/test/test_battery_state_broadcaster.cpp
new file mode 100644
index 0000000..e69de29
diff --git a/battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp b/battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp
new file mode 100644
index 0000000..993d0a5
--- /dev/null
+++ b/battery_state_broadcaster/test/test_load_battery_state_broadcaster.cpp
@@ -0,0 +1,31 @@
+#include <gmock/gmock.h>
+#include <memory>
+
+#include "controller_manager/controller_manager.hpp"
+#include "hardware_interface/resource_manager.hpp"
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/utilities.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+TEST(TestLoadBatteryStateBroadcaster, load_controller)
+{
+  std::shared_ptr<rclcpp::Executor> executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  controller_manager::ControllerManager cm(
+      std::make_unique<hardware_interface::ResourceManager>(ros2_control_test_assets::minimal_robot_urdf),
+      executor,
+      "test_controller_manager");
+
+  ASSERT_NE(cm.load_controller("test_battery_state_broadcaster", "battery_state_broadcaster/BatteryStateBroadcaster"),
+            nullptr);
+}
+
+int main(int argc, char** argv)
+{
+  ::testing::InitGoogleMock(&argc, argv);
+  rclcpp::init(argc, argv);
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}