refactor(nebula_decoders_hesai): implement generic Hesai decoder (aut…

…owarefoundation#42)

* Implement generic Hesai decoder [WIP]

Implement a templated class HesaiDecoder which can handle all currently
supported Hesai sensors.

* Reach geometric equivalence to previous implementation

* Refactor, correct timing correction (for most sensors)

* Revert raising limits for constexpr evaluation

* Hoist angle_corrector_t from packet structs to HesaiSensor

* Add support for firing time correction, fix return types

* Update design document for HesaiDecoder

* Refactor unit tests, add debug counters to HesaiDecoder

* Fix point filtering bugs, update unit tests

Unit tests now expect a `dual_return_distance_threshold == 0.1` and
the `<=` resp. `>=` operators for excluding points under/over a limit.

* Update unit test data to remove duplicate points, correct ordering

* Remove debug output, add more docs, improve unit tests

* Fix typos

* Update signature of getReturnType

* Fix limited precision lookup tables

* Increase distance precision

* Update unprecise unit test reference data

* Add azimuth--field LUT for AT128

* Reserve max size for hesai decoder pointclouds

* Revert from debug to original unit test checks

* Implement scan timestamp calculation mid-packet

If a scan is completed mid-packet, the new scan's timestamp is neither that of the current, nor that of the next packet.
Rather, it is the current packet's timestamp plus the minimum time offset
of any point in the remainder of the packet.

* Optimize Hesai decoder performance

Instead of separating azimuth/elevation correction and sin/cos lookups,
these values are now all returned by `getCorrectedAngleData`.

The `return_units` vector is now allocated once per return group,
instead of every unit.

`findField` for AT128 now contains not modulos or arithmetic and thus
is significantly faster.

* Update docs with generic decoder design

* Add guide for adding new Hesai sensors

* Delete debug scripts

* Fix bound for `findField` loop in angle corrector

* cspell. fix merge error

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

---------

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>
Co-authored-by: Maximilian Schmeller <maximilian.schmeller@tier4.jp>
Co-authored-by: Abraham Monrroy Cano <abrahammonrroy@yahoo.com>
Co-authored-by: amc-nu <abraham.monrroy@gmail.com>

.cspell.json

@@ -3,27 +3,32 @@
   "language": "en",
   "allowCompoundWords": true,
   "words": [
+    "adctp",
+    "Adctp",
+    "AT",
     "block_id",
-    "UDP_SEQ",
-    "STD_COUT",
-    "PANDARQT",
-    "PANDARXT",
-    "PANDARAT",
+    "gprmc",
+    "Hesai",
+    "memcpy",
+    "nohup",
+    "nproc",
     "pandar",
     "PANDAR",
-    "AT",
-    "XT",
-    "QT",
-    "XTM",
+    "PANDARAT",
+    "PANDARQT",
+    "PANDARXT",
     "Pdelay",
-    "gprmc",
-    "Vccint",
-    "vccint",
-    "adctp",
-    "Adctp",
+    "QT",
+    "schedutil",
+    "STD_COUT",
     "stds",
-    "nproc",
-    "nohup",
-    "schedutil"
+    "struct",
+    "structs",
+    "UDP_SEQ",
+    "vccint",
+    "Vccint",
+    "XT",
+    "XTM",
+    "mkdoxy"
   ]
 }

docs/hesai_decoder_design.md

