Add calibration possibility to IMU broadcaster (ros-controls#1833)

christophfroehlich · web-flow · commit f920e6d9031a · 2025-08-29T09:36:37.000+02:00
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -134,7 +134,7 @@ repos:
     rev: v2.4.1
     hooks:
       - id: codespell
-        args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel']
+        args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel,NED']
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
@@ -12,6 +12,10 @@ force_torque_sensor_broadcaster
 *******************************
 * Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 <https://github.com/ros-controls/ros2_controllers/pull/1647/files>`__).
 
+imu_sensor_broadcaster
+*******************************
+* IMU sensor broadcaster is now a chainable controller. It supports a calibration by means of a rotation, defined as euler angles, to its target frame. (`#1833 <https://github.com/ros-controls/ros2_controllers/pull/1833/files>`__).
+
 joint_trajectory_controller
 *******************************
 * The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 <https://github.com/ros-controls/ros2_controllers/pull/1759>`__).
diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt
@@ -7,6 +7,7 @@ export_windows_symbols()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   controller_interface
+  Eigen3
   generate_parameter_library
   hardware_interface
   pluginlib
@@ -37,6 +38,7 @@ target_include_directories(imu_sensor_broadcaster PUBLIC
 target_link_libraries(imu_sensor_broadcaster PUBLIC
                       imu_sensor_broadcaster_parameters
                       controller_interface::controller_interface
+                      Eigen3::Eigen
                       hardware_interface::hardware_interface
                       pluginlib::pluginlib
                       rclcpp::rclcpp
@@ -68,6 +70,17 @@ if(BUILD_TESTING)
   target_include_directories(test_imu_sensor_broadcaster PRIVATE include)
   target_link_libraries(test_imu_sensor_broadcaster
     imu_sensor_broadcaster
+    tf2::tf2
+    tf2_geometry_msgs::tf2_geometry_msgs
+  )
+
+  find_package(tf2 REQUIRED)
+  find_package(tf2_geometry_msgs REQUIRED)
+  ament_add_gmock(test_imu_transform test/test_imu_transform.cpp)
+  target_link_libraries(test_imu_transform
+    imu_sensor_broadcaster
+    tf2::tf2
+    tf2_geometry_msgs::tf2_geometry_msgs
   )
 endif()
 
diff --git a/imu_sensor_broadcaster/imu_sensor_broadcaster.xml b/imu_sensor_broadcaster/imu_sensor_broadcaster.xml
@@ -1,6 +1,6 @@
 <library path="imu_sensor_broadcaster">
   <class name="imu_sensor_broadcaster/IMUSensorBroadcaster"
-         type="imu_sensor_broadcaster::IMUSensorBroadcaster" base_class_type="controller_interface::ControllerInterface">
+         type="imu_sensor_broadcaster::IMUSensorBroadcaster" base_class_type="controller_interface::ChainableControllerInterface">
   <description>
 	  This controller publishes the readings of an IMU sensor as sensor_msgs/Imu message.
   </description>
diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp
@@ -20,19 +20,21 @@
 #define IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_
 
 #include <memory>
+#include <vector>
 
+#include "Eigen/Dense"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 #include "semantic_components/imu_sensor.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 
-#include "controller_interface/controller_interface.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
 // auto-generated by generate_parameter_library
 #include "imu_sensor_broadcaster/imu_sensor_broadcaster_parameters.hpp"
 
 namespace imu_sensor_broadcaster
 {
-class IMUSensorBroadcaster : public controller_interface::ControllerInterface
+class IMUSensorBroadcaster : public controller_interface::ChainableControllerInterface
 {
 public:
   controller_interface::InterfaceConfiguration command_interface_configuration() const override;
@@ -49,18 +51,25 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface
   controller_interface::CallbackReturn on_deactivate(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  controller_interface::return_type update(
+  controller_interface::return_type update_and_write_commands(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override;
+
 protected:
   std::shared_ptr<ParamListener> param_listener_;
   Params params_;
+  Eigen::Quaterniond r_;
 
   std::unique_ptr<semantic_components::IMUSensor> imu_sensor_;
 
   using StatePublisher = realtime_tools::RealtimePublisher<sensor_msgs::msg::Imu>;
   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr sensor_state_publisher_;
   std::unique_ptr<StatePublisher> realtime_publisher_;
+  sensor_msgs::msg::Imu state_message_;
 };
 
 }  // namespace imu_sensor_broadcaster
diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_transform.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_transform.hpp
@@ -0,0 +1,98 @@
+// Copyright 2025 AIT Austrian Institute of Technology GmbH
+//
+// 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: Christoph Froehlich
+ * Adapted from
+ * https://github.com/ros-perception/imu_pipeline/blob/jazzy/imu_transformer/include/imu_transformer/tf2_sensor_msgs.h
+ */
+
+#ifndef IMU_SENSOR_BROADCASTER__IMU_TRANSFORM_HPP_
+#define IMU_SENSOR_BROADCASTER__IMU_TRANSFORM_HPP_
+
+#include <Eigen/Dense>
+#include "sensor_msgs/msg/imu.hpp"
+
+namespace imu_sensor_broadcaster
+{
+/**
+ * @brief Euler to quaternion Z-Y'-X'' convention
+ * https://eigen.tuxfamily.org/dox-devel/group__TutorialGeometry.html
+ */
+Eigen::Quaterniond quat_from_euler(double roll, double pitch, double yaw)
+{
+  return Eigen::Quaterniond(
+    Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
+    Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
+    Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()));
+}
+
+/**
+ * @brief Transforms a covariance array from one frame to another
+ */
+inline void transform_covariance(
+  std::array<double, 9> & out, const std::array<double, 9> & in, Eigen::Quaterniond r)
+{
+  Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > cov_in(in.data());
+  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > cov_out(out.data());
+  cov_out = r * cov_in * r.inverse();
+}
+
+/**
+ * @brief Transforms sensor_msgs::Imu data from one frame to another
+ */
+inline void do_transform(
+  sensor_msgs::msg::Imu & imu_out, const sensor_msgs::msg::Imu & imu_in, const Eigen::Quaterniond r)
+{
+  imu_out.header = imu_in.header;
+
+  Eigen::Vector3d vel =
+    r * Eigen::Vector3d(
+          imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
+
+  imu_out.angular_velocity.x = vel.x();
+  imu_out.angular_velocity.y = vel.y();
+  imu_out.angular_velocity.z = vel.z();
+
+  transform_covariance(imu_out.angular_velocity_covariance, imu_in.angular_velocity_covariance, r);
+
+  Eigen::Vector3d accel =
+    r * Eigen::Vector3d(
+          imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
+
+  imu_out.linear_acceleration.x = accel.x();
+  imu_out.linear_acceleration.y = accel.y();
+  imu_out.linear_acceleration.z = accel.z();
+
+  transform_covariance(
+    imu_out.linear_acceleration_covariance, imu_in.linear_acceleration_covariance, r);
+
+  // Orientation expresses attitude of the new frame_id in a fixed world frame. This is why the
+  // transform here applies in the opposite direction.
+  Eigen::Quaterniond orientation =
+    Eigen::Quaterniond(
+      imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z) *
+    r.inverse();
+
+  imu_out.orientation.w = orientation.w();
+  imu_out.orientation.x = orientation.x();
+  imu_out.orientation.y = orientation.y();
+  imu_out.orientation.z = orientation.z();
+
+  // Orientation is measured relative to the fixed world frame, so it doesn't change when applying a
+  // static transform to the sensor frame.
+  imu_out.orientation_covariance = imu_in.orientation_covariance;
+}
+}  // namespace imu_sensor_broadcaster
+#endif  // IMU_SENSOR_BROADCASTER__IMU_TRANSFORM_HPP_
diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml
@@ -24,6 +24,7 @@
 
   <depend>backward_ros</depend>
   <depend>controller_interface</depend>
+  <depend>eigen</depend>
   <depend>generate_parameter_library</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
@@ -38,6 +39,8 @@
   <test_depend>controller_manager</test_depend>
   <test_depend>hardware_interface_testing</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>
+  <test_depend>tf2</test_depend>
+  <test_depend>tf2_geometry_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp
@@ -21,14 +21,15 @@
 #include <memory>
 #include <string>
 
+#include "imu_sensor_broadcaster/imu_transform.hpp"
+
 namespace imu_sensor_broadcaster
 {
 controller_interface::CallbackReturn IMUSensorBroadcaster::on_init()
 {
   try
   {
     param_listener_ = std::make_shared<ParamListener>(get_node());
-    params_ = param_listener_->get_params();
   }
   catch (const std::exception & e)
   {
@@ -43,7 +44,20 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init()
 controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  params_ = param_listener_->get_params();
+  try
+  {
+    params_ = param_listener_->get_params();
+    r_ = quat_from_euler(
+      params_.rotation_offset.roll, params_.rotation_offset.pitch, params_.rotation_offset.yaw);
+    r_.normalize();
+  }
+  catch (const std::exception & e)
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(), "Exception thrown during config stage with message: %s \n",
+      e.what());
+    return CallbackReturn::ERROR;
+  }
 
   imu_sensor_ = std::make_unique<semantic_components::IMUSensor>(
     semantic_components::IMUSensor(params_.sensor_name));
@@ -62,18 +76,15 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
     return CallbackReturn::ERROR;
   }
 
-  realtime_publisher_->lock();
-  realtime_publisher_->msg_.header.frame_id = params_.frame_id;
+  state_message_.header.frame_id = params_.frame_id;
   // convert double vector to fixed-size array in the message
   for (size_t i = 0; i < 9; ++i)
   {
-    realtime_publisher_->msg_.orientation_covariance[i] = params_.static_covariance_orientation[i];
-    realtime_publisher_->msg_.angular_velocity_covariance[i] =
-      params_.static_covariance_angular_velocity[i];
-    realtime_publisher_->msg_.linear_acceleration_covariance[i] =
+    state_message_.orientation_covariance[i] = params_.static_covariance_orientation[i];
+    state_message_.angular_velocity_covariance[i] = params_.static_covariance_angular_velocity[i];
+    state_message_.linear_acceleration_covariance[i] =
       params_.static_covariance_linear_acceleration[i];
   }
-  realtime_publisher_->unlock();
 
   RCLCPP_DEBUG(get_node()->get_logger(), "configure successful");
   return CallbackReturn::SUCCESS;
@@ -110,22 +121,76 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_deactivate(
   return CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type IMUSensorBroadcaster::update(
+controller_interface::return_type IMUSensorBroadcaster::update_and_write_commands(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
-  if (realtime_publisher_ && realtime_publisher_->trylock())
+  sensor_msgs::msg::Imu input_imu{state_message_};
+  imu_sensor_->get_values_as_message(input_imu);
+  do_transform(state_message_, input_imu, r_);
+
+  if (realtime_publisher_)
   {
-    realtime_publisher_->msg_.header.stamp = time;
-    imu_sensor_->get_values_as_message(realtime_publisher_->msg_);
-    realtime_publisher_->unlockAndPublish();
+    state_message_.header.stamp = time;
+    realtime_publisher_->try_publish(state_message_);
   }
 
   return controller_interface::return_type::OK;
 }
 
+controller_interface::return_type IMUSensorBroadcaster::update_reference_from_subscribers(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  return controller_interface::return_type::OK;
+}
+
+std::vector<hardware_interface::StateInterface> IMUSensorBroadcaster::on_export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> exported_state_interfaces;
+
+  std::string export_prefix = get_node()->get_name();
+  if (!params_.sensor_name.empty())
+  {
+    // Update the prefix and get the proper IMU sensor naming
+    export_prefix = export_prefix + "/" + params_.sensor_name;
+  }
+
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "orientation.x", &state_message_.orientation.x));
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "orientation.y", &state_message_.orientation.y));
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "orientation.z", &state_message_.orientation.z));
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "orientation.w", &state_message_.orientation.w));
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "angular_velocity.x", &state_message_.angular_velocity.x));
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "angular_velocity.y", &state_message_.angular_velocity.y));
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "angular_velocity.z", &state_message_.angular_velocity.z));
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "linear_acceleration.x", &state_message_.linear_acceleration.x));
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "linear_acceleration.y", &state_message_.linear_acceleration.y));
+  exported_state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      export_prefix, "linear_acceleration.z", &state_message_.linear_acceleration.z));
+
+  return exported_state_interfaces;
+}
+
 }  // namespace imu_sensor_broadcaster
 
 #include "pluginlib/class_list_macros.hpp"
 
 PLUGINLIB_EXPORT_CLASS(
-  imu_sensor_broadcaster::IMUSensorBroadcaster, controller_interface::ControllerInterface)
+  imu_sensor_broadcaster::IMUSensorBroadcaster, controller_interface::ChainableControllerInterface)
diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml
diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp
diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp
diff --git a/imu_sensor_broadcaster/test/test_imu_transform.cpp b/imu_sensor_broadcaster/test/test_imu_transform.cpp
