Add magnetic_field_sensor semantic component (#2627) (#2655)

(cherry picked from commit df846fe)

Co-authored-by: Aarav Gupta <[email protected]>

Author: mergify[bot]

controller_interface/CMakeLists.txt

@@ -79,6 +79,13 @@ if(BUILD_TESTING)
     ${sensor_msgs_TARGETS}
   )
 
+  ament_add_gmock(test_magnetic_field_sensor test/test_magnetic_field_sensor.cpp)
+  target_link_libraries(test_magnetic_field_sensor
+    controller_interface
+    hardware_interface::hardware_interface
+    ${sensor_msgs_TARGETS}
+  )
+
   ament_add_gmock(test_pose_sensor test/test_pose_sensor.cpp)
   target_link_libraries(test_pose_sensor
     controller_interface

controller_interface/include/semantic_components/magnetic_field_sensor.hpp

@@ -0,0 +1,73 @@
+// Copyright 2025 Aarav Gupta
+//
+// 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.
+
+#ifndef SEMANTIC_COMPONENTS__MAGNETIC_FIELD_SENSOR_HPP_
+#define SEMANTIC_COMPONENTS__MAGNETIC_FIELD_SENSOR_HPP_
+
+#include <string>
+
+#include "semantic_components/semantic_component_interface.hpp"
+#include "sensor_msgs/msg/magnetic_field.hpp"
+
+namespace semantic_components
+{
+class MagneticFieldSensor : public SemanticComponentInterface<sensor_msgs::msg::MagneticField>
+{
+public:
+  explicit MagneticFieldSensor(const std::string & name)
+  : SemanticComponentInterface(
+      name, {name + "/" + "magnetic_field.x", name + "/" + "magnetic_field.y",
+             name + "/" + "magnetic_field.z"})
+  {
+  }
+
+  /// Returns values as sensor_msgs::msg::MagneticField
+  /**
+   * \return MagneticField message from values
+   */
+  bool get_values_as_message(sensor_msgs::msg::MagneticField & message)
+  {
+    update_data_from_interfaces();
+    message.magnetic_field.x = data_[0];
+    message.magnetic_field.y = data_[1];
+    message.magnetic_field.z = data_[2];
+    return true;
+  }
+
+private:
+  /**
+   * @brief Update the data array from the state interfaces.
+   * @note This method is thread-safe and non-blocking.
+   * @note This method might return stale data if the data is not updated. This is to ensure that
+   * the data from the sensor is not discontinuous.
+   */
+  void update_data_from_interfaces()
+  {
+    for (auto i = 0u; i < data_.size(); ++i)
+    {
+      const auto data = state_interfaces_[i].get().get_optional();
+      if (data.has_value())
+      {
+        data_[i] = data.value();
+      }
+    }
+  }
+
+  // Array to store the data of the magnetic field sensor
+  std::array<double, 3> data_{{0.0, 0.0, 0.0}};
+};
+
+}  // namespace semantic_components
+
+#endif  // SEMANTIC_COMPONENTS__MAGNETIC_FIELD_SENSOR_HPP_

controller_interface/test/test_magnetic_field_sensor.cpp

@@ -0,0 +1,82 @@
+// Copyright 2025 Aarav Gupta
+//
+// 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.
+
+#include "test_magnetic_field_sensor.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+#include "sensor_msgs/msg/magnetic_field.hpp"
+
+void MagneticFieldSensorTest::TearDown() { magnetic_field_sensor_.reset(nullptr); }
+
+TEST_F(MagneticFieldSensorTest, validate_all)
+{
+  // Create the magnetic field sensor
+  magnetic_field_sensor_ = std::make_unique<TestableMagneticFieldSensor>(sensor_name_);
+
+  // validate the component name
+  ASSERT_EQ(magnetic_field_sensor_->name_, sensor_name_);
+
+  // Validate the space reserved for interface_names_ and state_interfaces_
+  // Note : Using capacity() for state_interfaces_ as no such interfaces are defined yet
+  ASSERT_EQ(magnetic_field_sensor_->interface_names_.size(), size_);
+  ASSERT_EQ(magnetic_field_sensor_->state_interfaces_.capacity(), size_);
+
+  // Validate the default interface_names_
+  ASSERT_TRUE(
+    std::equal(
+      magnetic_field_sensor_->interface_names_.begin(),
+      magnetic_field_sensor_->interface_names_.end(), full_interface_names_.begin(),
+      full_interface_names_.end()));
+
+  // Get the interface names
+  std::vector<std::string> interface_names = magnetic_field_sensor_->get_state_interface_names();
+
+  // Assign values
+  auto magnetic_field_x = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, magnetic_field_interface_names_[0], &magnetic_field_values_[0]);
+  auto magnetic_field_y = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, magnetic_field_interface_names_[1], &magnetic_field_values_[1]);
+  auto magnetic_field_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, magnetic_field_interface_names_[2], &magnetic_field_values_[2]);
+
+  // Create local state interface vector
+  std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
+  temp_state_interfaces.reserve(10);
+
+  // Insert the interfaces in jumbled sequence
+  temp_state_interfaces.emplace_back(magnetic_field_y);
+  temp_state_interfaces.emplace_back(magnetic_field_z);
+  temp_state_interfaces.emplace_back(magnetic_field_x);
+
+  // Now call the function to make them in order like interface_names
+  magnetic_field_sensor_->assign_loaned_state_interfaces(temp_state_interfaces);
+
+  // Validate the count of state_interfaces_
+  ASSERT_EQ(magnetic_field_sensor_->state_interfaces_.size(), size_);
+
+  // Validate get_values_as_message()
+  sensor_msgs::msg::MagneticField temp_message;
+  ASSERT_TRUE(magnetic_field_sensor_->get_values_as_message(temp_message));
+  ASSERT_EQ(temp_message.magnetic_field.x, magnetic_field_values_[0]);
+  ASSERT_EQ(temp_message.magnetic_field.y, magnetic_field_values_[1]);
+  ASSERT_EQ(temp_message.magnetic_field.z, magnetic_field_values_[2]);
+
+  // release the state_interfaces_
+  magnetic_field_sensor_->release_interfaces();
+
+  // validate the count of state_interfaces_
+  ASSERT_EQ(magnetic_field_sensor_->state_interfaces_.size(), 0u);
+}