@@ -0,0 +1,175 @@
+# Generic Hesai Decoder
+
+Since sensors from the same vendor often follow similar conventions when it comes to packet structure and data processing steps, a generic decoder can be used for most of the decoding work.
+This document outlines the requirements and design of the generic Hesai decoder.
+
+## Requirements
+
+There shall only be one decoder class which makes use of static (template) polymorphism to handle different sensor types.
+This way, runtime overhead for this generalization is `0`.
+
+### Packet formats
+
+For all handled Hesai sensors, the packet structure follows this rough format:
+1. (optional) header: static sensor info and feature flags
+2. body: point data
+3. tail and other appendices: timestamp, operation mode info
+
+### Decoding steps
+
+For all handled Hesai sensors, decoding a packet follows these steps:
+```python
+def unpack(packet):
+    parse_and_validate(packet)
+    # return group: one (single-return) or more (multi-return) 
+    # blocks that belong to the same azimuth
+    for return_group in packet:
+        if is_start_of_new_scan(return_group):
+            # swap output buffers etc.
+        decode(return_group)
+
+def decode(return_group):
+    for unit in return_group:
+        filter by:
+          distance thresholds
+          distance to other returns
+        correct azimuth/elevation *
+        compute x/y/z using sin/cos lookup tables *
+        compute time_offset to scan *
+        determine return_type *
+        append to pointcloud
+```
+
+The steps marked with __*__ are model-specific:
+
+* angle correction
+* timing correction
+* return type assignment
+
+### Angle correction
+
+There are two approaches between all the supported sensors:
+* Calibration file based
+* Correction file based (currently only used by AT128)
+
+For both approaches, sin/cos lookup tables can be computed.
+However, the resolution and calculation of these tables is different.
+
+#### Calibration based
+
+For each laser channel, a fixed elevation angle and azimuth angle offset are defined in the calibration file.
+Thus, sin/cos for elevation are only a function of the laser channel (not dependent on azimuth) while those for azimuth are a function of azimuth AND elevation.
+
+Lookup tables for elevation can thus be sized with `n_channels`, yielding a maximum size of 
+`128 * sizeof(float) = 512B` each.
+
+For azimuth, the size is `n_channels * n_azimuths = n_channels * 360 * azimuth_resolution <= 128 * 36000`.
+This yields a table size of `128 * 36000 * sizeof(float) ≈ 18.4MB`.
+
+#### Correction based
+
+While azimuth and elevation correction also have a per-channel component, an additional component depending on azimuth AND channel is present.
+The angular resolution of AT128 is `1 / (100 * 256) deg` and the per-channel correction as well as the additional component are defined as integers with the same or `1 / 100 deg` resolution respectively.
+This means that a lookup table of length `360 * 100 * 256` will contain sin/cos for all corrected values, since the resolution of corrections is smaller/equal to the base angular resolution.
+
+The lookup tables (of which there only need to be two: sin, cos; both usable for azimuth/elevation) each have a size of `360 * 100 * 256 * sizeof(float) ≈ 36.9MB`.
+
+### Timing correction
+
+Each sensor features an absolute timestamp per packet and formulae to compute the relative time between a unit or block and the packet.
+
+The desired output features a start timestamp per scan, and a relative timestamp to the scan for each point.
+
+Thus, the scan timestamp must be computed as the timestamp of the earliest point in the scan, and all packet-relative point times need to be converted to scan-relative ones.
+The earliest point in a scan is guaranteed to be in the first return group (≈ first 1-3 blocks) of the scan.
+Note that a scan can start mid-packet, if the scan phase does not align with packet bounds.
+
+The block offset follows a formula linear in the block index for all sensor models which additionally depends on the number of returns of the currently active `return_mode`. The parametrization is different for each sensor.
+
+The channel offset is given as a formula, table or set of tables for all sensors. A few sensors' formula is influenced by factors such as high resolution mode (128E3X, 128E4X), alternate firing sequences (QT128) and near/farfield firing (128E3X).
+
+### Return types
+
+While there is a wide range of different supported return modes (e.g. single (first), single (strongest), dual (first, last), etc.) their handling is largely the same.
+Differences only arise in multi-return (dual or triple) in the output order of the returns, and in the handling of some returns being duplicates (e.g. in dual(first, strongest), the first return coincides with the strongest one).
+
+Here is an exhaustive list of differences:
+* For Dual (First, Last) `0x3B`, 128E3X, 128E4X and XT32 reverse the output order (Last, First)
+* For Dual (Last, Strongest) `0x39`, all sensors except XT32M place the second strongest return in the even block if last == strongest
+* For Dual (First, Strongest) `0x3c`, the same as for `0x39` holds.
+
+For all other return modes, duplicate points are output if the two returns coincide.
+
+## Implementation
+
+### `HesaiPacket`
+
+Packets are defined as **packed** structs to enable parsing via `memcpy`.
+The sensor-specific layout for sensor XYZ is defined in `PacketXYZ` and usually employs an own `TailXYZ` struct.
+The header formats are largely shared between sensors.
+The packet body (i.e. point data) is mainly parameterized by bytes per point, points per block, and blocks per body. Thus, parameterized templated structs are used. A few skews such as fine azimuth blocks and blocks with a start-of-block (SOB) header exist and are implemented as their own structs.
+
+![HesaiPacket diagram](./GenericHesaiDecoder-Packet%20Formats.png)
+
+### `HesaiSensor`
+
+Each sensor model has its own class `PandarXYZ : HesaiSensor<...>` that defines packet type and timing, and return mode handling logic. Angle correction is the same for 90% of sensors and thus outsourced into `AngleCorrector` and subclasses. These are template arguments for `HesaiSensor`.
+Return mode handling has a default implementation that is supplemented by additional logic only in 3 sensors.
+
+### `AngleCorrector`
+
+The angle corrector has three main tasks:
+* compute corrected azimuth/elevation for given azimuth and channel
+* implement `hasScanCompleted()` logic that decides where one scan ends and the next starts
+* compute and provide lookup tables for sin/cos/etc.
+
+The two angle correction types are calibration-based and correction-based. In both approaches, a file from the sensor is used to extract the angle correction for each azimuth/channel.
+For all approaches, cos/sin lookup tables in the appropriate size are generated (see requirements section above).
+
+### `HesaiDecoder<SensorT>`
+
+The decoder is in charge of the control flow and shared decoding steps of all sensors.
+It is a template class taking a sensor type `SensorT` from which packet type, angle correction etc. are deducted at compile time.
+Thus, this unified decoder is an almost zero-cost abstraction.
+
+Its tasks are:
+* parsing an incoming packet
+* managing decode/output point buffers
+* converting all points in the packet using the sensor-specific functions of `SensorT` where necessary
+
+`HesaiDecoder<SensorT>` is a subclass of the existing `HesaiScanDecoder` to allow all template instantiations to be assigned to variables of the supertype.
+
+## Supporting a new sensor
+
+To support a new sensor model, first familiarize with the already implemented decoders.
+Then, consult the new sensor's datasheet and identify the following parameters:
+
+| Parameter | Chapter | Possible values | Notes |
+|-|-|-|-|
+| Header format | 3.1 | `Header12B`, `Header8B`, ... | `Header12B` is the standard and comprises the UDP pre-header and header (6+6B) mentioned in the data sheets |
+| Blocks per packet | 3.1 | `2`, `6`, `10`, ... | |
+| Number of channels | 3.1 | `32`, `40`, `64`, ... |  |
+| Unit format | 3.1 | `Unit3B`, `Unit4B`, ... |  |
+| Angle correction | App. 3 | `CALIBRATION`, `CORRECTION`, ... | The datasheet usually specifies whether a calibration/correction file is used |
+| Timing correction | App. 2 |  | There is usually a block and channel component. These come in the form of formulas/lookup tables. For most sensors, these depend on return mode and for some, features like high resolution mode, alternate firing etc. might change the timing |
+| Return type handling | 3.1 |  | Return modes are handled identically for most sensors but some re-order the returns or replace returns if there are duplicates |
+| Bytes per second | 1.4 | |  |
+| Lowest supported frequency | 1.4 | `5 Hz`, `10 Hz`, ... |  |
+
+| Chapter | Full title |
+|-|-|
+|1.4| Introduction > Specifications|
+|3.1| Data Structure > Point Cloud Data Packet|
+|App. 2| Absolute Time of Point Cloud Data|
+|App. 3| Angle Correction|
+
+With this information, create a `PacketMySensor` struct and `SensorMySensor` class.
+Reuse already-defined structs as much as possible (c.f. `Packet128E3X` and `Packet128E4X`).
+
+Implement timing correction in `SensorMySensor` and define the class constants `float MIN_RANGE`,
+`float MAX_RANGE` and `size_t MAX_SCAN_BUFFER_POINTS`.
+The former two are used for filtering out too-close and too-far away points while the latter is used to
+allocate pointcloud buffers. 
+Set `MAX_SCAN_BUFFER_POINTS = bytes_per_second / lowest_supported_frequency` from the parameters found above.
+
+If there are any non-standard features your sensor has, implement them as generically as possible to allow for future sensors to re-use your code.

