add tests for magnetometer_broadcaster/MagnetometerBroadcaster

christianrauch · christianrauch · commit cddcb8905d03 · 2026-03-20T18:51:03.000+01:00
diff --git a/magnetometer_broadcaster/CMakeLists.txt b/magnetometer_broadcaster/CMakeLists.txt
@@ -43,6 +43,33 @@ target_link_libraries(magnetometer_broadcaster PUBLIC
 pluginlib_export_plugin_description_file(
   controller_interface magnetometer_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)
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_magnetometer_broadcaster
+    test/test_load_magnetometer_broadcaster.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/magnetometer_broadcaster_params.yaml)
+  target_include_directories(test_load_magnetometer_broadcaster PRIVATE include)
+  target_link_libraries(test_load_magnetometer_broadcaster
+    magnetometer_broadcaster
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  ament_add_gmock(test_magnetometer_broadcaster
+    test/test_magnetometer_broadcaster.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/magnetometer_broadcaster_params.yaml)
+  target_link_libraries(test_magnetometer_broadcaster
+    magnetometer_broadcaster
+    ros2_control_test_assets::ros2_control_test_assets
+    magnetometer_broadcaster
+  )
+endif()
+
 install(
   TARGETS
     magnetometer_broadcaster
diff --git a/magnetometer_broadcaster/test/magnetometer_broadcaster_params.yaml b/magnetometer_broadcaster/test/magnetometer_broadcaster_params.yaml
@@ -0,0 +1,4 @@
+test_magnetometer_broadcaster:
+  ros__parameters:
+    sensor_name: magnetometer
+    frame_id: magnetometer_frame
diff --git a/magnetometer_broadcaster/test/test_load_magnetometer_broadcaster.cpp b/magnetometer_broadcaster/test/test_load_magnetometer_broadcaster.cpp
@@ -0,0 +1,46 @@
+// Copyright 2026 ros2_control development team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ * Authors: Christian Rauch, Wiktor Bajor, Jakub Delicat
+ */
+
+#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(TestLoadMagnetometerBroadcaster, load_controller)
+{
+  rclcpp::init(0, nullptr);
+  std::shared_ptr<rclcpp::Executor> executor =
+    std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  controller_manager::ControllerManager cm(
+    executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
+  const std::string test_file_path =
+    std::string(TEST_FILES_DIRECTORY) + "/magnetometer_broadcaster_params.yaml";
+
+  cm.set_parameter({"test_magnetometer_broadcaster.params_file", test_file_path});
+  cm.set_parameter(
+    {"test_magnetometer_broadcaster.type", "magnetometer_broadcaster/MagnetometerBroadcaster"});
+
+  ASSERT_NE(cm.load_controller("test_magnetometer_broadcaster"), nullptr);
+  rclcpp::shutdown();
+}
diff --git a/magnetometer_broadcaster/test/test_magnetometer_broadcaster.cpp b/magnetometer_broadcaster/test/test_magnetometer_broadcaster.cpp
@@ -0,0 +1,203 @@
+// Copyright 2026 ros2_control development team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ * Authors: Christian Rauch, Wiktor Bajor, Jakub Delicat
+ */
+
+#include <gmock/gmock.h>
+#include <class_loader/class_loader.hpp>
+#include <controller_interface/controller_interface.hpp>
+#include <hardware_interface/loaned_state_interface.hpp>
+#include <hardware_interface/types/hardware_interface_return_values.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+#include <magnetometer_broadcaster/magnetometer_broadcaster_parameters.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/wait_result_kind.hpp>
+#include <rclcpp/wait_set.hpp>
+#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
+#include <ros2_control_test_assets/descriptions.hpp>
+#include <sensor_msgs/msg/magnetic_field.hpp>
+#include <utility>
+
+using callback_return_type =
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace
+{
+rclcpp::WaitResultKind wait_for(std::shared_ptr<rclcpp::SubscriptionBase> subscription)
+{
+  rclcpp::WaitSet wait_set;
+  wait_set.add_subscription(subscription);
+  return wait_set.wait().kind();
+}
+
+rclcpp::NodeOptions create_node_options_with_overriden_parameters(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  auto node_options = rclcpp::NodeOptions();
+  node_options.parameter_overrides(parameters);
+  return node_options;
+}
+}  // namespace
+
+class MagnetometerBroadcasterTest : public ::testing::Test
+{
+public:
+  MagnetometerBroadcasterTest() { rclcpp::init(0, nullptr); }
+
+  ~MagnetometerBroadcasterTest() { rclcpp::shutdown(); }
+
+  void SetUp()
+  {
+    broadcaster = loader->createInstance<controller_interface::ControllerInterface>(
+      "magnetometer_broadcaster::MagnetometerBroadcaster");
+  }
+
+  void TearDown() { broadcaster.reset(); }
+
+  void setup_broadcaster()
+  {
+    std::vector<hardware_interface::LoanedStateInterface> state_ifs;
+    state_ifs.emplace_back(magnetic_field_x, nullptr);
+    state_ifs.emplace_back(magnetic_field_y, nullptr);
+    state_ifs.emplace_back(magnetic_field_z, nullptr);
+
+    broadcaster->assign_interfaces({}, std::move(state_ifs));
+  }
+
+  sensor_msgs::msg::MagneticField subscribe_and_get_message()
+  {
+    rclcpp::Node test_subscription_node("test_subscription_node");
+    auto subscription = test_subscription_node.create_subscription<sensor_msgs::msg::MagneticField>(
+      "/test_magnetometer_broadcaster/magnetic_field", 10,
+      [](const sensor_msgs::msg::MagneticField::SharedPtr) {});
+    broadcaster->update(rclcpp::Time{}, rclcpp::Duration::from_seconds(0));
+    wait_for(subscription);
+
+    rclcpp::MessageInfo msg_info;
+    sensor_msgs::msg::MagneticField msg;
+    subscription->take(msg, msg_info);
+    return msg;
+  }
+
+  controller_interface::ControllerInterfaceParams create_ctrl_params(
+    const rclcpp::NodeOptions & node_options, const std::string & robot_description = "")
+  {
+    controller_interface::ControllerInterfaceParams params;
+    params.controller_name = "test_magnetometer_broadcaster";
+    params.robot_description = robot_description;
+    params.update_rate = 0;
+    params.node_namespace = "";
+    params.node_options = node_options;
+    return params;
+  }
+
+protected:
+  const std::string sensor_name_ = "magnetometer";
+  const rclcpp::Parameter sensor_name_param_ = rclcpp::Parameter("sensor_name", sensor_name_);
+  const std::string frame_id_ = "magnetometer_frame";
+  const rclcpp::Parameter frame_id_param_ = rclcpp::Parameter("frame_id", frame_id_);
+
+  std::array<double, 3> sensor_values_ = {{20e-6, 30e-6, 40e-6}};
+  hardware_interface::StateInterface::SharedPtr magnetic_field_x =
+    std::make_shared<hardware_interface::StateInterface>(
+      sensor_name_, "magnetic_field.x", &sensor_values_[0]);
+  hardware_interface::StateInterface::SharedPtr magnetic_field_y =
+    std::make_shared<hardware_interface::StateInterface>(
+      sensor_name_, "magnetic_field.y", &sensor_values_[1]);
+  hardware_interface::StateInterface::SharedPtr magnetic_field_z =
+    std::make_shared<hardware_interface::StateInterface>(
+      sensor_name_, "magnetic_field.z", &sensor_values_[2]);
+
+  std::unique_ptr<class_loader::ClassLoader> loader =
+    std::make_unique<class_loader::ClassLoader>(std::string{});
+
+  controller_interface::ControllerInterfaceSharedPtr broadcaster = nullptr;
+};
+
+TEST_F(MagnetometerBroadcasterTest, whenNoParamsAreSetThenInitShouldFail)
+{
+  const auto result = broadcaster->init(create_ctrl_params(
+    broadcaster->define_custom_node_options(), ros2_control_test_assets::minimal_robot_urdf));
+  ASSERT_EQ(result, controller_interface::return_type::ERROR);
+}
+
+TEST_F(MagnetometerBroadcasterTest, whenOnlySensorNameIsSetThenInitShouldFail)
+{
+  const auto node_options = create_node_options_with_overriden_parameters({sensor_name_param_});
+  const auto result = broadcaster->init(
+    create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf));
+  ASSERT_EQ(result, controller_interface::return_type::ERROR);
+}
+
+TEST_F(
+  MagnetometerBroadcasterTest,
+  whenAllRequiredArgumentsAreSetThenInitConfigureAndActivationShouldSucceed)
+{
+  const auto node_options =
+    create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_param_});
+  const auto result = broadcaster->init(
+    create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf));
+  ASSERT_EQ(result, controller_interface::return_type::OK);
+  ASSERT_EQ(broadcaster->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS);
+  ASSERT_EQ(broadcaster->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS);
+}
+
+TEST_F(MagnetometerBroadcasterTest, whenBroadcasterIsActiveShouldPublishWithCovarianceSetToZero)
+{
+  const auto node_options =
+    create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_param_});
+  const auto result = broadcaster->init(
+    create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf));
+  ASSERT_EQ(result, controller_interface::return_type::OK);
+  ASSERT_EQ(broadcaster->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS);
+  setup_broadcaster();
+  ASSERT_EQ(broadcaster->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS);
+
+  const auto msg = subscribe_and_get_message();
+  EXPECT_EQ(msg.header.frame_id, frame_id_);
+  EXPECT_EQ(msg.magnetic_field.x, sensor_values_[0]);
+  EXPECT_EQ(msg.magnetic_field.y, sensor_values_[1]);
+  EXPECT_EQ(msg.magnetic_field.z, sensor_values_[2]);
+
+  const std::array<double, 9> expected_covariance = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+  ASSERT_THAT(msg.magnetic_field_covariance, ::testing::ElementsAreArray(expected_covariance));
+}
+
+TEST_F(
+  MagnetometerBroadcasterTest,
+  whenBroadcasterIsActiveAndStaticCovarianceShouldPublishWithStaticCovariance)
+{
+  const std::array<double, 9> static_covariance = {{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}};
+  const auto node_options = create_node_options_with_overriden_parameters(
+    {sensor_name_param_,
+     frame_id_param_,
+     {"static_covariance",
+      std::vector<double>{static_covariance.begin(), static_covariance.end()}}});
+  const auto result = broadcaster->init(
+    create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf));
+  ASSERT_EQ(result, controller_interface::return_type::OK);
+  ASSERT_EQ(broadcaster->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS);
+  setup_broadcaster();
+  ASSERT_EQ(broadcaster->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS);
+
+  const auto msg = subscribe_and_get_message();
+  EXPECT_EQ(msg.header.frame_id, frame_id_);
+  EXPECT_EQ(msg.magnetic_field.x, sensor_values_[0]);
+  EXPECT_EQ(msg.magnetic_field.y, sensor_values_[1]);
+  EXPECT_EQ(msg.magnetic_field.z, sensor_values_[2]);
+
+  ASSERT_THAT(msg.magnetic_field_covariance, ::testing::ElementsAreArray(static_covariance));
+}
