fix(robosense): make scan timestamping work with sensor mixins

nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp

@@ -29,18 +29,6 @@ struct SensorBase
   /// @return The index [0, num_decode_groups) within the decode group to which the packet belongs
   virtual size_t getDecodeGroupIndex(const uint8_t * const /* raw_packet */) const { return 0; }
 
-  /// @brief For a given start block index, find the earliest (lowest) relative time offset of any
-  /// point in the packet in or after the start block
-  /// @param start_block_id The index of the block in and after which to consider points
-  /// @param sensor_configuration The sensor configuration
-  /// @return The lowest point time offset (relative to the packet timestamp) of any point in or
-  /// after the start block, in nanoseconds
-  int getEarliestPointTimeOffsetForBlock(
-    uint32_t /* start_block_id */, const std::shared_ptr<const SensorConfigurationBase> & /* sensor_configuration */)
-  {
-    return 0;  // TODO(mojomex): implement
-  }
-
   /// @brief Whether the unit given by return_idx is a duplicate of any other unit in return_units
   /// @param return_idx The unit's index in the return_units vector
   /// @param return_units The vector of all the units corresponding to the same return group (i.e.

nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp

@@ -30,18 +30,35 @@ struct PointTimestampMixin
   virtual int32_t getPacketRelativeTimestamp(
     const PacketT & packet, const size_t block_id, const size_t channel_id,
     const ReturnMode return_mode) const = 0;
+
+  /// @brief For a given start block index, find the earliest (lowest) relative time offset of any
+  /// point in the packet in or after the start block, in nanoseconds
+  virtual int32_t getEarliestPointTimeOffsetForScan(
+    const PacketT & packet, const size_t block_id, const ReturnMode return_mode) const = 0;
 };
 
-/// @brief Return the timestamp field of each block as the timestamp of the block's units
+/// @brief Determine the start timestamp of the next scan based on the lowest point timestamp in the
+/// next return group. This mixin checks all point timestamps in the return group starting at
+/// `block_id` and returns the lowest one.
 template <typename PacketT>
-struct BlockTimestampUsMixin: public PointTimestampMixin<PacketT>
+struct BlockUnitIdBasedPointTimestampMixin : public PointTimestampMixin<PacketT>
 {
-  int32_t getPacketRelativeTimestamp(
-    const PacketT & packet, const size_t block_id, const size_t /* channel_id */,
-    const ReturnMode /* return_mode */) const override
+  virtual int32_t getPacketRelativeTimestamp(
+    const PacketT & packet, const size_t block_id, const size_t channel_id,
+    const ReturnMode return_mode) const = 0;
+
+  int32_t getEarliestPointTimeOffsetForScan(
+    const PacketT & packet, const size_t block_id, const ReturnMode return_mode) const override
   {
-    const auto * block = getBlock(packet, block_id);
-    return static_cast<int32_t>(getFieldValue(block->timestamp)) * 1000;
+    int32_t t_min = std::numeric_limits<int>::max();
+    auto n_returns = ReturnModeToNReturns(return_mode);
+    for (size_t blk = block_id; blk < block_id + n_returns; ++blk) {
+      for (size_t ch = 0; ch < PacketT::N_CHANNELS; ++ch) {
+        t_min = std::min(t_min, getPacketRelativeTimestamp(packet, blk, ch, return_mode));
+      }
+    }
+
+    return t_min;
   }
 };
 

nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp

@@ -146,20 +146,19 @@ struct InfoPacket
 }  // namespace robosense_packet
 
 using namespace sensor_mixins;