mkdocs.yml

@@ -38,6 +38,7 @@ nav:
   - Design: design.md
   - About Nebula: about.md
   - Add Your Sensor: add_sensor.md
+  - Hesai Decoder Design: hesai_decoder_design.md
   - Nebula Common: nebula_common/links.md
   - Nebula Decoders: nebula_decoders/links.md
   - Nebula HW Interfaces: nebula_hw_interfaces/links.md

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -31,7 +31,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
 {
   os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port
      << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed
-     << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle;
+     << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle
+     << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold;
   return os;
 }
 
@@ -275,13 +276,23 @@ struct HesaiCorrection
   }
 
   static const int STEP3 = 200 * 256;
+
+  /// @brief Get azimuth adjustment for channel and precision azimuth
+  /// @param ch The channel id
+  /// @param azi The precision azimuth in (0.01 / 256) degree unit
+  /// @return The azimuth adjustment in 0.01 degree unit
   int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const
   {
     unsigned int i = std::floor(1.f * azi / STEP3);
     unsigned int l = azi - i * STEP3;
     float k = 1.f * l / STEP3;
     return round((1 - k) * azimuthOffset[ch * 180 + i] + k * azimuthOffset[ch * 180 + i + 1]);
   }
+
+  /// @brief Get elevation adjustment for channel and precision azimuth
+  /// @param ch The channel id
+  /// @param azi The precision azimuth in (0.01 / 256) degree unit
+  /// @return The elevation adjustment in 0.01 degree unit
   int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const
   {
     unsigned int i = std::floor(1.f * azi / STEP3);

nebula_common/include/nebula_common/nebula_common.hpp

@@ -25,7 +25,7 @@ enum class ReturnType : uint8_t {
   LAST_WEAK,
   IDENTICAL,
   SECOND,
-  SECOND_STRONGEST,
+  SECONDSTRONGEST,
   FIRST_STRONGEST,
   LAST_STRONGEST
 };
@@ -220,7 +220,7 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::ReturnType
     case ReturnType::SECOND:
       os << "Second";
       break;
-    case ReturnType::SECOND_STRONGEST:
+    case ReturnType::SECONDSTRONGEST:
       os << "SecondStrongest";
       break;
     case ReturnType::FIRST_STRONGEST:

nebula_decoders/CMakeLists.txt

@@ -29,15 +29,7 @@ include_directories(
 # Lidar Decoders
 # Hesai
 ament_auto_add_library(nebula_decoders_hesai SHARED
-        src/nebula_decoders_hesai/hesai_decoder.cpp
-        src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp
-        src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp
-        src/nebula_decoders_hesai/decoders/pandar_qt_64_decoder.cpp
-        src/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.cpp
-        src/nebula_decoders_hesai/decoders/pandar_xt_decoder.cpp
-        src/nebula_decoders_hesai/decoders/pandar_xtm_decoder.cpp
-        src/nebula_decoders_hesai/decoders/pandar_at_decoder.cpp
-        src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp
+        src/nebula_decoders_hesai/hesai_driver.cpp
         )
 
 # Velodyne

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "nebula_common/hesai/hesai_common.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <cstdint>
+
+namespace nebula
+{
+namespace drivers
+{
+
+struct CorrectedAngleData
+{
+  float azimuth_rad;
+  float elevation_rad;
+  float sin_azimuth;
+  float cos_azimuth;
+  float sin_elevation;
+  float cos_elevation;
+};
+
+/// @brief Handles angle correction for given azimuth/channel combinations, as well as trigonometry
+/// lookup tables
+class AngleCorrector
+{
+protected:
+  const std::shared_ptr<HesaiCalibrationConfiguration> sensor_calibration_;
+  const std::shared_ptr<HesaiCorrection> sensor_correction_;
+
+public:
+  AngleCorrector(
+    const std::shared_ptr<HesaiCalibrationConfiguration> & sensor_calibration,
+    const std::shared_ptr<HesaiCorrection> & sensor_correction)
+  : sensor_calibration_(sensor_calibration), sensor_correction_(sensor_correction)
+  {
+  }
+
+  /// @brief Get the corrected azimuth and elevation for a given block and channel, along with their
+  /// sin/cos values.
+  /// @param block_azimuth The block's azimuth (including optional fine azimuth), in the sensor's
+  /// angle unit
+  /// @param channel_id The laser channel's id
+  /// @return The corrected angles (azimuth, elevation) in radians and their sin/cos values
+  virtual CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) = 0;
+
+  /// @brief Returns true if the current azimuth lies in a different (new) scan compared to the last
+  /// 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
+  /// @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;
+};
+
+}  // namespace drivers
+}  // namespace nebula