From b16e81106f0cd007d206d827b14d53d90eca6c67 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Tue, 7 Oct 2025 17:56:08 +0530 Subject: [PATCH 1/6] Add magnetic_field_sensor semantic component --- .../magnetic_field_sensor.hpp | 73 +++++++++++++++++ .../test/test_magnetic_field_sensor.cpp | 82 +++++++++++++++++++ .../test/test_magnetic_field_sensor.hpp | 63 ++++++++++++++ 3 files changed, 218 insertions(+) create mode 100644 controller_interface/include/semantic_components/magnetic_field_sensor.hpp create mode 100644 controller_interface/test/test_magnetic_field_sensor.cpp create mode 100644 controller_interface/test/test_magnetic_field_sensor.hpp diff --git a/controller_interface/include/semantic_components/magnetic_field_sensor.hpp b/controller_interface/include/semantic_components/magnetic_field_sensor.hpp new file mode 100644 index 0000000000..853bf2d98b --- /dev/null +++ b/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 + +#include "semantic_components/semantic_component_interface.hpp" +#include "sensor_msgs/msg/magnetic_field.hpp" + +namespace semantic_components +{ +class MagneticFieldSensor : public SemanticComponentInterface +{ +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) const + { + 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() const + { + 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 + mutable std::array data_{{0.0, 0.0, 0.0}}; +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__MAGNETIC_FIELD_SENSOR_HPP_ diff --git a/controller_interface/test/test_magnetic_field_sensor.cpp b/controller_interface/test/test_magnetic_field_sensor.cpp new file mode 100644 index 0000000000..629dbb2dd8 --- /dev/null +++ b/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 +#include +#include +#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(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 interface_names = magnetic_field_sensor_->get_state_interface_names(); + + // Assign values + auto magnetic_field_x = std::make_shared( + sensor_name_, magnetic_field_interface_names_[0], &magnetic_field_values_[0]); + auto magnetic_field_y = std::make_shared( + sensor_name_, magnetic_field_interface_names_[1], &magnetic_field_values_[1]); + auto magnetic_field_z = std::make_shared( + sensor_name_, magnetic_field_interface_names_[2], &magnetic_field_values_[2]); + + // Create local state interface vector + std::vector 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); +} diff --git a/controller_interface/test/test_magnetic_field_sensor.hpp b/controller_interface/test/test_magnetic_field_sensor.hpp new file mode 100644 index 0000000000..84e31cf9f4 --- /dev/null +++ b/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 +#include +#include + +#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 magnetic_field_values_ = {{4.4, 5.5, 6.6}}; + std::unique_ptr magnetic_field_sensor_; + + std::vector full_interface_names_; + const std::vector magnetic_field_interface_names_ = { + "magnetic_field.x", "magnetic_field.y", "magnetic_field.z"}; +}; + +#endif // TEST_MAGNETIC_FIELD_SENSOR_HPP_ From cab54b594d3eeb452b7fe8941c08d12de9660f71 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 8 Oct 2025 10:15:32 +0530 Subject: [PATCH 2/6] Apply changes from code review --- .../include/semantic_components/magnetic_field_sensor.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/semantic_components/magnetic_field_sensor.hpp b/controller_interface/include/semantic_components/magnetic_field_sensor.hpp index 853bf2d98b..0321fd088e 100644 --- a/controller_interface/include/semantic_components/magnetic_field_sensor.hpp +++ b/controller_interface/include/semantic_components/magnetic_field_sensor.hpp @@ -52,7 +52,7 @@ class MagneticFieldSensor : public SemanticComponentInterface data_{{0.0, 0.0, 0.0}}; + std::array data_{{0.0, 0.0, 0.0}}; }; } // namespace semantic_components From 8d8718a13751aa38b0984c5c750980040e00812f Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 8 Oct 2025 12:34:46 +0530 Subject: [PATCH 3/6] Apply changes from code review --- .../include/semantic_components/magnetic_field_sensor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/magnetic_field_sensor.hpp b/controller_interface/include/semantic_components/magnetic_field_sensor.hpp index 0321fd088e..7ce424cc3f 100644 --- a/controller_interface/include/semantic_components/magnetic_field_sensor.hpp +++ b/controller_interface/include/semantic_components/magnetic_field_sensor.hpp @@ -36,7 +36,7 @@ class MagneticFieldSensor : public SemanticComponentInterface Date: Thu, 9 Oct 2025 08:04:14 +0530 Subject: [PATCH 4/6] Add test to CMakeLists.txt --- controller_interface/CMakeLists.txt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 95f974db77..52105df453 100644 --- a/controller_interface/CMakeLists.txt +++ b/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 From c356023818658429d2b44666de359a39f347d78d Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 9 Oct 2025 18:10:27 +0530 Subject: [PATCH 5/6] Update documentation --- doc/release_notes.rst | 4 ++++ hardware_interface/doc/semantic_components.rst | 1 + 2 files changed, 5 insertions(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index e08600c234..153874789f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -5,6 +5,10 @@ Release Notes: Kilted Kaiju to Lyrical Luth This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases. +controller_interface +******************** +* The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. + controller_manager ****************** diff --git a/hardware_interface/doc/semantic_components.rst b/hardware_interface/doc/semantic_components.rst index 1991270170..7beb8b0e59 100644 --- a/hardware_interface/doc/semantic_components.rst +++ b/hardware_interface/doc/semantic_components.rst @@ -12,6 +12,7 @@ List of existing ``SemanticComponentInterface`` `(link to header file) `__, used by :ref:`IMU Sensor Broadcaster ` * `ForceTorqueSensor `__, used by :ref:`Force Torque Sensor Broadcaster ` * `GPSSensor `__ + * `MagneticFieldSensor `__ * `PoseSensor `__, used by :ref:`Pose Broadcaster ` * `RangeSensor `__, used by :ref:`Range Sensor Broadcaster ` From 2a915ae9e4a6280c63b1bf8de0deb1a66f69cfa8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 9 Oct 2025 18:59:14 +0200 Subject: [PATCH 6/6] Update doc/release_notes.rst --- doc/release_notes.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 153874789f..5c37d7f934 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -7,7 +7,7 @@ This list summarizes important changes between Kilted Kaiju (previous) and Lyric controller_interface ******************** -* The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. +* The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. `(#2627 `__) controller_manager ******************