From 83e6cc8a545acfc5987383316a4c87884c10877f Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 26 Dec 2023 17:36:48 +0900 Subject: [PATCH] chore(nebula_decoders): move `RETURN_GROUP_STRIDE` to `PacketBase`, fix formatting, comments --- .../nebula_decoders_common/sensor.hpp | 12 ++-- .../decoders/bpearl_v3.hpp | 8 +-- .../decoders/bpearl_v4.hpp | 8 +-- .../decoders/helios.hpp | 2 - .../decoders/robosense_decoder.hpp | 69 ++++++++++--------- .../decoders/robosense_packet.hpp | 27 +++++--- 6 files changed, 70 insertions(+), 56 deletions(-) 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 bf01a36a2..e3eb200a9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp @@ -13,10 +13,8 @@ namespace drivers /// @brief Base class for all LiDAR sensors that are compatible with the generic decoder. template -class SensorBase +struct SensorBase { -private: -public: typedef PacketT packet_t; typedef typename packet_t::body_t body_t; typedef typename body_t::block_t block_t; @@ -58,8 +56,10 @@ class SensorBase } if ( - getFieldValue(return_units[return_idx]->distance) == getFieldValue(return_units[i]->distance) && - getFieldValue(return_units[return_idx]->reflectivity) == getFieldValue(return_units[i]->reflectivity)) { + getFieldValue(return_units[return_idx]->distance) == + getFieldValue(return_units[i]->distance) && + getFieldValue(return_units[return_idx]->reflectivity) == + getFieldValue(return_units[i]->reflectivity)) { return true; } } @@ -83,7 +83,7 @@ class SensorBase return ReturnType::IDENTICAL; } - // TODO(mojomex): this switch is not exhaustive + // TODO(mojomex): this is exhaustive only for Robosense. Extend to all ReturnModes in the future switch (return_mode) { case ReturnMode::SINGLE_FIRST: return ReturnType::FIRST; 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 38f430a13..e09c86b77 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 @@ -10,9 +10,9 @@ #include "nebula_decoders/nebula_decoders_common/sensor_mixins/validity.hpp" #include "nebula_decoders/nebula_decoders_common/util.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_common.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/sensor_info.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_common.hpp" #include "boost/endian/buffers.hpp" @@ -240,12 +240,12 @@ class BpearlV3 static constexpr float MAX_RANGE = 30.f; static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; - static constexpr std::array RETURN_GROUP_STRIDE = {0, 1, 0}; - BpearlV3( const std::shared_ptr sensor_config, const std::shared_ptr & calibration_config) - : BpearlChannelMixin(calibration_config), AngleBasedScanCompletionMixin(sensor_config), AngleCorrectorCalibrationBased(calibration_config) + : BpearlChannelMixin(calibration_config), + AngleBasedScanCompletionMixin(sensor_config), + AngleCorrectorCalibrationBased(calibration_config) { } 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 914b6a4e1..5adbc9838 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 @@ -11,9 +11,9 @@ #include "nebula_decoders/nebula_decoders_common/sensor_mixins/validity.hpp" #include "nebula_decoders/nebula_decoders_common/util.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_common.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/sensor_info.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_common.hpp" #include "boost/endian/buffers.hpp" @@ -210,12 +210,12 @@ class BpearlV4 static constexpr float MAX_RANGE = 30.f; static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; - static constexpr std::array RETURN_GROUP_STRIDE = {0, 1, 0}; - BpearlV4( const std::shared_ptr sensor_config, const std::shared_ptr & calibration_config) - : BpearlChannelMixin(calibration_config), AngleBasedScanCompletionMixin(sensor_config), AngleCorrectorCalibrationBased(calibration_config) + : BpearlChannelMixin(calibration_config), + AngleBasedScanCompletionMixin(sensor_config), + AngleCorrectorCalibrationBased(calibration_config) { } 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 b0af548ed..16870bcdd 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 @@ -258,8 +258,6 @@ class Helios static constexpr float MAX_RANGE = 150.f; static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; - static constexpr std::array RETURN_GROUP_STRIDE = {0, 1, 0}; - Helios( const std::shared_ptr sensor_config, const std::shared_ptr & calibration_config) 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 c2cfc2d89..f91830af5 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 @@ -33,18 +33,19 @@ using namespace nebula::drivers::sensor_mixins; template class RobosenseDecoder : public RobosenseScanDecoder { + using PacketT = typename SensorT::packet_t; // I want C++20 concepts :') - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); - static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); + static_assert(std::is_base_of_v, SensorT>); protected: /// @brief Configuration for this decoder @@ -62,9 +63,9 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @brief Some sensors need to decode return groups across multiple packets. This is dictated by /// whether or not their return groups are strided along the packet axis (0) static constexpr size_t decode_group_size_ = - SensorT::RETURN_GROUP_STRIDE[0] ? SensorT::packet_t::MAX_RETURNS : 1; + 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_; std::vector decode_group_timestamps_; /// @brief The timestamp of the last completed scan in nanoseconds @@ -82,10 +83,10 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @return Whether the packet was parsed successfully bool parsePacket(const robosense_msgs::msg::RobosensePacket & msop_packet) { - if (msop_packet.size < sizeof(typename SensorT::packet_t)) { + if (msop_packet.size < sizeof(PacketT)) { RCLCPP_ERROR_STREAM( logger_, "Packet size mismatch:" << msop_packet.size << " | Expected at least:" - << sizeof(typename SensorT::packet_t)); + << sizeof(PacketT)); return false; } @@ -114,7 +115,7 @@ class RobosenseDecoder : public RobosenseScanDecoder decode_group_.emplace_back(); // Guaranteed to be at packet_idx if (std::memcpy( &decode_group_[packet_idx], msop_packet.data.data(), - sizeof(typename SensorT::packet_t))) { + sizeof(PacketT))) { const auto & parsed_packet = decode_group_[packet_idx]; decode_group_timestamps_.emplace_back(sensor_.getPacketTimestamp(parsed_packet)); return true; @@ -135,19 +136,25 @@ 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 + // * 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 + // * if they were std::vectors, they would be dynamically allocated (this function runs *per + // point*, so this was slowing performance to ~25%) boost::container::static_vector< - const typename SensorT::packet_t::body_t::block_t::unit_t *, SensorT::packet_t::MAX_RETURNS> + const typename PacketT::body_t::block_t::unit_t *, PacketT::MAX_RETURNS> return_units{}; - boost::container::static_vector packet_idxs{}; - boost::container::static_vector block_idxs{}; - boost::container::static_vector unit_idxs{}; + boost::container::static_vector packet_idxs{}; + 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. for (size_t return_idx = 0; return_idx < n_returns; ++return_idx) { - size_t packet_idx = start_packet_id + return_idx * SensorT::RETURN_GROUP_STRIDE[0]; - size_t block_idx = start_block_id + return_idx * SensorT::RETURN_GROUP_STRIDE[1]; - size_t unit_idx = start_unit_id + return_idx * SensorT::RETURN_GROUP_STRIDE[2]; + 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]; + size_t unit_idx = start_unit_id + return_idx * PacketT::RETURN_GROUP_STRIDE[2]; packet_idxs[return_idx] = packet_idx; block_idxs[return_idx] = block_idx; @@ -250,11 +257,10 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @param unit_idx The channel index of the point /// @param return_mode The currently active return mode uint32_t getPointTimeRelative( - size_t packet_idx, size_t block_idx, size_t unit_idx, - ReturnMode return_mode) + size_t packet_idx, size_t block_idx, size_t unit_idx, ReturnMode return_mode) { - const auto point_to_packet_offset_ns = - sensor_.getPacketRelativeTimestamp(decode_group_[packet_idx], block_idx, unit_idx, return_mode); + 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]; auto packet_to_scan_offset_ns = static_cast(packet_timestamp_ns - decode_scan_timestamp_ns_); @@ -334,18 +340,19 @@ class RobosenseDecoder : public RobosenseScanDecoder // The advance/stride BETWEEN return groups. Not to be confused with RETURN_GROUP_STRIDE, which // is the stride between units WITHIN a return group. - std::array advance; + std::array advance; std::transform( - SensorT::RETURN_GROUP_STRIDE.begin(), SensorT::RETURN_GROUP_STRIDE.end(), advance.begin(), + PacketT::RETURN_GROUP_STRIDE.begin(), PacketT::RETURN_GROUP_STRIDE.end(), + advance.begin(), [=](bool is_strided_dimension) { return is_strided_dimension ? n_returns : 1; }); for (size_t packet_id = 0; packet_id < decode_group_.size(); packet_id += advance[0]) { - for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += advance[1]) { + 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); } - for (size_t channel_id = 0; channel_id < SensorT::packet_t::N_CHANNELS; + for (size_t channel_id = 0; channel_id < PacketT::N_CHANNELS; channel_id += advance[2]) { convertReturnGroup(packet_id, block_id, channel_id, n_returns, return_mode); } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp index bd74453b4..a3ca6ae7d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp @@ -1,14 +1,15 @@ #pragma once -#include "boost/endian/buffers.hpp" #include "nebula_common/robosense/robosense_common.hpp" #include "nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp" +#include "boost/endian/buffers.hpp" + #include #include -#include -#include #include +#include +#include using namespace boost::endian; @@ -21,7 +22,9 @@ namespace robosense_packet #pragma pack(push, 1) -struct RobosensePacket {}; +struct RobosensePacket +{ +}; struct Timestamp { @@ -49,7 +52,6 @@ struct RobosensePacketTimestampMixin : public sensor_mixins::PacketTimestampMixi } }; - struct Unit { big_uint16_buf_t distance; @@ -80,14 +82,22 @@ struct Body /// @tparam nChannels The number of channels per block /// @tparam maxReturns The maximum number of returns, e.g. 2 for dual return /// @tparam degreeSubdivisions The resolution of the azimuth angle in the packet, e.g. 100 if packet +/// @tparam returnsStridedPacket Whether returns within a return group are strided across packets +/// @tparam returnsStridedBlock Whether returns are strided across blocks +/// @tparam returnsStridedChannel Whether returns are strided across channels /// azimuth is given in 1/100th of a degree -template +template < + size_t nBlocks, size_t nChannels, size_t maxReturns, size_t degreeSubdivisions, + bool returnsStridedPacket = 0, bool returnsStridedBlock = 1, bool returnsStridedChannel = 0> struct PacketBase { static constexpr size_t N_BLOCKS = nBlocks; static constexpr size_t N_CHANNELS = nChannels; static constexpr size_t MAX_RETURNS = maxReturns; static constexpr size_t DEGREE_SUBDIVISIONS = degreeSubdivisions; + + static constexpr std::array RETURN_GROUP_STRIDE = { + returnsStridedPacket, returnsStridedBlock, returnsStridedChannel}; }; struct IpAddress @@ -152,9 +162,8 @@ struct ChannelAngleCorrection [[nodiscard]] float getAngle() const { - return sign.value() == ANGLE_SIGN_FLAG - ? static_cast(angle.value()) / 100.0f - : static_cast(angle.value()) / -100.0f; + return sign.value() == ANGLE_SIGN_FLAG ? static_cast(angle.value()) / 100.0f + : static_cast(angle.value()) / -100.0f; } };