controller_interface/test/test_magnetic_field_sensor.hpp

@@ -0,0 +1,63 @@
+// Copyright 2025 Aarav Gupta
+//
+// 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.
+
+#ifndef TEST_MAGNETIC_FIELD_SENSOR_HPP_
+#define TEST_MAGNETIC_FIELD_SENSOR_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gmock/gmock.h"
+#include "semantic_components/magnetic_field_sensor.hpp"
+
+// implementing and friending so we can access member variables
+class TestableMagneticFieldSensor : public semantic_components::MagneticFieldSensor
+{
+  FRIEND_TEST(MagneticFieldSensorTest, validate_all);
+
+public:
+  // Use generation of interface names
+  explicit TestableMagneticFieldSensor(const std::string & name) : MagneticFieldSensor(name) {}
+
+  virtual ~TestableMagneticFieldSensor() = default;
+};
+
+class MagneticFieldSensorTest : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    full_interface_names_.reserve(size_);
+    for (auto index = 0u; index < size_; ++index)
+    {
+      full_interface_names_.emplace_back(
+        sensor_name_ + "/" + magnetic_field_interface_names_[index]);
+    }
+  }
+
+  void TearDown();
+
+protected:
+  const size_t size_ = 3;
+  const std::string sensor_name_ = "test_magnetometer";
+  std::array<double, 3> magnetic_field_values_ = {{4.4, 5.5, 6.6}};
+  std::unique_ptr<TestableMagneticFieldSensor> magnetic_field_sensor_;
+
+  std::vector<std::string> full_interface_names_;
+  const std::vector<std::string> magnetic_field_interface_names_ = {
+    "magnetic_field.x", "magnetic_field.y", "magnetic_field.z"};
+};
+
+#endif  // TEST_MAGNETIC_FIELD_SENSOR_HPP_

doc/release_notes.rst

@@ -31,6 +31,10 @@ For details see the controller_manager section.
 * A new ``SemanticComponentCommandInterface`` semantic component provides capabilities analogous to the ``SemanticComponentInterface``, but for write-only devices (`#1945 <https://github.com/ros-controls/ros2_control/pull/1945>`_)
 * The new semantic command interface ``LedRgbDevice`` provides standard (command) interfaces for 3-channel LED devices (`#1945 <https://github.com/ros-controls/ros2_control/pull/1945>`_)
 
+controller_interface
+********************
+* The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. `(#2627 <https://github.com/ros-controls/ros2_control/pull/2627>`__)
+
 controller_manager
 ******************
 * URDF is now passed to controllers on init (`#1088 <https://github.com/ros-controls/ros2_control/pull/1088>`_)

hardware_interface/doc/semantic_components.rst

@@ -12,6 +12,7 @@ List of existing ``SemanticComponentInterface`` `(link to header file) <https://
    * `IMUSensor <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_interface/include/semantic_components/imu_sensor.hpp>`__, used by :ref:`IMU Sensor Broadcaster <imu_sensor_broadcaster_userdoc>`
    * `ForceTorqueSensor <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_interface/include/semantic_components/force_torque_sensor.hpp>`__, used by :ref:`Force Torque Sensor Broadcaster <force_torque_sensor_broadcaster_userdoc>`
    * `GPSSensor <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_interface/include/semantic_components/gps_sensor.hpp>`__
+   * `MagneticFieldSensor <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_interface/include/semantic_components/magnetic_field_sensor.hpp>`__
    * `PoseSensor <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_interface/include/semantic_components/pose_sensor.hpp>`__, used by :ref:`Pose Broadcaster <pose_broadcaster_userdoc>`
    * `RangeSensor <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_interface/include/semantic_components/range_sensor.hpp>`__, used by :ref:`Range Sensor Broadcaster <range_sensor_broadcaster_userdoc>`