Skip to content

Commit

Permalink
fix(robosense): fix compiler error for decoder
Browse files Browse the repository at this point in the history
  • Loading branch information
mojomex committed Dec 26, 2023
1 parent b31f880 commit aaed2d6
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 14 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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<M1Block<M1Unit, Packet::N_CHANNELS>, Packet::N_BLOCKS> body_t;
Header header;
Expand Down Expand Up @@ -141,8 +141,6 @@ class M1 : public SensorBase<robosense_packet::m1::Packet>,
static constexpr float MAX_RANGE = 150.f;
static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000;

static constexpr std::array<bool, 3> RETURN_GROUP_STRIDE{1, 0, 0};

M1()
{
for (size_t i = 0; i < 65536; ++i) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<PacketT> decode_group_;
std::vector<uint64_t> decode_group_timestamps_;
std::vector<uint64_t> decode_group_timestamps_ns_;

/// @brief The timestamp of the last completed scan in nanoseconds
uint64_t output_scan_timestamp_ns_;
Expand Down Expand Up @@ -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
Expand All @@ -150,13 +152,6 @@ class RobosenseDecoder : public RobosenseScanDecoder
boost::container::static_vector<size_t, PacketT::MAX_RETURNS> block_idxs{};
boost::container::static_vector<size_t, PacketT::MAX_RETURNS> 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<const unit_t *, packet_t::MAX_RETURNS> return_units{};
boost::container::static_vector<size_t, packet_t::MAX_RETURNS> packet_idxs{};
boost::container::static_vector<size_t, packet_t::MAX_RETURNS> block_idxs{};
boost::container::static_vector<size_t, packet_t::MAX_RETURNS> 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];
Expand Down Expand Up @@ -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<uint32_t>(packet_timestamp_ns - decode_scan_timestamp_ns_);
return packet_to_scan_offset_ns + point_to_packet_offset_ns;
Expand All @@ -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);
}

Expand Down

0 comments on commit aaed2d6

Please sign in to comment.