diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp index e3eb200a9..09642e9cb 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp +++ b/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 & /* 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. diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp index e0ec24fa1..0b4c16031 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp +++ b/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 -struct BlockTimestampUsMixin: public PointTimestampMixin +struct BlockUnitIdBasedPointTimestampMixin : public PointTimestampMixin { - 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(getFieldValue(block->timestamp)) * 1000; + int32_t t_min = std::numeric_limits::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; } }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp index e09c86b77..90b615748 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp +++ b/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, - public PointTimestampMixin, - public robosense_packet::RobosensePacketTimestampMixin, - public ReturnModeMixin, - public BlockAziChannelElevMixin, - public BasicReflectivityMixin, - public DistanceMixin, - public BpearlChannelMixin, - public NonZeroDistanceIsValidMixin, - public AngleBasedScanCompletionMixin< - robosense_packet::bpearl_v3::Packet, RobosenseSensorConfiguration>, - public AngleCorrectorCalibrationBased +using BpearlV3Packet = robosense_packet::bpearl_v3::Packet; + +class BpearlV3 : public SensorBase, + public BlockUnitIdBasedPointTimestampMixin, + public robosense_packet::RobosensePacketTimestampMixin, + public ReturnModeMixin, + public BlockAziChannelElevMixin, + public BasicReflectivityMixin, + public DistanceMixin, + public BpearlChannelMixin, + public NonZeroDistanceIsValidMixin, + public AngleBasedScanCompletionMixin, + public AngleCorrectorCalibrationBased { private: static constexpr int firing_time_offset_ns_single_[12][32]{ diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp index 5adbc9838..f283a7f8c 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp +++ b/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, - public PointTimestampMixin, - public robosense_packet::RobosensePacketTimestampMixin, - public ReturnModeMixin, - public BlockAziChannelElevMixin, - public BasicReflectivityMixin, - public DistanceMixin, - public BpearlChannelMixin, - public NonZeroDistanceIsValidMixin, - public AngleBasedScanCompletionMixin< - robosense_packet::bpearl_v4::Packet, RobosenseSensorConfiguration>, - public AngleCorrectorCalibrationBased +using BpearlV4Packet = robosense_packet::bpearl_v4::Packet; + +class BpearlV4 : public SensorBase, + public BlockUnitIdBasedPointTimestampMixin, + public robosense_packet::RobosensePacketTimestampMixin, + public ReturnModeMixin, + public BlockAziChannelElevMixin, + public BasicReflectivityMixin, + public DistanceMixin, + public BpearlChannelMixin, + public NonZeroDistanceIsValidMixin, + public AngleBasedScanCompletionMixin, + public AngleCorrectorCalibrationBased { private: static constexpr int firing_time_offset_ns_single_[12][32] = { diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp index 16870bcdd..24c40a2d5 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp +++ b/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, - public PointTimestampMixin, - public robosense_packet::RobosensePacketTimestampMixin, - public ReturnModeMixin, - public BlockAziChannelElevMixin, - public BasicReflectivityMixin, - public DistanceMixin, - public ChannelIsUnitMixin, - public NonZeroDistanceIsValidMixin, - public AngleBasedScanCompletionMixin< - robosense_packet::helios::Packet, RobosenseSensorConfiguration>, - public AngleCorrectorCalibrationBased +: public SensorBase, + public BlockUnitIdBasedPointTimestampMixin, + public robosense_packet::RobosensePacketTimestampMixin, + public ReturnModeMixin, + public BlockAziChannelElevMixin, + public BasicReflectivityMixin, + public DistanceMixin, + public ChannelIsUnitMixin, + public NonZeroDistanceIsValidMixin, + public AngleBasedScanCompletionMixin, + public AngleCorrectorCalibrationBased { private: static constexpr int firing_time_offset_ns_single_[12][32] = { diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp index f91830af5..81b59288a 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp +++ b/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]) {