-
-class BpearlV3
-: public SensorBase<robosense_packet::bpearl_v3::Packet>,
-  public PointTimestampMixin<robosense_packet::bpearl_v3::Packet>,
-  public robosense_packet::RobosensePacketTimestampMixin<robosense_packet::bpearl_v3::Packet>,
-  public ReturnModeMixin<robosense_packet::bpearl_v3::Packet>,
-  public BlockAziChannelElevMixin<robosense_packet::bpearl_v3::Packet>,
-  public BasicReflectivityMixin<robosense_packet::bpearl_v3::Packet>,
-  public DistanceMixin<robosense_packet::bpearl_v3::Packet>,
-  public BpearlChannelMixin<robosense_packet::bpearl_v3::Packet>,
-  public NonZeroDistanceIsValidMixin<robosense_packet::bpearl_v3::Packet>,
-  public AngleBasedScanCompletionMixin<
-    robosense_packet::bpearl_v3::Packet, RobosenseSensorConfiguration>,
-  public AngleCorrectorCalibrationBased<robosense_packet::bpearl_v3::Packet>
+using BpearlV3Packet = robosense_packet::bpearl_v3::Packet;
+
+class BpearlV3 : public SensorBase<BpearlV3Packet>,
+                 public BlockUnitIdBasedPointTimestampMixin<BpearlV3Packet>,
+                 public robosense_packet::RobosensePacketTimestampMixin<BpearlV3Packet>,
+                 public ReturnModeMixin<BpearlV3Packet>,
+                 public BlockAziChannelElevMixin<BpearlV3Packet>,
+                 public BasicReflectivityMixin<BpearlV3Packet>,
+                 public DistanceMixin<BpearlV3Packet>,
+                 public BpearlChannelMixin<BpearlV3Packet>,
+                 public NonZeroDistanceIsValidMixin<BpearlV3Packet>,
+                 public AngleBasedScanCompletionMixin<BpearlV3Packet, RobosenseSensorConfiguration>,
+                 public AngleCorrectorCalibrationBased<BpearlV3Packet>
 {
 private:
   static constexpr int firing_time_offset_ns_single_[12][32]{

nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp

@@ -113,20 +113,19 @@ struct InfoPacket
 }  // namespace robosense_packet
 
 using namespace sensor_mixins;
-
-class BpearlV4
-: public SensorBase<robosense_packet::bpearl_v4::Packet>,
-  public PointTimestampMixin<robosense_packet::bpearl_v4::Packet>,
-  public robosense_packet::RobosensePacketTimestampMixin<robosense_packet::bpearl_v4::Packet>,
-  public ReturnModeMixin<robosense_packet::bpearl_v4::Packet>,
-  public BlockAziChannelElevMixin<robosense_packet::bpearl_v4::Packet>,
-  public BasicReflectivityMixin<robosense_packet::bpearl_v4::Packet>,
-  public DistanceMixin<robosense_packet::bpearl_v4::Packet>,
-  public BpearlChannelMixin<robosense_packet::bpearl_v4::Packet>,
-  public NonZeroDistanceIsValidMixin<robosense_packet::bpearl_v4::Packet>,
-  public AngleBasedScanCompletionMixin<
-    robosense_packet::bpearl_v4::Packet, RobosenseSensorConfiguration>,
-  public AngleCorrectorCalibrationBased<robosense_packet::bpearl_v4::Packet>
+using BpearlV4Packet = robosense_packet::bpearl_v4::Packet;
+
+class BpearlV4 : public SensorBase<BpearlV4Packet>,
+                 public BlockUnitIdBasedPointTimestampMixin<BpearlV4Packet>,
+                 public robosense_packet::RobosensePacketTimestampMixin<BpearlV4Packet>,
+                 public ReturnModeMixin<BpearlV4Packet>,
+                 public BlockAziChannelElevMixin<BpearlV4Packet>,
+                 public BasicReflectivityMixin<BpearlV4Packet>,
+                 public DistanceMixin<BpearlV4Packet>,
+                 public BpearlChannelMixin<BpearlV4Packet>,
+                 public NonZeroDistanceIsValidMixin<BpearlV4Packet>,
+                 public AngleBasedScanCompletionMixin<BpearlV4Packet, RobosenseSensorConfiguration>,
+                 public AngleCorrectorCalibrationBased<BpearlV4Packet>
 {
 private:
   static constexpr int firing_time_offset_ns_single_[12][32] = {

nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp

@@ -159,20 +159,20 @@ struct InfoPacket
 }  // namespace robosense_packet
 
 using namespace sensor_mixins;
+using HeliosPacket = robosense_packet::helios::Packet;
 
 class Helios
-: public SensorBase<robosense_packet::helios::Packet>,
-  public PointTimestampMixin<robosense_packet::helios::Packet>,
-  public robosense_packet::RobosensePacketTimestampMixin<robosense_packet::helios::Packet>,
-  public ReturnModeMixin<robosense_packet::helios::Packet>,
-  public BlockAziChannelElevMixin<robosense_packet::helios::Packet>,
-  public BasicReflectivityMixin<robosense_packet::helios::Packet>,
-  public DistanceMixin<robosense_packet::helios::Packet>,
-  public ChannelIsUnitMixin<robosense_packet::helios::Packet>,
-  public NonZeroDistanceIsValidMixin<robosense_packet::helios::Packet>,
-  public AngleBasedScanCompletionMixin<
-    robosense_packet::helios::Packet, RobosenseSensorConfiguration>,
-  public AngleCorrectorCalibrationBased<robosense_packet::helios::Packet>
+: public SensorBase<HeliosPacket>,
+  public BlockUnitIdBasedPointTimestampMixin<HeliosPacket>,
+  public robosense_packet::RobosensePacketTimestampMixin<HeliosPacket>,
+  public ReturnModeMixin<HeliosPacket>,
+  public BlockAziChannelElevMixin<HeliosPacket>,
+  public BasicReflectivityMixin<HeliosPacket>,
+  public DistanceMixin<HeliosPacket>,
+  public ChannelIsUnitMixin<HeliosPacket>,
+  public NonZeroDistanceIsValidMixin<HeliosPacket>,
+  public AngleBasedScanCompletionMixin<HeliosPacket, RobosenseSensorConfiguration>,
+  public AngleCorrectorCalibrationBased<HeliosPacket>
 {
 private:
   static constexpr int firing_time_offset_ns_single_[12][32] = {

nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp

@@ -267,7 +267,7 @@ class RobosenseDecoder : public RobosenseScanDecoder
     return packet_to_scan_offset_ns + point_to_packet_offset_ns;
   }
 
-  void onScanCompleted(size_t packet_id, size_t block_id)
+  void onScanCompleted(size_t packet_id, size_t block_id, ReturnMode return_mode)
   {
     std::swap(decode_pc_, output_pc_);
     decode_pc_->clear();
@@ -279,7 +279,7 @@ class RobosenseDecoder : public RobosenseScanDecoder
     // remainder of the packet
     decode_scan_timestamp_ns_ =
       decode_group_timestamps_[packet_id] +
-      sensor_.getEarliestPointTimeOffsetForBlock(block_id, sensor_configuration_);
+      sensor_.getEarliestPointTimeOffsetForScan(decode_group_[packet_id], block_id, return_mode);
   }
 
 public:
@@ -350,7 +350,7 @@ class RobosenseDecoder : public RobosenseScanDecoder
       for (size_t block_id = 0; block_id < PacketT::N_BLOCKS; block_id += advance[1]) {
         bool scan_completed = sensor_.checkScanCompleted(decode_group_[packet_id], block_id);
         if (scan_completed) {
-          onScanCompleted(packet_id, block_id);
+          onScanCompleted(packet_id, block_id, return_mode);
         }
         for (size_t channel_id = 0; channel_id < PacketT::N_CHANNELS;
              channel_id += advance[2]) {