broadcaster for magnetic field values from a magnetometer

doc/controllers_index.rst

@@ -66,6 +66,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same
    Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst>
    GPS Sensor Broadcaster <../gps_sensor_broadcaster/doc/userdoc.rst>
    State Interfaces Broadcaster <../state_interfaces_broadcaster/doc/userdoc.rst>
+   Magnetometer Broadcaster <../magnetometer_broadcaster/doc/userdoc.rst>
 
 Filters
 **********************

doc/release_notes.rst

@@ -54,3 +54,7 @@ steering_controllers_library
 *****************************
 * Parameter ``tf_frame_prefix`` added with the similar functionality to other controllers. (`#2080 <https://github.com/ros-controls/ros2_controllers/pull/2080>`_).
 * Set odometry service added to be used at runtime. (`#2244 <https://github.com/ros-controls/ros2_controllers/pull/2244>`_).
+
+magnetometer_broadcaster
+************************
+New package to broadcast ``sensor_msgs/msg/MagneticField`` from state interfaces defined by the ``semantic_components::MagneticFieldSensor``.

magnetometer_broadcaster/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.16)
+project(magnetometer_broadcaster)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  generate_parameter_library
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  sensor_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(magnetometer_broadcaster_parameters
+  src/magnetometer_broadcaster_parameters.yaml
+)
+
+add_library(magnetometer_broadcaster SHARED
+  src/magnetometer_broadcaster.cpp
+)
+target_compile_features(magnetometer_broadcaster PUBLIC cxx_std_20)
+target_link_libraries(magnetometer_broadcaster PUBLIC
+                      magnetometer_broadcaster_parameters
+                      controller_interface::controller_interface
+                      hardware_interface::hardware_interface
+                      pluginlib::pluginlib
+                      rclcpp::rclcpp
+                      rclcpp_lifecycle::rclcpp_lifecycle
+                      realtime_tools::realtime_tools
+                      ${sensor_msgs_TARGETS})
+
+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
+    magnetometer_broadcaster_parameters
+  EXPORT export_magnetometer_broadcaster
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_magnetometer_broadcaster HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

magnetometer_broadcaster/doc/userdoc.rst

@@ -0,0 +1,24 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/magnetometer_broadcaster/doc/userdoc.rst
+
+.. _magnetometer_broadcaster_userdoc:
+
+Magnetometer Broadcaster
+--------------------------------
+Broadcaster for magnetometer messages (type: ``sensor_msgs/msg/MagneticField``), using the ``semantic_components::MagneticFieldSensor``.
+
+Parameters
+^^^^^^^^^^^
+This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters. The parameter `definition file located in the src folder <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/magnetometer_broadcaster/src/magnetometer_broadcaster_parameters.yaml>`_ contains descriptions for all the parameters used by the controller.
+
+List of parameters
+=========================
+.. generate_parameter_library_details:: ../src/magnetometer_broadcaster_parameters.yaml
+
+
+An example parameter file
+=========================
+
+An example parameter file for this controller can be found in `the test directory <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/magnetometer_broadcaster/test/magnetometer_broadcaster_params.yaml>`_:
+
+.. literalinclude:: ../test/magnetometer_broadcaster_params.yaml
+   :language: yaml

magnetometer_broadcaster/magnetometer_broadcaster.xml

@@ -0,0 +1,10 @@
+<library path="magnetometer_broadcaster">
+  <class name="magnetometer_broadcaster/MagnetometerBroadcaster"
+         type="magnetometer_broadcaster::MagnetometerBroadcaster"
+         base_class_type="controller_interface::ControllerInterface"
+>
+  <description>
+      This controller publishes the readings of a magnetometer as sensor_msgs/msg/MagneticField message.
+  </description>
+  </class>
+</library>

magnetometer_broadcaster/package.xml

@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd"
+            schematypens="http://www.w3.org/2001/XMLSchema"
+?>
+<package format="3">
+  <name>magnetometer_broadcaster</name>
+  <version>6.4.0</version>
+  <description>Controller to publish readings of a magnetometer.</description>
+
+  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
+  <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>
+  <maintainer email="christoph.froehlich@ait.ac.at">Christoph Froehlich</maintainer>
+  <maintainer email="sai.kishor@pal-robotics.com">Sai Kishor Kothakota</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://control.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-controls/ros2_controllers/issues</url>
+  <url type="repository">https://github.com/ros-controls/ros2_controllers/</url>
+
+  <author email="Rauch.Christian@gmx.de">Christian Rauch</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>ros2_control_cmake</build_depend>
+
+  <depend>backward_ros</depend>
+  <depend>controller_interface</depend>
+  <depend>eigen</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

magnetometer_broadcaster/src/magnetometer_broadcaster.cpp

