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 <[email protected]>

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
 

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

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;
   }
 };
 

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
+}  // namespace nebula

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<int>(
-           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();

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.