diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/m1.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/m1.hpp index 35f44a935..77cc5ef9a 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/m1.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/m1.hpp @@ -64,7 +64,7 @@ struct M1Unit big_uint16_buf_t reserved; }; -struct Packet : public PacketBase<25, 5, 2, 100> +struct Packet : public PacketBase<25, 5, 2, 100, true, false, false> { typedef Body, Packet::N_BLOCKS> body_t; Header header; @@ -141,8 +141,6 @@ class M1 : public SensorBase, static constexpr float MAX_RANGE = 150.f; static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; - static constexpr std::array RETURN_GROUP_STRIDE{1, 0, 0}; - M1() { for (size_t i = 0; i < 65536; ++i) { 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 c0c3236a4..397ba6650 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 @@ -66,7 +66,7 @@ class RobosenseDecoder : public RobosenseScanDecoder PacketT::RETURN_GROUP_STRIDE[0] ? PacketT::MAX_RETURNS : 1; /// @brief The current group of packets being decoded. std::vector decode_group_; - std::vector decode_group_timestamps_; + std::vector decode_group_timestamps_ns_; /// @brief The timestamp of the last completed scan in nanoseconds uint64_t output_scan_timestamp_ns_; @@ -137,7 +137,9 @@ class RobosenseDecoder : public RobosenseScanDecoder size_t start_packet_id, size_t start_block_id, size_t start_unit_id, size_t n_returns, ReturnMode return_mode) { - // These are Boost static_vectors because + // Find the units corresponding to the same return group as the current one. + // These are used to find duplicates in multi-return mode. + // Using Boost static_vectors because // * if they were std::arrays, we would have to drag along a `size` variable as `n_returns` is // variable // and variable-length arrays cannot be statically allocated @@ -150,13 +152,6 @@ class RobosenseDecoder : public RobosenseScanDecoder boost::container::static_vector block_idxs{}; boost::container::static_vector unit_idxs{}; - // Find the units corresponding to the same return group as the current one. - // These are used to find duplicates in multi-return mode. - boost::container::static_vector return_units{}; - boost::container::static_vector packet_idxs{}; - boost::container::static_vector block_idxs{}; - boost::container::static_vector unit_idxs{}; - for (size_t return_idx = 0; return_idx < n_returns; ++return_idx) { size_t packet_idx = start_packet_id + return_idx * PacketT::RETURN_GROUP_STRIDE[0]; size_t block_idx = start_block_id + return_idx * PacketT::RETURN_GROUP_STRIDE[1]; @@ -259,7 +254,7 @@ class RobosenseDecoder : public RobosenseScanDecoder { const auto point_to_packet_offset_ns = sensor_.getPacketRelativeTimestamp( decode_group_[packet_idx], block_idx, unit_idx, return_mode); - const auto packet_timestamp_ns = decode_group_timestamps_[packet_idx]; + const auto packet_timestamp_ns = decode_group_timestamps_ns_[packet_idx]; auto packet_to_scan_offset_ns = static_cast(packet_timestamp_ns - decode_scan_timestamp_ns_); return packet_to_scan_offset_ns + point_to_packet_offset_ns; @@ -276,7 +271,7 @@ class RobosenseDecoder : public RobosenseScanDecoder // calculated as the packet timestamp plus the lowest time offset of any point in the // remainder of the packet decode_scan_timestamp_ns_ = - decode_group_timestamps_[packet_id] + + decode_group_timestamps_ns_[packet_id] + sensor_.getEarliestPointTimeOffsetForScan(decode_group_[packet_id], block_id, return_mode); }