feat: add IMU decoding support to OT128

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -57,6 +57,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase
   bool hires_mode;
   std::optional<uint32_t> blockage_mask_horizontal_bin_size_mdeg;
   std::optional<std::string> sync_diagnostics_topic;
+  bool imu_enabled;
 };
 /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator)
 /// @param os
@@ -96,6 +97,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
   os << "Synchronization Diagnostics: "
      << (arg.sync_diagnostics_topic ? ("enabled, topic: " + arg.sync_diagnostics_topic.value())
                                     : "disabled");
+  os << '\n';
+  os << "IMU: " << (arg.imu_enabled ? "enabled" : "disabled");
   return os;
 }
 
@@ -579,6 +582,19 @@ inline int int_from_return_mode_hesai(
   return -1;
 }
 
+/// @brief Whether the given sensor model supports IMU
+/// @param sensor_model Sensor model
+/// @return True if the sensor model supports IMU, false otherwise
+inline bool supports_imu(const SensorModel & sensor_model)
+{
+  switch (sensor_model) {
+    case SensorModel::HESAI_PANDAR128_E4X:
+      return true;
+    default:
+      return false;
+  }
+}
+
 /// @brief Whether the given sensor model supports functional safety
 /// @param sensor_model Sensor model
 /// @return True if the sensor model supports functional safety, false otherwise

nebula_common/include/nebula_common/imu_types.hpp

@@ -0,0 +1,41 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// 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.
+
+#pragma once
+
+#include <cstdint>
+
+namespace nebula::drivers
+{
+
+/// @brief A timestamped IMU reading.
+struct ImuReading
+{
+  /// @brief Absolute timestamp in nanoseconds
+  uint64_t absolute_timestamp_ns{0};
+  /// @brief Acceleration in m/s^2 on the IMU X axis
+  float accel_mps2_x{0.0F};
+  /// @brief Acceleration in m/s^2 on the IMU Y axis
+  float accel_mps2_y{0.0F};
+  /// @brief Acceleration in m/s^2 on the IMU Z axis
+  float accel_mps2_z{0.0F};
+  /// @brief Angular velocity in rad/s around the IMU X axis
+  float ang_vel_rps_x{0.0F};
+  /// @brief Angular velocity in rad/s around the IMU Y axis
+  float ang_vel_rps_y{0.0F};
+  /// @brief Angular velocity in rad/s around the IMU Z axis
+  float ang_vel_rps_z{0.0F};
+};
+
+}  // namespace nebula::drivers

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp

@@ -21,6 +21,7 @@
 #include "nebula_decoders/nebula_decoders_hesai/decoders/functional_safety.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"
+#include "nebula_decoders/nebula_decoders_hesai/decoders/imu.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/packet_loss_detector.hpp"
 
 #include <nebula_common/hesai/hesai_common.hpp>
@@ -73,6 +74,9 @@ class HesaiDecoder : public HesaiScanDecoder
   /// @brief Decodes azimuth/elevation angles given calibration/correction data
   typename SensorT::angle_corrector_t angle_corrector_;
 
+  /// @brief Decodes IMU data for supported sensors
+  std::shared_ptr<ImuDecoderTypedBase<typename SensorT::packet_t>> imu_decoder_{};
+
   /// @brief Decodes functional safety data for supported sensors
   std::shared_ptr<FunctionalSafetyDecoderTypedBase<typename SensorT::packet_t>>
     functional_safety_decoder_;
@@ -318,6 +322,7 @@ class HesaiDecoder : public HesaiScanDecoder
     const std::shared_ptr<const typename SensorT::angle_corrector_t::correction_data_t> &
       correction_data,
     const std::shared_ptr<loggers::Logger> & logger,
+    const std::shared_ptr<ImuDecoderTypedBase<typename SensorT::packet_t>> & imu_decoder,
     const std::shared_ptr<FunctionalSafetyDecoderTypedBase<typename SensorT::packet_t>> &
       functional_safety_decoder,
     const std::shared_ptr<PacketLossDetectorTypedBase<typename SensorT::packet_t>> &
@@ -327,6 +332,7 @@ class HesaiDecoder : public HesaiScanDecoder
     angle_corrector_(
       correction_data, sensor_configuration_->cloud_min_angle,
       sensor_configuration_->cloud_max_angle, sensor_configuration_->cut_angle),
