diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index f00bce664..808b21b1d 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -57,6 +57,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase bool hires_mode; std::optional blockage_mask_horizontal_bin_size_mdeg; std::optional 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 diff --git a/nebula_common/include/nebula_common/imu_types.hpp b/nebula_common/include/nebula_common/imu_types.hpp new file mode 100644 index 000000000..08a430bf4 --- /dev/null +++ b/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 + +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 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 2f387da0f..37c41dc3f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/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 @@ -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> imu_decoder_{}; + /// @brief Decodes functional safety data for supported sensors std::shared_ptr> functional_safety_decoder_; @@ -318,6 +322,7 @@ class HesaiDecoder : public HesaiScanDecoder const std::shared_ptr & correction_data, const std::shared_ptr & logger, + const std::shared_ptr> & imu_decoder, const std::shared_ptr> & functional_safety_decoder, const std::shared_ptr> & @@ -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. diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp index 189791e8a..1bd5392fc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp +++ b/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 +struct HasImu : std::false_type +{ +}; + +template +struct HasImu< + PacketT, std::void_t< + decltype(std::declval().tail.imu_timestamp), + decltype(std::declval().tail.imu_acceleration_unit), + decltype(std::declval().tail.imu_angular_velocity_unit), + decltype(std::declval().tail.imu_x_axis_acceleration), + decltype(std::declval().tail.imu_y_axis_acceleration), + decltype(std::declval().tail.imu_z_axis_acceleration), + decltype(std::declval().tail.imu_x_axis_angular_velocity), + decltype(std::declval().tail.imu_y_axis_angular_velocity), + decltype(std::declval().tail.imu_z_axis_angular_velocity)>> : std::true_type +{ +}; + } // namespace nebula::drivers::hesai_packet diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp index 50ec7e41b..b6de6af85 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp +++ b/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 +#include #include #include @@ -62,6 +63,8 @@ class HesaiScanDecoder using pointcloud_callback_t = std::function; + using imu_callback_t = std::function; + 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 & packet) = 0; virtual void set_pointcloud_callback(pointcloud_callback_t callback) = 0; + + virtual void set_imu_callback(imu_callback_t /* callback */) {} }; } // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/imu.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/imu.hpp new file mode 100644 index 000000000..bbf53b856 --- /dev/null +++ b/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 + +#include +#include +#include + +namespace nebula::drivers +{ + +class ImuDecoderBase +{ +public: + using imu_cb_t = HesaiScanDecoder::imu_callback_t; + virtual ~ImuDecoderBase() = default; +}; + +template +class ImuDecoderTypedBase : public ImuDecoderBase +{ +public: + virtual void update(const PacketT & /*packet*/) {} + virtual void set_callback(imu_cb_t /*cb*/) {} +}; + +template +class ImuDecoder : public ImuDecoderTypedBase +{ + 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(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(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(packet.tail.imu_x_axis_acceleration) * accel_scale; + reading.accel_mps2_y = static_cast(packet.tail.imu_y_axis_acceleration) * accel_scale; + reading.accel_mps2_z = static_cast(packet.tail.imu_z_axis_acceleration) * accel_scale; + reading.ang_vel_rps_x = static_cast(packet.tail.imu_x_axis_angular_velocity) * ang_scale; + reading.ang_vel_rps_y = static_cast(packet.tail.imu_y_axis_angular_velocity) * ang_scale; + reading.ang_vel_rps_z = static_cast(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 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp index 50fe0a5ce..f0e257e65 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp +++ b/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; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 8e76c3227..192fac9b0 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/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 +#include + #include #include diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index e3a144864..9e2b8edd9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -52,7 +52,7 @@ class HesaiDriver const std::shared_ptr & sensor_configuration, const std::shared_ptr & 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 blockage_mask_plugin = nullptr); @@ -118,6 +118,7 @@ class HesaiDriver calibration_configuration, const std::shared_ptr & 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, diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 19718f26e..7e1363e57 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -27,7 +27,7 @@ HesaiDriver::HesaiDriver( const std::shared_ptr & sensor_configuration, const std::shared_ptr & calibration_data, const std::shared_ptr & logger, - HesaiScanDecoder::pointcloud_callback_t pointcloud_cb, + HesaiScanDecoder::pointcloud_callback_t pointcloud_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, @@ -40,46 +40,46 @@ HesaiDriver::HesaiDriver( switch (sensor_configuration->sensor_model) { case SensorModel::HESAI_PANDAR64: scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb); + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb); break; case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR40M: scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb); + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb); break; case SensorModel::HESAI_PANDARQT64: scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb); + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb); break; case SensorModel::HESAI_PANDARQT128: { scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb); + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb); break; } case SensorModel::HESAI_PANDARXT16: scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb); + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb); break; case SensorModel::HESAI_PANDARXT32: scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb); + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb); break; case SensorModel::HESAI_PANDARXT32M: scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb); + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb); break; case SensorModel::HESAI_PANDARAT128: scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb); + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb); break; case SensorModel::HESAI_PANDAR128_E3X: { scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb); + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb); break; } case SensorModel::HESAI_PANDAR128_E4X: { scan_decoder_ = initialize_decoder( - sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb, + sensor_configuration, calibration_data, imu_cb, alive_cb, stuck_cb, status_cb, lost_cb, std::move(blockage_mask_plugin)); break; } @@ -92,6 +92,9 @@ HesaiDriver::HesaiDriver( } scan_decoder_->set_pointcloud_callback(std::move(pointcloud_cb)); + if (imu_cb) { + scan_decoder_->set_imu_callback(std::move(imu_cb)); + } } template @@ -99,7 +102,7 @@ std::shared_ptr HesaiDriver::initialize_decoder( const std::shared_ptr & sensor_configuration, const std::shared_ptr & 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 blockage_mask_plugin) @@ -109,11 +112,19 @@ std::shared_ptr HesaiDriver::initialize_decoder( initialize_functional_safety_decoder(alive_cb, stuck_cb, status_cb, sensor_rpm); auto packet_loss_detector = initialize_packet_loss_detector(lost_cb); + std::shared_ptr> imu_decoder; + if constexpr (hesai_packet::HasImu::value) { + auto decoder = std::make_shared>(); + if (imu_cb) decoder->set_callback(std::move(imu_cb)); + imu_decoder = decoder; + } + using CalibT = typename SensorT::angle_corrector_t::correction_data_t; - return std::make_shared>( + auto decoder = std::make_shared>( sensor_configuration, std::dynamic_pointer_cast(calibration_configuration), - logger_->child("Decoder"), functional_safety_decoder, packet_loss_detector, + logger_->child("Decoder"), imu_decoder, functional_safety_decoder, packet_loss_detector, std::move(blockage_mask_plugin)); + return decoder; } PacketDecodeResult HesaiDriver::parse_cloud_packet(const std::vector & packet) diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml index 449495246..497da8563 100644 --- a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml @@ -29,6 +29,8 @@ retry_hw: true dual_return_distance_threshold: 0.1 hires_mode: true + imu: + enabled: true diagnostics: pointcloud_publish_rate: frequency_ok: diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp index 9e62317fd..73a83079f 100644 --- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -32,6 +32,7 @@ #include #include +#include #include #include @@ -141,6 +142,8 @@ class HesaiDecoderWrapper NEBULA_PUBLISHER_PTR(sensor_msgs::msg::PointCloud2) aw_points_ex_pub_; NEBULA_PUBLISHER_PTR(sensor_msgs::msg::PointCloud2) aw_points_base_pub_; + NEBULA_PUBLISHER_PTR(sensor_msgs::msg::Imu) imu_pub_; + NEBULA_PUBLISHER_PTR(sensor_msgs::msg::Image) blockage_mask_pub_; custom_diagnostic_tasks::RateBoundStatus publish_diagnostic_; diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index 922814ccf..84eb96d1c 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/nebula_ros/schema/Pandar128E4X.schema.json @@ -117,6 +117,17 @@ "blockage_mask_output": { "$ref": "sub/lidar_hesai.json#/definitions/blockage_mask_output" }, + "imu": { + "type": "object", + "properties": { + "enabled": { + "type": "boolean", + "default": true, + "description": "Enable IMU output" + } + }, + "additionalProperties": false + }, "diagnostics": { "$ref": "sub/lidar_hesai.json#/definitions/diagnostics", "required": [ diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 9c90d110e..3aa851eb2 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -3,6 +3,7 @@ #include "nebula_ros/hesai/decoder_wrapper.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/functional_safety.hpp" +#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" #include "nebula_ros/common/agnocast_wrapper/nebula_agnocast_wrapper.hpp" #include "nebula_ros/common/rclcpp_logger.hpp" #include "nebula_ros/hesai/diagnostics/functional_safety_diagnostic_task.hpp" @@ -87,6 +88,12 @@ HesaiDecoderWrapper::HesaiDecoderWrapper( aw_points_ex_pub_ = NEBULA_CREATE_PUBLISHER2( sensor_msgs::msg::PointCloud2, &parent_node_, "aw_points_ex", pointcloud_qos); + // IMU publisher (optional) + if (sensor_cfg_->imu_enabled && drivers::supports_imu(sensor_cfg_->sensor_model)) { + imu_pub_ = NEBULA_CREATE_PUBLISHER2( + sensor_msgs::msg::Imu, &parent_node_, "imu", rclcpp::SensorDataQoS()); + } + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); diagnostic_updater.add(publish_diagnostic_); @@ -305,10 +312,31 @@ std::shared_ptr HesaiDecoderWrapper::initialize_driver( lost_cb = [this](uint64_t n_lost) { packet_loss_diagnostic_->on_lost(n_lost); }; } + drivers::HesaiScanDecoder::imu_callback_t imu_cb; + + if (sensor_cfg_->imu_enabled && drivers::supports_imu(sensor_cfg_->sensor_model)) { + imu_cb = [this](const auto & imu) { + if (NEBULA_HAS_ANY_SUBSCRIPTIONS(imu_pub_) == 0) { + return; + } + + auto msg = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(imu_pub_); + msg->header.stamp = rclcpp::Time(imu.absolute_timestamp_ns); + msg->header.frame_id = sensor_cfg_->frame_id; + msg->linear_acceleration.x = imu.accel_mps2_x; + msg->linear_acceleration.y = imu.accel_mps2_y; + msg->linear_acceleration.z = imu.accel_mps2_z; + msg->angular_velocity.x = imu.ang_vel_rps_x; + msg->angular_velocity.y = imu.ang_vel_rps_y; + msg->angular_velocity.z = imu.ang_vel_rps_z; + imu_pub_->publish(std::move(msg)); + }; + } + return std::make_shared( config, calibration, std::make_shared(logger_), - std::move(pointcloud_cb), std::move(alive_cb), std::move(stuck_cb), std::move(status_cb), - std::move(lost_cb), std::move(blockage_mask_plugin)); + std::move(pointcloud_cb), std::move(imu_cb), std::move(alive_cb), std::move(stuck_cb), + std::move(status_cb), std::move(lost_cb), std::move(blockage_mask_plugin)); } nebula::Status HesaiDecoderWrapper::status() diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index acb9185fd..0c0255b5b 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -179,6 +179,11 @@ nebula::Status HesaiRosWrapper::declare_and_get_sensor_config_params() config.hires_mode = this->declare_parameter("hires_mode", param_read_write()); } + // IMU settings (enabled flag) + if (drivers::supports_imu(config.sensor_model)) { + config.imu_enabled = this->declare_parameter("imu.enabled", false, param_read_only()); + } + { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); RCLCPP_DEBUG_STREAM(get_logger(), config.sensor_model);