From c71a157ffc634da331d6a7ca015a8bf7eeb1409e Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Wed, 29 Nov 2023 19:52:55 +0900 Subject: [PATCH] fix: make AT128 scans align with scan_phase (#84) * Fix AT128 scan_angle handling Plot angle timings rel to ToS * Temporary diagnostic tools * Clean up code * Remove accidentally added directories from git * Roll back temporary changes, add comments * More doc comments * More clarifications * Adapt scan_phase for AT128 to pass unit tests * Fix new AT128 scan cutting * Scans are always cut at the start of a mirror now * Scan phase is sent as-is (in output coordinates) to the sensor, since it handles the conversion internally now. * Remove unused conversion functions * Add AT128 software version requirements to README * Revert unrelated formatting changes --------- Co-authored-by: Maximilian Schmeller --- README.md | 6 +++-- .../decoders/angle_corrector.hpp | 4 +++- .../angle_corrector_calibration_based.hpp | 10 ++++++-- .../angle_corrector_correction_based.hpp | 18 ++++++++------- .../decoders/hesai_decoder.hpp | 23 +++++++++---------- nebula_ros/config/hesai/PandarAT128.yaml | 2 +- 6 files changed, 37 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 9b7936eda..9aa0ce573 100644 --- a/README.md +++ b/README.md @@ -93,7 +93,7 @@ Supported models, where sensor_model is the ROS param to be used at launch: | HESAI | Pandar XT32M | PandarXT32M | PandarXT32M.yaml | :warning: | | HESAI | Pandar QT64 | PandarQT64 | PandarQT64.yaml | :heavy_check_mark: | | HESAI | Pandar QT128 | PandarQT128 | PandarQT128.yaml | :warning: | -| HESAI | Pandar AT128 | PandarAT128 | PandarAT128.yaml | :heavy_check_mark: | +| HESAI | Pandar AT128 | PandarAT128 | PandarAT128.yaml | :heavy_check_mark:* | | HESAI | Pandar 128E4X | Pandar128E4X | Pandar128E4X.yaml | :warning: | | Velodyne | VLP-16 | VLP16 | VLP16.yaml | :warning: | | Velodyne | VLP-16-HiRes | VLP16 | | :x: | @@ -103,7 +103,9 @@ Supported models, where sensor_model is the ROS param to be used at launch: Test status:\ :heavy_check_mark:: complete\ :warning:: some functionality yet to be tested\ -:x: : untested +:x:: untested\ +*: AT128 needs software version 3.50.8 or newer for the `scan_angle` setting to work correctly. + ## ROS parameters diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp index 7910884ef..42fd0a008 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp @@ -49,8 +49,10 @@ class AngleCorrector /// azimuth /// @param current_azimuth The current azimuth value in the sensor's angle resolution /// @param last_azimuth The last azimuth in the sensor's angle resolution + /// @param sync_azimuth The azimuth set in the sensor configuration, for which the + /// timestamp is aligned to the full second /// @return true if the current azimuth is in a different scan than the last one, false otherwise - virtual bool hasScanned(int current_azimuth, int last_azimuth) = 0; + virtual bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp index 4743d54f0..3e0c999b0 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp @@ -74,9 +74,15 @@ class AngleCorrectorCalibrationBased : public AngleCorrector elevation_cos_[channel_id]}; } - bool hasScanned(int current_azimuth, int last_azimuth) override + bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) override { - return current_azimuth < last_azimuth; + // Cut the scan when the azimuth passes over the sync_azimuth + uint32_t current_diff_from_sync = + (MAX_AZIMUTH_LEN + current_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; + uint32_t last_diff_from_sync = + (MAX_AZIMUTH_LEN + last_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; + + return current_diff_from_sync < last_diff_from_sync; } }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp index f97a4ca89..be60078de 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp @@ -84,16 +84,18 @@ class AngleCorrectorCorrectionBased : public AngleCorrector cos_[azimuth], sin_[elevation], cos_[elevation]}; } - bool hasScanned(int current_azimuth, int last_azimuth) override + bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override { - int field = findField(current_azimuth); - int last_field = findField(last_azimuth); - - // RCLCPP_DEBUG_STREAM( - // logger_, '{' << _(field) << _(last_field) << _(current_azimuth) << _(last_azimuth) << '}'); - return last_field != field; + // For AT128, the scan is always cut at the beginning of the field: + // If we would cut at `sync_azimuth`, the points left of it would be + // from the previous field and therefore significantly older than the + // points right of it. + // This also means that the pointcloud timestamp is only at top of second + // if the `sync_azimuth` aligns with the beginning of the field (e.g. 30deg for AT128). + // The absolute point time for points at `sync_azimuth` is still at top of second. + return findField(current_azimuth) != findField(last_azimuth); } }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula 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 ea317a699..cc589e929 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 @@ -140,7 +140,6 @@ class HesaiDecoder : public HesaiScanDecoder NebulaPoint point; point.distance = distance; point.intensity = unit.reflectivity; - // TODO(mojomex) add header offset to scan offset correction point.time_stamp = getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id); @@ -167,10 +166,12 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief Checks whether the last processed block was the last block of a scan /// @param current_phase The azimuth of the last processed block + /// @param sync_phase The azimuth set in the sensor configuration, for which the + /// timestamp is aligned to the full second /// @return Whether the scan has completed - bool checkScanCompleted(int current_phase) + bool checkScanCompleted(uint32_t current_phase, uint32_t sync_phase) { - return angle_corrector_.hasScanned(current_phase, last_phase_); + return angle_corrector_.hasScanned(current_phase, last_phase_, sync_phase); } /// @brief Get the distance of the given unit in meters @@ -233,17 +234,15 @@ class HesaiDecoder : public HesaiScanDecoder } const size_t n_returns = hesai_packet::get_n_returns(packet_.tail.return_mode); - int current_azimuth; + uint32_t current_azimuth; for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) { - current_azimuth = - (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS + - packet_.body.blocks[block_id].get_azimuth() - - static_cast( - sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS)) % - (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS); - - bool scan_completed = checkScanCompleted(current_azimuth); + current_azimuth = packet_.body.blocks[block_id].get_azimuth(); + + bool scan_completed = checkScanCompleted( + current_azimuth, + sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS); + if (scan_completed) { std::swap(decode_pc_, output_pc_); decode_pc_->clear(); diff --git a/nebula_ros/config/hesai/PandarAT128.yaml b/nebula_ros/config/hesai/PandarAT128.yaml index 4e4687495..5ad0027bb 100644 --- a/nebula_ros/config/hesai/PandarAT128.yaml +++ b/nebula_ros/config/hesai/PandarAT128.yaml @@ -7,7 +7,7 @@ data_port: 2368 # LiDAR Data Port gnss_port: 10110 # LiDAR GNSS Port return_mode: "LastStrongest" # See readme for supported return modes - scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] + scan_phase: 30.0 # Angle where scans begin (degrees, [30.,150.]) packet_mtu_size: 1500 # Packet MTU size rotation_speed: 200 # Motor RPM, the sensor's internal spin rate. cloud_min_angle: 0 # Field of View, start degrees.