+    imu_decoder_(imu_decoder),
     functional_safety_decoder_(functional_safety_decoder),
     packet_loss_detector_(packet_loss_detector),
     scan_cut_angles_(
@@ -368,6 +374,11 @@ class HesaiDecoder : public HesaiScanDecoder
       functional_safety_decoder_->update(packet_);
     }
 
+    // Decode IMU via plugin if present
+    if (imu_decoder_) {
+      imu_decoder_->update(packet_);
+    }
+
     // FYI: This is where the CRC would be checked. Since this caused performance issues in the
     // past, and since the frame check sequence of the packet is already checked by the NIC, we skip
     // it here.

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp

@@ -265,4 +265,25 @@ struct HasPacketLossDetection<
 {
 };
 
+// Helper trait to determine if a given packet tail contains IMU fields
+template <typename PacketT, typename = void>
+struct HasImu : std::false_type
+{
+};
+
+template <typename PacketT>
+struct HasImu<
+  PacketT, std::void_t<
+             decltype(std::declval<PacketT>().tail.imu_timestamp),
+             decltype(std::declval<PacketT>().tail.imu_acceleration_unit),
+             decltype(std::declval<PacketT>().tail.imu_angular_velocity_unit),
+             decltype(std::declval<PacketT>().tail.imu_x_axis_acceleration),
+             decltype(std::declval<PacketT>().tail.imu_y_axis_acceleration),
+             decltype(std::declval<PacketT>().tail.imu_z_axis_acceleration),
+             decltype(std::declval<PacketT>().tail.imu_x_axis_angular_velocity),
+             decltype(std::declval<PacketT>().tail.imu_y_axis_angular_velocity),
+             decltype(std::declval<PacketT>().tail.imu_z_axis_angular_velocity)>> : std::true_type
+{
+};
+
 }  // namespace nebula::drivers::hesai_packet

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp

@@ -16,6 +16,7 @@
 #define NEBULA_WS_HESAI_SCAN_DECODER_HPP
 
 #include <nebula_common/hesai/hesai_common.hpp>
+#include <nebula_common/imu_types.hpp>
 #include <nebula_common/point_types.hpp>
 #include <nebula_common/util/expected.hpp>
 
@@ -62,6 +63,8 @@ class HesaiScanDecoder
   using pointcloud_callback_t =
     std::function<void(const NebulaPointCloudPtr & pointcloud, double timestamp_s)>;
 
+  using imu_callback_t = std::function<void(const ImuReading & imu)>;
+
   HesaiScanDecoder(HesaiScanDecoder && c) = delete;
   HesaiScanDecoder & operator=(HesaiScanDecoder && c) = delete;
   HesaiScanDecoder(const HesaiScanDecoder & c) = delete;
@@ -77,6 +80,8 @@ class HesaiScanDecoder
   virtual PacketDecodeResult unpack(const std::vector<uint8_t> & packet) = 0;
 
   virtual void set_pointcloud_callback(pointcloud_callback_t callback) = 0;
+
+  virtual void set_imu_callback(imu_callback_t /* callback */) {}
 };
 }  // namespace nebula::drivers
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/imu.hpp