@@ -0,0 +1,147 @@
+// Copyright 2026 Christian Rauch
+// Copyright 2021 PAL Robotics SL.
+//
+// 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, Subhas Das, Denis Stogl, Victor Lopez
+ */
+
+#include <memory>
+#include <vector>
+
+#include <controller_interface/controller_interface.hpp>
+#include <magnetometer_broadcaster/magnetometer_broadcaster_parameters.hpp>
+#include <pluginlib/class_list_macros.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+#include <realtime_tools/realtime_publisher.hpp>
+#include <semantic_components/magnetic_field_sensor.hpp>
+#include <sensor_msgs/msg/magnetic_field.hpp>
+
+namespace magnetometer_broadcaster
+{
+
+class MagnetometerBroadcaster : public controller_interface::ControllerInterface
+{
+public:
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override
+  {
+    return {.type = controller_interface::interface_configuration_type::NONE};
+  }
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override
+  {
+    return {
+      .type = controller_interface::interface_configuration_type::INDIVIDUAL,
+      .names = magnetometer_->get_state_interface_names(),
+    };
+  }
+
+  controller_interface::CallbackReturn on_init() override
+  {
+    try
+    {
+      param_listener_ = std::make_shared<ParamListener>(get_node());
+    }
+    catch (const std::exception & e)
+    {
+      RCLCPP_ERROR_STREAM(
+        get_node()->get_logger(), "Exception thrown during init stage with message: " << e.what());
+      return CallbackReturn::ERROR;
+    }
+
+    return CallbackReturn::SUCCESS;
+  }
+
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & /*previous_state*/) override
+  {
+    try
+    {
+      params_ = param_listener_->get_params();
+    }
+    catch (const std::exception & e)
+    {
+      RCLCPP_ERROR_STREAM(
+        get_node()->get_logger(),
+        "Exception thrown during config stage with message: " << e.what());
+      return CallbackReturn::ERROR;
+    }
+
+    magnetometer_ = std::make_unique<semantic_components::MagneticFieldSensor>(params_.sensor_name);
+    try
+    {
+      sensor_state_publisher_ = get_node()->create_publisher<sensor_msgs::msg::MagneticField>(
+        "~/magnetic_field", rclcpp::SystemDefaultsQoS());
+      realtime_publisher_ = std::make_unique<StatePublisher>(sensor_state_publisher_);
+    }
+    catch (const std::exception & e)
+    {
+      RCLCPP_ERROR_STREAM(
+        get_node()->get_logger(),
+        "Exception thrown during publisher creation at configure stage with message: " << e.what());
+      return CallbackReturn::ERROR;
+    }
+
+    state_message_.header.frame_id = params_.frame_id;
+    std::copy(
+      params_.static_covariance.begin(), params_.static_covariance.end(),
+      state_message_.magnetic_field_covariance.begin());
+
+    return CallbackReturn::SUCCESS;
+  }
+
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & /*previous_state*/) override
+  {
+    magnetometer_->assign_loaned_state_interfaces(state_interfaces_);
+    return CallbackReturn::SUCCESS;
+  }
+
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & /*previous_state*/) override
+  {
+    magnetometer_->release_interfaces();
+    return CallbackReturn::SUCCESS;
+  }
+
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & /*period*/) override
+  {
+    magnetometer_->get_values_as_message(state_message_);
+
+    if (realtime_publisher_)
+    {
+      state_message_.header.stamp = time;
+      realtime_publisher_->try_publish(state_message_);
+    }
+
+    return controller_interface::return_type::OK;
+  }
+
+protected:
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  std::unique_ptr<semantic_components::MagneticFieldSensor> magnetometer_;
+
+  using StatePublisher = realtime_tools::RealtimePublisher<sensor_msgs::msg::MagneticField>;
+  rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr sensor_state_publisher_;
+  std::unique_ptr<StatePublisher> realtime_publisher_;
+  sensor_msgs::msg::MagneticField state_message_;
+};
+
+}  // namespace magnetometer_broadcaster
+
+PLUGINLIB_EXPORT_CLASS(
+  magnetometer_broadcaster::MagnetometerBroadcaster, controller_interface::ControllerInterface)

magnetometer_broadcaster/src/magnetometer_broadcaster_parameters.yaml

@@ -0,0 +1,29 @@
+magnetometer_broadcaster:
+  sensor_name: {
+    type: string,
+    default_value: "",
+    read_only: true,
+    description: "Defines sensor name used as prefix for its interfaces.
+    Interface names are: ``<sensor_name>/magnetic_field.x, <sensor_name>/magnetic_field.x, <sensor_name>/magnetic_field.z.``",
+    validation: {
+      not_empty<>: null
+    }
+  }
+  frame_id: {
+    type: string,
+    default_value: "",
+    read_only: true,
+    description: "Sensor's frame_id in which values are published.",
+    validation: {
+      not_empty<>: null
+    }
+  }
+  static_covariance: {
+    type: double_array,
+    default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    read_only: true,
+    description: "Static covariance. Row major about x, y, z axes",
+    validation: {
+      fixed_size<>: [9],
+    }
+  }

magnetometer_broadcaster/test/magnetometer_broadcaster_params.yaml

@@ -0,0 +1,4 @@
+test_magnetometer_broadcaster:
+  ros__parameters:
+    sensor_name: magnetometer
+    frame_id: magnetometer_frame