@@ -0,0 +1,87 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// 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.
+
+#pragma once
+
+#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp"
+#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"
+
+#include <nebula_common/imu_types.hpp>
+
+#include <cstdint>
+#include <functional>
+#include <utility>
+
+namespace nebula::drivers
+{
+
+class ImuDecoderBase
+{
+public:
+  using imu_cb_t = HesaiScanDecoder::imu_callback_t;
+  virtual ~ImuDecoderBase() = default;
+};
+
+template <typename PacketT>
+class ImuDecoderTypedBase : public ImuDecoderBase
+{
+public:
+  virtual void update(const PacketT & /*packet*/) {}
+  virtual void set_callback(imu_cb_t /*cb*/) {}
+};
+
+template <typename PacketT>
+class ImuDecoder : public ImuDecoderTypedBase<PacketT>
+{
+  static constexpr float g = 9.80665f;
+
+public:
+  using imu_cb_t = typename ImuDecoderBase::imu_cb_t;
+
+  void set_callback(imu_cb_t cb) override { on_imu_ = std::move(cb); }
+
+  void update(const PacketT & packet) override
+  {
+    if (!on_imu_) return;
+    uint32_t imu_ts = packet.tail.imu_timestamp;
+    if (imu_ts == last_imu_timestamp_) return;
+    last_imu_timestamp_ = imu_ts;
+
+    // Convert units to SI
+    // Acceleration: A * U * 0.001 mg -> mg -> m/s^2
+    const float accel_scale =
+      static_cast<float>(packet.tail.imu_acceleration_unit) * 0.001f * 1e-3f * g;
+    // Angular velocity: W * U * 0.01 mdps -> dps -> rad/s
+    const float ang_scale_deg =
+      static_cast<float>(packet.tail.imu_angular_velocity_unit) * 0.01f * 1e-3f;
+    const float ang_scale = deg2rad(ang_scale_deg);
+
+    ImuReading reading;
+    reading.absolute_timestamp_ns = hesai_packet::get_timestamp_ns(packet);
+    reading.accel_mps2_x = static_cast<float>(packet.tail.imu_x_axis_acceleration) * accel_scale;
+    reading.accel_mps2_y = static_cast<float>(packet.tail.imu_y_axis_acceleration) * accel_scale;
+    reading.accel_mps2_z = static_cast<float>(packet.tail.imu_z_axis_acceleration) * accel_scale;
+    reading.ang_vel_rps_x = static_cast<float>(packet.tail.imu_x_axis_angular_velocity) * ang_scale;
+    reading.ang_vel_rps_y = static_cast<float>(packet.tail.imu_y_axis_angular_velocity) * ang_scale;
+    reading.ang_vel_rps_z = static_cast<float>(packet.tail.imu_z_axis_angular_velocity) * ang_scale;
+
+    on_imu_(reading);
+  }
+
+private:
+  uint32_t last_imu_timestamp_{0};
+  imu_cb_t on_imu_;
+};
+
+}  // namespace nebula::drivers

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp

@@ -52,12 +52,12 @@ struct Tail128E3X
   uint16_t imu_acceleration_unit;
   uint16_t imu_angular_velocity_unit;
   uint32_t imu_timestamp;
-  uint16_t imu_x_axis_acceleration;
-  uint16_t imu_y_axis_acceleration;
-  uint16_t imu_z_axis_acceleration;
-  uint16_t imu_x_axis_angular_velocity;
-  uint16_t imu_y_axis_angular_velocity;
-  uint16_t imu_z_axis_angular_velocity;
+  int16_t imu_x_axis_acceleration;
+  int16_t imu_y_axis_acceleration;
+  int16_t imu_z_axis_acceleration;
+  int16_t imu_x_axis_angular_velocity;
+  int16_t imu_y_axis_angular_velocity;
+  int16_t imu_z_axis_angular_velocity;
 
   uint32_t crc_tail;
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp

@@ -18,6 +18,9 @@
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp"
 
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/Transform.h>
+
 #include <iostream>
 #include <vector>
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp

@@ -52,7 +52,7 @@ class HesaiDriver
     const std::shared_ptr<const drivers::HesaiSensorConfiguration> & sensor_configuration,
     const std::shared_ptr<const drivers::HesaiCalibrationConfigurationBase> &
       calibration_configuration,
-    FunctionalSafetyDecoderBase::alive_cb_t alive_cb,
+    HesaiScanDecoder::imu_callback_t imu_cb, FunctionalSafetyDecoderBase::alive_cb_t alive_cb,
     FunctionalSafetyDecoderBase::stuck_cb_t stuck_cb,
     FunctionalSafetyDecoderBase::status_cb_t status_cb, PacketLossDetectorBase::lost_cb_t lost_cb,
     std::shared_ptr<point_filters::BlockageMaskPlugin> blockage_mask_plugin = nullptr);
@@ -118,6 +118,7 @@ class HesaiDriver
       calibration_configuration,
     const std::shared_ptr<loggers::Logger> & logger,
     HesaiScanDecoder::pointcloud_callback_t pointcloud_cb,
+    HesaiScanDecoder::imu_callback_t imu_cb = nullptr,
     FunctionalSafetyDecoderBase::alive_cb_t alive_cb = nullptr,
     FunctionalSafetyDecoderBase::stuck_cb_t stuck_cb = nullptr,
     FunctionalSafetyDecoderBase::status_cb_t status_cb = nullptr,