From 9b56534dd66e6d1c035c2401245f00cc2c095aa3 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 5 Dec 2023 14:56:11 +0900 Subject: [PATCH 01/17] fix(robosense): update launch IPs and ports to match datasheets --- nebula_ros/launch/robosense_launch.all_hw.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nebula_ros/launch/robosense_launch.all_hw.xml b/nebula_ros/launch/robosense_launch.all_hw.xml index 8a3f73037..4d36c99ff 100644 --- a/nebula_ros/launch/robosense_launch.all_hw.xml +++ b/nebula_ros/launch/robosense_launch.all_hw.xml @@ -7,10 +7,10 @@ - - - - + + + + From aa5e1f4b54b48cb7e21f9989593576dd9ea8de73 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 5 Dec 2023 14:57:49 +0900 Subject: [PATCH 02/17] fix(robosense): handle unsupported sensors in GetChannelSize switch --- .../include/nebula_common/robosense/robosense_common.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp index e92302f2d..cfca5fe7e 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -62,6 +62,8 @@ size_t GetChannelSize(const SensorModel & model) return 32; case SensorModel::ROBOSENSE_HELIOS: return 32; + default: + throw std::runtime_error("Unknown sensor model"); } } From 905c9c3b548376e89fa0d51c0bd69a9ffa2befbb Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 5 Dec 2023 15:00:28 +0900 Subject: [PATCH 03/17] fix(robosense): reduce logging output, especially when drivers are initializing --- .../robosense/robosense_decoder_ros_wrapper.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp index d779e3bc6..8f6f713a0 100644 --- a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp @@ -61,10 +61,13 @@ void RobosenseDriverRosWrapper::ReceiveScanMsgCallback( std::const_pointer_cast(sensor_cfg_ptr_)); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } + + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Driver not initialized."); + return; } if (!is_received_info) { - RCLCPP_WARN_STREAM(this->get_logger(), "Waiting for info packet."); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Waiting for info packet."); return; } @@ -74,6 +77,10 @@ void RobosenseDriverRosWrapper::ReceiveScanMsgCallback( driver_ptr_->ConvertScanToPointcloud(scan_msg); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); + if (!driver_ptr_->HasScanned()) { + return; + }; + if (pointcloud == nullptr) { RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); return; @@ -124,7 +131,8 @@ void RobosenseDriverRosWrapper::ReceiveInfoMsgCallback( } if (!info_driver_ptr_) { - RCLCPP_WARN_STREAM(this->get_logger(), "Info driver has not been initialized yet."); + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Info driver has not been initialized yet."); return; } @@ -175,7 +183,7 @@ Status RobosenseDriverRosWrapper::InitializeDriver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration) { - RCLCPP_INFO_STREAM(this->get_logger(), "Initializing driver..."); + RCLCPP_INFO(this->get_logger(), "Initializing driver..."); driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), std::static_pointer_cast( @@ -227,7 +235,7 @@ Status RobosenseDriverRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); + this->declare_parameter("data_port", 6699, descriptor); sensor_configuration.data_port = this->get_parameter("data_port").as_int(); } { From b25e981f9fca6e09816a8f303c8db8da46ab21a9 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 5 Dec 2023 15:31:11 +0900 Subject: [PATCH 04/17] refactor(nebula_decoders): add composable sensors and mixins Currently there is a lot of code duplication happening between sensor models, e.g. for accessing specific fields, determining return types, etc. Furthermore, exotic/non-standard sensors require extra branches or parameters in the decoder. This commit introduces sensor mixins and generic field accessors, which together can be used to compose a wide range of sensor implementations. Currently handled functionalities: * Generic accessors for packet blocks and units * Generic accessors for boost::endian and native struct members * Mixin for getting point azimuth/elevation * Mixin for getting channel IDs * Mixin for getting distance units, distances * Mixin for getting point intensity * Mixin for getting sensor return mode * Mixin for checking scan completion (not limited to angle-based checks) * Mixin for getting packet and point timestamps * Mixin for point pre-validation (e.g. distance !=0 etc.) Sensors implemented using this system must inherit from SensorPase, as well as from all of the used generic/specific mixins they are composed of. --- .../nebula_decoders_common/sensor.hpp | 107 ++++++++++++++++++ .../sensor_mixins/angles.hpp | 87 ++++++++++++++ .../sensor_mixins/channel.hpp | 38 +++++++ .../sensor_mixins/distance.hpp | 48 ++++++++ .../sensor_mixins/intensity.hpp | 41 +++++++ .../sensor_mixins/return_mode.hpp | 39 +++++++ .../sensor_mixins/scan_completion.hpp | 57 ++++++++++ .../sensor_mixins/timestamp.hpp | 50 ++++++++ .../sensor_mixins/validity.hpp | 40 +++++++ .../nebula_decoders_common/util.hpp | 46 ++++++++ 10 files changed, 553 insertions(+) create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/intensity.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/return_mode.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/scan_completion.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/validity.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/util.hpp diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp new file mode 100644 index 000000000..383205317 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp @@ -0,0 +1,107 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_decoders/nebula_decoders_common/util.hpp" + +#include +#include + +namespace nebula +{ +namespace drivers +{ + +/// @brief Base class for all LiDAR sensors that are compatible with the generic decoder. +template +class SensorBase +{ +private: +public: + typedef PacketT packet_t; + typedef typename packet_t::body_t body_t; + typedef typename body_t::block_t block_t; + typedef typename block_t::unit_t unit_t; + + SensorBase() = default; + virtual ~SensorBase() = default; + + /// @brief Decide whereto to decode the given raw packet. This is only non-zero for sensors that + /// split return groups across multiple packets + /// @param raw_packet The raw bytes to be decoded + /// @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. + /// length 2 for dual-return with both units having the same channel but coming from different + /// blocks) + /// @return true if the unit is identical to any other one in return_units, false otherwise + template + static bool isDuplicate(const CollectionT & return_units, const size_t return_idx) + { + for (size_t i = 0; i < return_units.size(); ++i) { + if (i == return_idx) { + continue; + } + + if ( + return_units[return_idx]->distance.value() == return_units[i]->distance.value() && + return_units[return_idx]->reflectivity.value() == return_units[i]->reflectivity.value()) { + return true; + } + } + + return false; + } + + /// @brief Get the return type of the point given by return_idx + /// + /// @param return_units The units corresponding to all the returns in the group. These are usually + /// from the same column across adjascent blocks. + /// @param return_idx The block index of the point within the group of blocks that make up the + /// return group (e.g. either 0 or 1 for dual return) + /// @param return_mode The sensor's currently active return mode + /// @return The return type of the point + template + static ReturnType getReturnType( + const CollectionT & return_units, const size_t return_idx, const ReturnMode & return_mode) + { + if (isDuplicate(return_units, return_idx)) { + return ReturnType::IDENTICAL; + } + + // TODO(mojomex): this switch is not exhaustive + switch (return_mode) { + case ReturnMode::SINGLE_FIRST: + return ReturnType::FIRST; + case ReturnMode::SINGLE_LAST: + return ReturnType::LAST; + case ReturnMode::SINGLE_STRONGEST: + return ReturnType::STRONGEST; + case ReturnMode::DUAL: + if (return_idx == 0) { + return ReturnType::STRONGEST; + } else { + return ReturnType::LAST; + } + default: + return ReturnType::UNKNOWN; + } + } +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp new file mode 100644 index 000000000..8574caa04 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_common/util.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace sensor_mixins +{ + +struct CorrectedAngleData +{ + float azimuth_rad; + float elevation_rad; + float sin_azimuth; + float cos_azimuth; + float sin_elevation; + float cos_elevation; +}; + +/// @brief Retrieves raw angles for a given unit. The angles do not have to be in any particular unit +/// as long as they will be corrected by a suitable @ref AngleCorrectorMixin +template +struct AnglesMixin +{ + /// @brief Retrieves the raw (pre-correction) azimuth of a given unit + virtual int32_t getRawAzimuth( + const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0; + + /// @brief Retrieves the raw (pre-correction) elevation of a given unit + virtual int32_t getRawElevation( + const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0; +}; + +/// @brief Converts raw angles into corrected ones, along with their trigonometric functions. +template +struct AngleCorrectorMixin +{ + /// @brief Corrects the raw input anglesand computes their sin/cos + virtual CorrectedAngleData getCorrectedAngleData(int32_t raw_azimuth, int32_t raw_elevation) const = 0; +}; + +/// @brief Reads raw azimuth from a block's `azimuth` field and returns the unit id as elevation +template +struct BlockAziChannelElevMixin : public AnglesMixin +{ + int32_t getRawAzimuth( + const PacketT & packet, const size_t block_id, const size_t /* unit_id */) const override + { + const auto * block = getBlock(packet, block_id); + return getFieldValue(block->azimuth); + } + + int32_t getRawElevation( + const PacketT & /* packet */, const size_t /* block_id */, const size_t unit_id) const override + { + return static_cast(unit_id); + } +}; + +/// @brief Reads azimuth and elevation from fields in the given unit +template +struct AnglesInUnitMixin : public AnglesMixin +{ + int32_t getRawAzimuth( + const PacketT & packet, const size_t block_id, const size_t unit_id) const override + { + const auto * unit = getUnit(packet, block_id, unit_id); + return getFieldValue(unit->azimuth); + } + + int32_t getRawElevation( + const PacketT & packet, const size_t block_id, const size_t unit_id) const override + { + const auto * unit = getUnit(packet, block_id, unit_id); + return getFieldValue(unit->elevation); + } +}; + +} // namespace sensor_mixins +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp new file mode 100644 index 000000000..a9810e632 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_common/util.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace sensor_mixins +{ + +/// @brief Retrieves the channel ID (laser ID) of a given unit +template +struct ChannelMixin +{ + /// @brief Retrieves the channel ID (laser ID) of a given unit + virtual uint8_t getChannel( + const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0; +}; + +/// @brief Returns the given unit ID as channel ID +template +struct ChannelIsUnitMixin: public ChannelMixin +{ + uint8_t getChannel( + const PacketT & /* packet */, const size_t /* block_id */, const size_t unit_id) const override + { + return unit_id; + } +}; + +} // namespace sensor_mixins +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp new file mode 100644 index 000000000..efb7968d4 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_common/util.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace sensor_mixins +{ + +/// @brief Retrieves the packet's distance unit and calculates unit distances in meters +template +struct DistanceMixin +{ + /// @brief Returns the value to multiply distance values in the packet with to get meters. + virtual double getDistanceUnit(const PacketT & packet) const = 0; + + /// @brief Returns the distance value of the given unit in meters. + virtual double getDistance( + const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0; +}; + +/// @brief Reads the distance unit from the packet header and distance from each unit's `distance` +/// field. +template +struct HeaderMmDisUnitMixin : public DistanceMixin +{ + double getDistanceUnit(const PacketT & packet) const override + { + return packet.header.dis_unit / 1000.; + } + + double getDistance( + const PacketT & packet, const size_t block_id, const size_t unit_id) const override + { + const auto * unit = getUnit(packet, block_id, unit_id); + return getFieldValue(unit->distance) * getDistanceUnit(packet); + } +}; + +} // namespace sensor_mixins +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/intensity.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/intensity.hpp new file mode 100644 index 000000000..87890953b --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/intensity.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_common/util.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace sensor_mixins +{ + +/// @brief Retrieves and, if needed, converts the intensity of a given unit to the range [0, 255]. +/// Intensities from 0-100 refer to diffuse reflections whereas intensities from 101-255 +/// refer to retroreflections. +template +struct IntensityMixin +{ + /// @brief Returns the intensity of the given unit. + virtual uint8_t getIntensity( + const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0; +}; + +/// @brief Reads the intensity from a unit's `reflectivity` field and does no correction. +template +struct BasicReflectivityMixin : public IntensityMixin +{ + uint8_t getIntensity( + const PacketT & packet, const size_t block_id, const size_t unit_id) const override + { + const auto * unit = getUnit(packet, block_id, unit_id); + return static_cast(getFieldValue(unit->reflectivity)); + } +}; + +} // namespace sensor_mixins +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/return_mode.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/return_mode.hpp new file mode 100644 index 000000000..d02b65076 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/return_mode.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_decoders/nebula_decoders_common/util.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace sensor_mixins +{ + +/// @brief Retrieves the current return mode of the sensor +template +struct ReturnModeMixin +{ + /// @brief Retrieves the current return mode from a given packet and/or sensor configuration + virtual ReturnMode getReturnMode( + const PacketT & packet, const SensorConfigurationBase & config) const = 0; +}; + +/// @brief Returns the return mode from the sensor configuration +template +struct ReturnModeFromConfigMixin : public ReturnModeMixin +{ + ReturnMode getReturnMode( + const PacketT & /* packet */, const SensorConfigurationBase & config) const override + { + return config.return_mode; + } +}; + +} // namespace sensor_mixins +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/scan_completion.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/scan_completion.hpp new file mode 100644 index 000000000..05b8a7cf8 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/scan_completion.hpp @@ -0,0 +1,57 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_decoders/nebula_decoders_common/util.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace sensor_mixins +{ + +/// @brief Checks whether a given block is the start of a new scan. +template +struct ScanCompletionMixin +{ + /// @brief Returns whether the given block is the start of a new scan. + virtual bool checkScanCompleted(const PacketT & packet, const size_t block_id) = 0; +}; + +/// @brief Check scan completion based on the azimuth of the current block. +/// This mixin also reacts to `scan_phase` configuration, scans will be cut at the given +/// `scan_phase` angle. +template +struct AngleBasedScanCompletionMixin : public ScanCompletionMixin +{ + AngleBasedScanCompletionMixin(const std::shared_ptr & config) + : sensor_configuration_(config) + { + } + + bool checkScanCompleted(const PacketT & packet, const size_t block_id) override + { + const auto * block = getBlock(packet, block_id); + uint32_t current_azimuth = + (360 * PacketT::DEGREE_SUBDIVISIONS + block->get_azimuth() - + static_cast(sensor_configuration_->scan_phase * PacketT::DEGREE_SUBDIVISIONS)) % + (360 * PacketT::DEGREE_SUBDIVISIONS); + + bool scan_complete = current_azimuth < last_azimuth_; + + last_azimuth_ = current_azimuth; + return scan_complete; + } + +private: + uint32_t last_azimuth_{}; + const std::shared_ptr sensor_configuration_; +}; + +} // namespace sensor_mixins +} // namespace drivers +} // namespace nebula \ No newline at end of file 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 new file mode 100644 index 000000000..e0ec24fa1 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_decoders/nebula_decoders_common/util.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace sensor_mixins +{ + +/// @brief Retrieves a packet's timestamp +template +struct PacketTimestampMixin +{ + /// @brief Returns the timestamp of the packet in nanoseconds + virtual uint64_t getPacketTimestamp(const PacketT & packet) const = 0; +}; + +/// @brief Retrieves a point's timestamp relative to the packet's timestamp +template +struct PointTimestampMixin +{ + /// @brief Returns the packet-relative timestamp of the given unit, in nanoseconds + virtual int32_t getPacketRelativeTimestamp( + const PacketT & packet, const size_t block_id, const size_t channel_id, + const ReturnMode return_mode) const = 0; +}; + +/// @brief Return the timestamp field of each block as the timestamp of the block's units +template +struct BlockTimestampUsMixin: public PointTimestampMixin +{ + int32_t getPacketRelativeTimestamp( + const PacketT & packet, const size_t block_id, const size_t /* channel_id */, + const ReturnMode /* return_mode */) const override + { + const auto * block = getBlock(packet, block_id); + return static_cast(getFieldValue(block->timestamp)) * 1000; + } +}; + +} // namespace sensor_mixins +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/validity.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/validity.hpp new file mode 100644 index 000000000..aa0d34878 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/validity.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_common/util.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace sensor_mixins +{ + +/// @brief Briefly checks if a given unit is valid and should be decoded. +/// This check is designed to be fast and should not perform any expensive calculations. +template +struct ValidityMixin +{ + /// @brief Checks if the given unit can/should be decoded (e.g. if not all fields are 0) + virtual bool isValid( + const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0; +}; + +/// @brief Reports units as valid if their distance field is non-zero. +template +struct NonZeroDistanceIsValidMixin : public ValidityMixin +{ + bool isValid( + const PacketT & packet, const size_t block_id, const size_t unit_id) const override + { + const auto * unit = getUnit(packet, block_id, unit_id); + return getFieldValue(unit->distance) != 0; + } +}; + +} // namespace sensor_mixins +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/util.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/util.hpp new file mode 100644 index 000000000..58cb63848 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/util.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ + +/// @brief Get a pointer to the block identified by `block_id` in the given packet. +template +const typename PacketT::body_t::block_t * getBlock(const PacketT & packet, const size_t block_id) +{ + return &packet.body.blocks[block_id]; +} + +/// @brief Get a pointer to the unit identified by `block_id` and `unit_id` in the given packet. +template +const typename PacketT::body_t::block_t::unit_t * getUnit( + const PacketT & packet, const size_t block_id, const size_t unit_id) +{ + return &packet.body.blocks[block_id].units[unit_id]; +} + +/// @brief Field accessor for Boost endian buffers. Can be used to generically access fields across +/// packet types. +/// @return t.value() +template +typename T::value_type getFieldValue(T t) +{ + return t.value(); +} + +/// @brief Field accessor for primitive types. Can be used to generically access fields across +/// packet types. +/// @return t (as-is) +template +std::enable_if_t, T> getFieldValue(T t) +{ + return t; +} + +} // namespace drivers +} // namespace nebula \ No newline at end of file From 775a3b1ccb66e7b2c675dc8a2e5fa679a62dc351 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 5 Dec 2023 16:00:14 +0900 Subject: [PATCH 05/17] refactor(robosense): move Helios, Bpearl to composable sensor implementation, rewrite decoder In anticipation of the Robosense M1 PR, the `robosense_decoder.hpp` had to become more generic to support non-angle-based scan cutting along with completely different mechanisms for angle data, dual return and timestamps. Thus, Robosense is the first vendor to be moved to the new composable sensor model, starting with Helios and Bpearl. * Introduce decode groups: multiple packets can be grouped and decoded together, as is necessary for RSM1 * Packet/Block/Unit stride definition for generically handling different return group layouts * Decoder now uses generic sensor member functions to access all point data * Rewrite sensor classes to use SensorBase and mixins * Add SensorInfo classes for the info decoder * Currently, timing and return types are not fully re-implemented * `angle_corrector.hpp` is deleted in favor of sensor mixins * The driver now instantiates sensor objects passed to the decoder --- .../include/nebula_common/nebula_common.hpp | 29 ++ .../robosense/robosense_common.hpp | 12 - .../decoders/angle_corrector.hpp | 57 --- .../angle_corrector_calibration_based.hpp | 60 +-- .../decoders/bpearl_common.hpp | 54 +++ .../decoders/bpearl_v3.hpp | 118 ++++-- .../decoders/bpearl_v4.hpp | 135 ++++--- .../decoders/helios.hpp | 131 ++++--- .../decoders/robosense_decoder.hpp | 354 +++++++++++------- .../decoders/robosense_info_decoder.hpp | 19 +- .../decoders/robosense_info_decoder_base.hpp | 10 +- .../decoders/robosense_packet.hpp | 43 +-- .../decoders/robosense_scan_decoder.hpp | 4 + .../decoders/robosense_sensor.hpp | 124 ------ .../decoders/sensor_info.hpp | 50 +++ .../robosense_driver.hpp | 9 +- .../robosense_info_driver.hpp | 2 +- .../robosense_driver.cpp | 44 ++- .../robosense_info_driver.cpp | 8 +- .../robosense_hw_interface.cpp | 23 +- .../robosense_msgs/msg/RobosensePacket.msg | 3 +- .../robosense_decoder_ros_wrapper.cpp | 8 +- 22 files changed, 735 insertions(+), 562 deletions(-) delete mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_common.hpp delete mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/sensor_info.hpp diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index c75359aa8..3199efe24 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -121,6 +121,35 @@ inline ReturnType ReturnModeToReturnType(const ReturnMode & mode) } } +inline uint8_t ReturnModeToNReturns(const ReturnMode & mode) +{ + switch (mode) { + case ReturnMode::SINGLE_STRONGEST: + case ReturnMode::SINGLE_FIRST: + case ReturnMode::SINGLE_LAST: + case ReturnMode::STRONGEST: + case ReturnMode::FIRST: + case ReturnMode::LAST: + return 1; + case ReturnMode::DUAL_STRONGEST_FIRST: + case ReturnMode::DUAL_FIRST_STRONGEST: + case ReturnMode::DUAL_STRONGEST_LAST: + case ReturnMode::DUAL_LAST_STRONGEST: + case ReturnMode::DUAL_LAST_FIRST: + case ReturnMode::DUAL_WEAK_FIRST: + case ReturnMode::DUAL_WEAK_LAST: + case ReturnMode::DUAL_FIRST: + case ReturnMode::DUAL_LAST: + case ReturnMode::DUAL_ONLY: + case ReturnMode::DUAL: + return 2; + case ReturnMode::TRIPLE: + return 3; + case ReturnMode::UNKNOWN: + throw std::runtime_error("Got unknown return mode"); + } +} + /// @brief Convert ReturnMode enum to integer /// @param mode /// @return Corresponding number diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp index cfca5fe7e..bbce1779c 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -71,7 +71,6 @@ struct ChannelCorrection { float azimuth{NAN}; float elevation{NAN}; - uint16_t channel{}; [[nodiscard]] bool has_value() const { return !std::isnan(azimuth) && !std::isnan(elevation); } }; @@ -187,17 +186,6 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase { return calibration[channel_id]; } - - void CreateCorrectedChannels() - { - for(auto& correction : calibration) { - uint16_t channel = 0; - for(const auto& compare:calibration) { - if(compare.elevation < correction.elevation) ++channel; - } - correction.channel = channel; - } - } }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp deleted file mode 100644 index 60b345861..000000000 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp +++ /dev/null @@ -1,57 +0,0 @@ -#pragma once - -#include "nebula_common/robosense/robosense_common.hpp" - -#include - -#include -#include - -namespace nebula -{ -namespace drivers -{ - -struct CorrectedAngleData -{ - float azimuth_rad; - float elevation_rad; - float sin_azimuth; - float cos_azimuth; - float sin_elevation; - float cos_elevation; - uint16_t corrected_channel_id; -}; - -/// @brief Handles angle correction for given azimuth/channel combinations, as well as trigonometry -/// lookup tables -class AngleCorrector -{ -protected: - const std::shared_ptr sensor_calibration_; - -public: - explicit AngleCorrector( - const std::shared_ptr & sensor_calibration) - : sensor_calibration_(sensor_calibration) - { - } - - /// @brief Get the corrected azimuth and elevation for a given block and channel, along with their - /// sin/cos values. - /// @param block_azimuth The block's azimuth (including optional fine azimuth), in the sensor's - /// angle unit - /// @param channel_id The laser channel's id - /// @return The corrected angles (azimuth, elevation) in radians and their sin/cos values - virtual CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) = 0; - - /// @brief Returns true if the current azimuth lies in a different (new) scan compared to the last - /// azimuth - /// @param current_azimuth The current azimuth value in the sensor's angle resolution - /// @param last_azimuth The last azimuth in the sensor's angle resolution - /// @return true if the current azimuth is in a different scan than the last one, false otherwise - virtual bool hasScanned(int current_azimuth, int last_azimuth) = 0; -}; - -} // namespace drivers -} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp index 99bd0c02f..64f90cac9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp @@ -1,7 +1,7 @@ #pragma once #include "nebula_common/robosense/robosense_common.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp" #include @@ -10,32 +10,38 @@ namespace nebula namespace drivers { -template -class AngleCorrectorCalibrationBased : public AngleCorrector +template +class AngleCorrectorCalibrationBased : public sensor_mixins::AngleCorrectorMixin { private: - static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; + static constexpr size_t N_CHANNELS = PacketT::N_CHANNELS; + static constexpr size_t ANGLE_UNIT = PacketT::DEGREE_SUBDIVISIONS; + static constexpr size_t MAX_AZIMUTH_LEN = 360 * ANGLE_UNIT; - std::array elevation_angle_rad_{}; - std::array azimuth_offset_rad_{}; + std::array elevation_angle_rad_{}; + std::array azimuth_offset_rad_{}; std::array block_azimuth_rad_{}; - std::array elevation_cos_{}; - std::array elevation_sin_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_cos_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_sin_{}; + std::array elevation_cos_{}; + std::array elevation_sin_{}; + std::array, MAX_AZIMUTH_LEN> azimuth_cos_{}; + std::array, MAX_AZIMUTH_LEN> azimuth_sin_{}; public: explicit AngleCorrectorCalibrationBased( - const std::shared_ptr & sensor_calibration) - : AngleCorrector(sensor_calibration) + const std::shared_ptr & sensor_calibration) { if (sensor_calibration == nullptr) { throw std::runtime_error( "Cannot instantiate AngleCorrectorCalibrationBased without calibration data"); } - for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { + if (sensor_calibration->calibration.size() == 0) { + throw std::runtime_error( + "Cannot instantiate AngleCorrectorCalibrationBased with empty calibration data"); + } + + for (size_t channel_id = 0; channel_id < N_CHANNELS; ++channel_id) { const auto correction = sensor_calibration->GetCorrection(channel_id); float elevation_angle_deg = correction.elevation; float azimuth_offset_deg = correction.azimuth; @@ -48,36 +54,34 @@ class AngleCorrectorCalibrationBased : public AngleCorrector } for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH_LEN; block_azimuth++) { - block_azimuth_rad_[block_azimuth] = deg2rad(block_azimuth / static_cast(AngleUnit)); + block_azimuth_rad_[block_azimuth] = deg2rad(block_azimuth / static_cast(ANGLE_UNIT)); - for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { + for (size_t channel_id = 0; channel_id < N_CHANNELS; ++channel_id) { float precision_azimuth = block_azimuth_rad_[block_azimuth] + azimuth_offset_rad_[channel_id]; + // Robosense azimuths are clockwise, math is counter-clockwise + precision_azimuth = -precision_azimuth; + azimuth_cos_[block_azimuth][channel_id] = cosf(precision_azimuth); azimuth_sin_[block_azimuth][channel_id] = sinf(precision_azimuth); } } } - CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override + sensor_mixins::CorrectedAngleData getCorrectedAngleData( + int32_t raw_azimuth, int32_t raw_elevation) const override { - float azimuth_rad = block_azimuth_rad_[block_azimuth] + azimuth_offset_rad_[channel_id]; - float elevation_rad = elevation_angle_rad_[channel_id]; + float azimuth_rad = block_azimuth_rad_[raw_azimuth] + azimuth_offset_rad_[raw_elevation]; + float elevation_rad = elevation_angle_rad_[raw_elevation]; return { azimuth_rad, elevation_rad, - azimuth_sin_[block_azimuth][channel_id], - azimuth_cos_[block_azimuth][channel_id], - elevation_sin_[channel_id], - elevation_cos_[channel_id], - sensor_calibration_->calibration[channel_id].channel}; - } - - bool hasScanned(int current_azimuth, int last_azimuth) override - { - return current_azimuth < last_azimuth; + azimuth_sin_[raw_azimuth][raw_elevation], + azimuth_cos_[raw_azimuth][raw_elevation], + elevation_sin_[raw_elevation], + elevation_cos_[raw_elevation]}; } }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_common.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_common.hpp new file mode 100644 index 000000000..857dc6319 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_common.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp" + +#include +#include +#include + +using namespace nebula::drivers::sensor_mixins; + +namespace nebula +{ +namespace drivers +{ + +template +struct BpearlChannelMixin : public ChannelMixin +{ + BpearlChannelMixin(const std::shared_ptr & calibration) + { + if (calibration == nullptr) { + throw std::runtime_error("Cannot instantiate BpearlChannelMixin without calibration data"); + } + + if (calibration->calibration.size() != 32) { + std::stringstream ss; + ss << "Calibration has wrong number of channels: expected 32, got " + << calibration->calibration.size(); + throw std::runtime_error(ss.str()); + } + + for (uint8_t channel = 0; channel < 32; ++channel) { + const auto & correction = calibration->GetCorrection(channel); + uint8_t remap_to_channel = 0; + for (const auto & other_correction : calibration->calibration) { + if (other_correction.elevation < correction.elevation) ++remap_to_channel; + } + channel_map_[channel] = remap_to_channel; + } + } + + uint8_t getChannel( + const PacketT & /* packet */, const size_t /* block_id */, const size_t unit_id) const override + { + return channel_map_[unit_id]; + } + +private: + std::array channel_map_; +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file 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 a54c52bd6..38f430a13 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 @@ -1,7 +1,18 @@ #pragma once +#include "nebula_decoders/nebula_decoders_common/sensor.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/intensity.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/return_mode.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/scan_completion.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp" +#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/robosense_packet.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.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" @@ -10,6 +21,7 @@ #include using namespace boost::endian; +using namespace nebula::drivers::sensor_mixins; namespace nebula { @@ -131,19 +143,23 @@ struct InfoPacket #pragma pack(pop) } // namespace bpearl_v3 - -/// @brief Get the distance unit of the given @ref BpearlV3 packet in meters. -/// @return 0.005m (0.5cm) -template <> -inline double get_dis_unit(const bpearl_v3::Packet & /* packet */) -{ - return 0.005; -} - } // namespace robosense_packet -class BpearlV3 : public RobosenseSensor< - robosense_packet::bpearl_v3::Packet, robosense_packet::bpearl_v3::InfoPacket> +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 { private: static constexpr int firing_time_offset_ns_single_[12][32]{ @@ -219,6 +235,50 @@ class BpearlV3 : public RobosenseSensor< 31096, 31352, 31608, 31864, 32120, 27888, 28144, 28400, 28656, 28912, 29168, 29424, 29680, 30456, 30712, 30968, 31224, 31480, 31736, 31992, 32248}}; +public: + static constexpr float MIN_RANGE = 0.1f; + 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) + { + } + + int32_t getPacketRelativeTimestamp( + const packet_t & /* packet */, const size_t block_id, const size_t unit_id, + const ReturnMode return_mode) const override + { + if (return_mode == ReturnMode::DUAL) + return firing_time_offset_ns_dual_[block_id][unit_id]; + else + return firing_time_offset_ns_single_[block_id][unit_id]; + }; + + double getDistanceUnit(const packet_t & /* packet */) const override { return 0.005; } + + /// @brief Get the distance value of the given unit in meters. + double getDistance( + const packet_t & packet, const size_t block_id, const size_t unit_id) const override + { + const auto * unit = getUnit(packet, block_id, unit_id); + return getFieldValue(unit->distance) * getDistanceUnit(packet); + } + + ReturnMode getReturnMode( + const packet_t & /* packet */, const SensorConfigurationBase & config) const override + { + return config.return_mode; + } +}; + +class BpearlV3Info : public SensorInfoBase +{ +private: static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x01; static constexpr uint8_t LAST_RETURN_FLAG = 0x02; @@ -233,21 +293,8 @@ class BpearlV3 : public RobosenseSensor< static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; public: - static constexpr float MIN_RANGE = 0.1f; - static constexpr float MAX_RANGE = 30.f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; - - int getPacketRelativePointTimeOffset( - const uint32_t block_id, const uint32_t channel_id, - const std::shared_ptr & sensor_configuration) override - { - if (sensor_configuration->return_mode == ReturnMode::DUAL) - return firing_time_offset_ns_dual_[block_id][channel_id]; - else - return firing_time_offset_ns_single_[block_id][channel_id]; - } - - ReturnMode getReturnMode(const robosense_packet::bpearl_v3::InfoPacket & info_packet) + ReturnMode getReturnMode( + const robosense_packet::bpearl_v3::InfoPacket & info_packet) const override { switch (info_packet.return_mode.value()) { case DUAL_RETURN_FLAG: @@ -261,13 +308,13 @@ class BpearlV3 : public RobosenseSensor< } } - RobosenseCalibrationConfiguration getSensorCalibration( - const robosense_packet::bpearl_v3::InfoPacket & info_packet) + std::optional getSensorCalibration( + const robosense_packet::bpearl_v3::InfoPacket & info_packet) const override { return info_packet.sensor_calibration.getCalibration(); } - bool getSyncStatus(const robosense_packet::bpearl_v3::InfoPacket & info_packet) + bool getSyncStatus(const robosense_packet::bpearl_v3::InfoPacket & info_packet) const override { switch (info_packet.time_sync_mode.value()) { case SYNC_MODE_GPS_FLAG: @@ -284,7 +331,7 @@ class BpearlV3 : public RobosenseSensor< } std::map getSensorInfo( - const robosense_packet::bpearl_v3::InfoPacket & info_packet) + const robosense_packet::bpearl_v3::InfoPacket & info_packet) const override { std::map sensor_info; sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed.value()); @@ -310,14 +357,14 @@ class BpearlV3 : public RobosenseSensor< sensor_info["serial_number"] = info_packet.serial_number.to_string(); sensor_info["zero_angle_offset"] = std::to_string(info_packet.zero_angle_offset.value()); - switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: + switch (getReturnMode(info_packet)) { + case ReturnMode::DUAL: sensor_info["return_mode"] = "dual"; break; - case STRONGEST_RETURN_FLAG: + case ReturnMode::SINGLE_STRONGEST: sensor_info["return_mode"] = "strongest"; break; - case LAST_RETURN_FLAG: + case ReturnMode::SINGLE_LAST: sensor_info["return_mode"] = "last"; break; default: @@ -458,5 +505,6 @@ class BpearlV3 : public RobosenseSensor< return sensor_info; } }; + } // namespace drivers } // namespace nebula \ No newline at end of file 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 80b0c35ec..914b6a4e1 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 @@ -1,7 +1,19 @@ #pragma once +#include "nebula_decoders/nebula_decoders_common/sensor.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/intensity.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/return_mode.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/scan_completion.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp" +#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/robosense_packet.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.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" @@ -10,6 +22,7 @@ #include using namespace boost::endian; +using namespace nebula::drivers::sensor_mixins; namespace nebula { @@ -97,19 +110,23 @@ struct InfoPacket #pragma pack(pop) } // namespace bpearl_v4 - -/// @brief Get the distance unit of the given @ref BpearlV3 packet in meters. -/// @return 0.0025m (0.25cm) -template <> -inline double get_dis_unit(const bpearl_v4::Packet & /* packet */) -{ - return 0.0025; -} - } // namespace robosense_packet -class BpearlV4 : public RobosenseSensor< - robosense_packet::bpearl_v4::Packet, robosense_packet::bpearl_v4::InfoPacket> +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 { private: static constexpr int firing_time_offset_ns_single_[12][32] = { @@ -188,58 +205,67 @@ class BpearlV4 : public RobosenseSensor< 29610, 29777, 29943, 30110, 30277, 30444, 30611, 30777, 30944, 31111, 31278, 31445, 31611, 31778, 31945, 32112, 32279, 32445, 32612, 32779, 32946}}; - static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; - static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x04; - static constexpr uint8_t LAST_RETURN_FLAG = 0x05; - static constexpr uint8_t FIRST_RETURN_FLAG = 0x06; - - static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; - static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; - static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; - static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; - - static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; - static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; - static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; - public: static constexpr float MIN_RANGE = 0.1f; static constexpr float MAX_RANGE = 30.f; static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; - int getPacketRelativePointTimeOffset( - const uint32_t block_id, const uint32_t channel_id, - const std::shared_ptr & sensor_configuration) override + 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) { - if (sensor_configuration->return_mode == ReturnMode::DUAL) - return firing_time_offset_ns_dual_[block_id][channel_id]; + } + + int32_t getPacketRelativeTimestamp( + const packet_t & /* packet */, const size_t block_id, const size_t unit_id, + const ReturnMode return_mode) const override + { + if (return_mode == ReturnMode::DUAL) + return firing_time_offset_ns_dual_[block_id][unit_id]; else - return firing_time_offset_ns_single_[block_id][channel_id]; + return firing_time_offset_ns_single_[block_id][unit_id]; + }; + + double getDistanceUnit(const packet_t & /* packet */) const override { return 0.0025; } + + /// @brief Get the distance value of the given unit in meters. + double getDistance( + const packet_t & packet, const size_t block_id, const size_t unit_id) const override + { + const auto * unit = getUnit(packet, block_id, unit_id); + return getFieldValue(unit->distance) * getDistanceUnit(packet); } - ReturnMode getReturnMode(const robosense_packet::bpearl_v4::InfoPacket & info_packet) + ReturnMode getReturnMode( + const packet_t & /* packet */, const SensorConfigurationBase & config) const override { - switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: - return ReturnMode::DUAL; - case STRONGEST_RETURN_FLAG: - return ReturnMode::SINGLE_STRONGEST; - case LAST_RETURN_FLAG: - return ReturnMode::SINGLE_LAST; - case FIRST_RETURN_FLAG: - return ReturnMode::SINGLE_FIRST; - default: - return ReturnMode::UNKNOWN; - } + return config.return_mode; // TODO(mojomex): add DIFOP packet handling back in } +}; + +class BpearlV4Info : public SensorInfoBase +{ +private: + static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; + static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; + static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; + static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; - RobosenseCalibrationConfiguration getSensorCalibration( - const robosense_packet::bpearl_v4::InfoPacket & info_packet) + static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; + static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; + static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; + +public: + std::optional getSensorCalibration( + const robosense_packet::bpearl_v4::InfoPacket & info_packet) const override { return info_packet.sensor_calibration.getCalibration(); } - bool getSyncStatus(const robosense_packet::bpearl_v4::InfoPacket & info_packet) + bool getSyncStatus(const robosense_packet::bpearl_v4::InfoPacket & info_packet) const override { switch (info_packet.time_sync_mode.value()) { case SYNC_MODE_GPS_FLAG: @@ -256,7 +282,7 @@ class BpearlV4 : public RobosenseSensor< } std::map getSensorInfo( - const robosense_packet::bpearl_v4::InfoPacket & info_packet) + const robosense_packet::bpearl_v4::InfoPacket & info_packet) const override { std::map sensor_info; sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed_setting.value()); @@ -280,17 +306,17 @@ class BpearlV4 : public RobosenseSensor< sensor_info["baud_rate"] = std::to_string(info_packet.baud_rate.value()); sensor_info["serial_number"] = info_packet.serial_number.to_string(); - switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: + switch (getReturnMode(info_packet)) { + case ReturnMode::DUAL: sensor_info["return_mode"] = "dual"; break; - case STRONGEST_RETURN_FLAG: + case ReturnMode::SINGLE_STRONGEST: sensor_info["return_mode"] = "strongest"; break; - case LAST_RETURN_FLAG: + case ReturnMode::SINGLE_LAST: sensor_info["return_mode"] = "last"; break; - case FIRST_RETURN_FLAG: + case ReturnMode::SINGLE_FIRST: sensor_info["return_mode"] = "first"; break; default: @@ -377,5 +403,6 @@ class BpearlV4 : public RobosenseSensor< return sensor_info; } }; + } // namespace drivers } // namespace nebula \ No newline at end of file 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 8ade0f893..b0af548ed 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 @@ -1,7 +1,17 @@ #pragma once +#include "nebula_decoders/nebula_decoders_common/sensor.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/intensity.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/return_mode.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp" +#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/robosense_packet.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/sensor_info.hpp" #include "boost/endian/buffers.hpp" @@ -10,6 +20,7 @@ #include using namespace boost::endian; +using namespace nebula::drivers::sensor_mixins; namespace nebula { @@ -147,8 +158,21 @@ struct InfoPacket } // namespace helios } // namespace robosense_packet +using namespace sensor_mixins; + class Helios -: public RobosenseSensor +: 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 { private: static constexpr int firing_time_offset_ns_single_[12][32] = { @@ -227,58 +251,79 @@ class Helios 296770, 298340, 299920, 301490, 303060, 304310, 305550, 306790, 308030, 309270, 310510, 311750, 313000, 314240, 315480, 316720, 317960, 319200, 320440, 321680, 322930}}; - static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; - static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x04; - static constexpr uint8_t LAST_RETURN_FLAG = 0x05; - static constexpr uint8_t FIRST_RETURN_FLAG = 0x06; - - static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; - static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; - static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; - static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; - - static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; - static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; - static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; + const std::shared_ptr calibration_config_; public: static constexpr float MIN_RANGE = 0.2f; static constexpr float MAX_RANGE = 150.f; static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; - int getPacketRelativePointTimeOffset( - const uint32_t block_id, const uint32_t channel_id, - const std::shared_ptr & sensor_configuration) override + static constexpr std::array RETURN_GROUP_STRIDE = {0, 1, 0}; + + Helios( + const std::shared_ptr sensor_config, + const std::shared_ptr & calibration_config) + : AngleBasedScanCompletionMixin(sensor_config), AngleCorrectorCalibrationBased(calibration_config) { - if (sensor_configuration->return_mode == ReturnMode::DUAL) - return firing_time_offset_ns_dual_[block_id][channel_id]; - else - return firing_time_offset_ns_single_[block_id][channel_id]; } - ReturnMode getReturnMode(const robosense_packet::helios::InfoPacket & info_packet) + int32_t getPacketRelativeTimestamp( + const packet_t & /* packet */, const size_t block_id, const size_t unit_id, + const ReturnMode return_mode) const override { - switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: - return ReturnMode::DUAL; - case STRONGEST_RETURN_FLAG: - return ReturnMode::SINGLE_STRONGEST; - case LAST_RETURN_FLAG: - return ReturnMode::SINGLE_LAST; - case FIRST_RETURN_FLAG: - return ReturnMode::SINGLE_FIRST; - default: - return ReturnMode::UNKNOWN; + if (return_mode == ReturnMode::DUAL) + return firing_time_offset_ns_dual_[block_id][unit_id]; + else + return firing_time_offset_ns_single_[block_id][unit_id]; + }; + + double getDistanceUnit(const packet_t & packet) const override + { + const uint8_t range_resolution = getFieldValue(packet.header.range_resolution); + if (range_resolution == 0) { + return 0.0050; + } else if (range_resolution == 1) { + return 0.0025; } + + throw std::runtime_error("Unknown range resolution"); + } + + /// @brief Get the distance value of the given unit in meters. + double getDistance( + const packet_t & packet, const size_t block_id, const size_t unit_id) const override + { + const auto * unit = getUnit(packet, block_id, unit_id); + return getFieldValue(unit->distance) * getDistanceUnit(packet); + } + + ReturnMode getReturnMode( + const packet_t & /* packet */, const SensorConfigurationBase & config) const override + { + return config.return_mode; // TODO(mojomex): add DIFOP packet handling back in } +}; + +class HeliosInfo : public SensorInfoBase +{ +private: + static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; + static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; + static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; + static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; - RobosenseCalibrationConfiguration getSensorCalibration( - const robosense_packet::helios::InfoPacket & info_packet) + static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; + static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; + static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; + +public: + std::optional getSensorCalibration( + const robosense_packet::helios::InfoPacket & info_packet) const override { return info_packet.sensor_calibration.getCalibration(); } - bool getSyncStatus(const robosense_packet::helios::InfoPacket & info_packet) + bool getSyncStatus(const robosense_packet::helios::InfoPacket & info_packet) const override { switch (info_packet.time_sync_mode.value()) { case SYNC_MODE_GPS_FLAG: @@ -295,7 +340,7 @@ class Helios } std::map getSensorInfo( - const robosense_packet::helios::InfoPacket & info_packet) + const robosense_packet::helios::InfoPacket & info_packet) const override { std::map sensor_info; sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed.value()); @@ -330,17 +375,17 @@ class Helios sensor_info["serial_number"] = info_packet.serial_number.to_string(); sensor_info["zero_angle_offset"] = std::to_string(info_packet.zero_angle_offset.value()); - switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: + switch (getReturnMode(info_packet)) { + case ReturnMode::DUAL: sensor_info["return_mode"] = "dual"; break; - case STRONGEST_RETURN_FLAG: + case ReturnMode::SINGLE_STRONGEST: sensor_info["return_mode"] = "strongest"; break; - case LAST_RETURN_FLAG: + case ReturnMode::SINGLE_LAST: sensor_info["return_mode"] = "last"; break; - case FIRST_RETURN_FLAG: + case ReturnMode::SINGLE_FIRST: sensor_info["return_mode"] = "first"; break; default: 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 79630bc23..4e4f47829 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 @@ -1,6 +1,15 @@ #pragma once #include "nebula_common/robosense/robosense_common.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/intensity.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/return_mode.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/scan_completion.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/validity.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp" @@ -9,32 +18,52 @@ #include "robosense_msgs/msg/robosense_packet.hpp" #include "robosense_msgs/msg/robosense_scan.hpp" +#include +#include + namespace nebula { namespace drivers { + +using namespace nebula::drivers::sensor_mixins; + template class RobosenseDecoder : public RobosenseScanDecoder { + // 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>); + protected: /// @brief Configuration for this decoder - const std::shared_ptr sensor_configuration_; + std::shared_ptr sensor_configuration_; + std::mutex sensor_configuration_mutex_; /// @brief The sensor definition, used for return mode and time offset handling - SensorT sensor_{}; - - /// @brief Decodes azimuth/elevation angles given calibration/correction data - typename SensorT::angle_corrector_t angle_corrector_; + SensorT sensor_; /// @brief The point cloud new points get added to NebulaPointCloudPtr decode_pc_; /// @brief The point cloud that is returned when a scan is complete NebulaPointCloudPtr output_pc_; - /// @brief The last decoded packet - typename SensorT::packet_t packet_; - /// @brief The last azimuth processed - int last_phase_; + /// @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; + /// @brief The current group of packets being decoded. + std::vector decode_group_; + /// @brief The timestamp of the last completed scan in nanoseconds uint64_t output_scan_timestamp_ns_; /// @brief The timestamp of the scan currently in progress @@ -43,19 +72,43 @@ class RobosenseDecoder : public RobosenseScanDecoder bool has_scanned_; rclcpp::Logger logger_; + rclcpp::Clock clock_; /// @brief Validates and parses MsopPacket. Currently only checks size, not checksums etc. /// @param msop_packet The incoming MsopPacket /// @return Whether the packet was parsed successfully bool parsePacket(const robosense_msgs::msg::RobosensePacket & msop_packet) { - if (msop_packet.data.size() < sizeof(typename SensorT::packet_t)) { + if (msop_packet.size < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << msop_packet.data.size() << " | Expected at least:" + logger_, "Packet size mismatch:" << msop_packet.size << " | Expected at least:" << sizeof(typename SensorT::packet_t)); return false; } - if (std::memcpy(&packet_, msop_packet.data.data(), sizeof(typename SensorT::packet_t))) { + + const size_t packet_idx = sensor_.getDecodeGroupIndex(msop_packet.data.data()); + if (packet_idx >= decode_group_size_) { + RCLCPP_ERROR( + logger_, "Packet index out of bounds: %lu (expected at most: %lu)", packet_idx, + decode_group_size_); + decode_group_.clear(); + return false; + } + + if (packet_idx > decode_group_.size()) { + RCLCPP_WARN_THROTTLE(logger_, clock_, 1000, "Dropped packets detected"); + decode_group_.clear(); + return false; + } + + if (packet_idx == 0) { + decode_group_.clear(); + } + + 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))) { return true; } @@ -65,124 +118,154 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @brief Converts a group of returns (i.e. 1 for single return, 2 for dual return, etc.) to /// points and appends them to the point cloud + /// @param start_packet_id The index of the first packet in the current decode group /// @param start_block_id The first block in the group of returns - /// @param n_blocks The number of returns in the group - void convertReturns(size_t start_block_id, size_t n_blocks) + /// @param start_unit_id The first channel in the return group + /// @param n_returns The number of returns in the group + /// @param return_mode The currently active return mode + void convertReturnGroup( + size_t start_packet_id, size_t start_block_id, size_t start_unit_id, size_t n_returns, + ReturnMode return_mode) { - uint64_t packet_timestamp_ns = robosense_packet::get_timestamp_ns(packet_); - uint32_t raw_azimuth = packet_.body.blocks[start_block_id].get_azimuth(); - - std::vector return_units; - - for (size_t channel_id = 0; channel_id < SensorT::packet_t::N_CHANNELS; ++channel_id) { - // Find the units corresponding to the same return group as the current one. - // These are used to find duplicates in multi-return mode. - return_units.clear(); - for (size_t block_offset = 0; block_offset < n_blocks; ++block_offset) { - return_units.push_back( - &packet_.body.blocks[block_offset + start_block_id].units[channel_id]); - } + std::vector return_units( + n_returns); + std::vector packet_idxs(n_returns); + std::vector block_idxs(n_returns); + std::vector unit_idxs(n_returns); + + // 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]; + + packet_idxs[return_idx] = packet_idx; + block_idxs[return_idx] = block_idx; + unit_idxs[return_idx] = unit_idx; + + return_units[return_idx] = &decode_group_[packet_idx].body.blocks[block_idx].units[unit_idx]; + } - for (size_t block_offset = 0; block_offset < n_blocks; ++block_offset) { - auto & unit = *return_units[block_offset]; + // For each return unit, validate it and check if it is a duplicate of any other + // unit in the return group. If not, convert it to a point and add it to the point cloud. + for (size_t return_idx = 0; return_idx < n_returns; ++return_idx) { + const auto & packet = decode_group_[packet_idxs[return_idx]]; + const auto block_idx = block_idxs[return_idx]; + const auto unit_idx = unit_idxs[return_idx]; - if (unit.distance.value() == 0) { - continue; - } + // 1. Validate point + if (!sensor_.isValid(packet, block_idx, unit_idx)) { + continue; + } - auto distance = getDistance(unit); + // 2. Range checks + auto distance = sensor_.getDistance(packet, block_idx, unit_idx); - if (distance < SensorT::MIN_RANGE || distance > SensorT::MAX_RANGE) { - continue; - } + if (distance < SensorT::MIN_RANGE || distance > SensorT::MAX_RANGE) { + continue; + } - auto return_type = - sensor_.getReturnType(sensor_configuration_->return_mode, block_offset, return_units); + // 3. Return group duplicate checks + auto return_type = sensor_.getReturnType(return_units, return_idx, return_mode); - // Keep only last of multiple identical points - if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) { - continue; - } + // Keep only last of multiple identical points + if (return_type == ReturnType::IDENTICAL && return_idx != n_returns - 1) { + continue; + } - // Keep only last (if any) of multiple points that are too close - if (block_offset != n_blocks - 1) { - bool is_below_multi_return_threshold = false; - - for (size_t return_idx = 0; return_idx < n_blocks; ++return_idx) { - if (return_idx == block_offset) { - continue; - } - - if ( - fabsf(getDistance(*return_units[return_idx]) - distance) < - sensor_configuration_->dual_return_distance_threshold) { - is_below_multi_return_threshold = true; - break; - } - } + // Keep only last (if any) of multiple points that are too close + if (return_idx != n_returns - 1) { + bool is_below_multi_return_threshold = false; - if (is_below_multi_return_threshold) { + for (size_t other_return_idx = 0; return_idx < n_returns; ++return_idx) { + if (other_return_idx == return_idx) { continue; } + + const auto & other_packet = decode_group_[packet_idxs[other_return_idx]]; + const auto other_block_idx = block_idxs[other_return_idx]; + const auto other_unit_idx = unit_idxs[other_return_idx]; + + if ( + fabsf(sensor_.getDistance(other_packet, other_block_idx, other_unit_idx) - distance) < + sensor_configuration_->dual_return_distance_threshold) { + is_below_multi_return_threshold = true; + break; + } } - NebulaPoint point; - point.distance = distance; - point.intensity = unit.reflectivity.value(); - point.time_stamp = - getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id); + if (is_below_multi_return_threshold) { + continue; + } + } - point.return_type = static_cast(return_type); + // 4. Get fields intrinsic to the point + NebulaPoint point; + point.distance = distance; + point.intensity = sensor_.getIntensity(packet, block_idx, unit_idx); - auto corrected_angle_data = angle_corrector_.getCorrectedAngleData(raw_azimuth, channel_id); - point.channel = corrected_angle_data.corrected_channel_id; + // 5. Do timing correction + point.time_stamp = getPointTimeRelative(packet, block_idx, unit_idx, return_mode); - // The raw_azimuth and channel are only used as indices, sin/cos functions use the precise - // corrected angles - float xyDistance = distance * corrected_angle_data.cos_elevation; - point.x = xyDistance * corrected_angle_data.cos_azimuth; - point.y = -xyDistance * corrected_angle_data.sin_azimuth; - point.z = distance * corrected_angle_data.sin_elevation; + // 6. Add point metadata + point.return_type = static_cast(return_type); + point.channel = sensor_.getChannel(packet, block_idx, unit_idx); - // The driver wrapper converts to degrees, expects radians - point.azimuth = corrected_angle_data.azimuth_rad; - point.elevation = corrected_angle_data.elevation_rad; + // 7. Do angle correction + const auto raw_azimuth = sensor_.getRawAzimuth(packet, block_idx, unit_idx); + const auto raw_elevation = sensor_.getRawElevation(packet, block_idx, unit_idx); + auto corrected_angle_data = sensor_.getCorrectedAngleData(raw_azimuth, raw_elevation); - decode_pc_->emplace_back(point); - } - } - } + // The driver wrapper converts to degrees, expects radians + point.azimuth = corrected_angle_data.azimuth_rad; + point.elevation = corrected_angle_data.elevation_rad; - /// @brief Checks whether the last processed block was the last block of a scan - /// @param current_phase The azimuth of the last processed block - /// @return Whether the scan has completed - bool checkScanCompleted(int current_phase) - { - return angle_corrector_.hasScanned(current_phase, last_phase_); - } + // 8. Convert to cartesian coordinates + // The raw_azimuth and channel are only used as indices, sin/cos functions use the precise + // corrected angles + float xyDistance = distance * corrected_angle_data.cos_elevation; + point.x = xyDistance * corrected_angle_data.cos_azimuth; + point.y = xyDistance * corrected_angle_data.sin_azimuth; + point.z = distance * corrected_angle_data.sin_elevation; - /// @brief Get the distance of the given unit in meters - /// @param unit The unit to get the distance from - /// @return The distance in meters - float getDistance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit) - { - return unit.distance.value() * robosense_packet::get_dis_unit(packet_); + decode_pc_->emplace_back(point); + } } /// @brief Get timestamp of point in nanoseconds, relative to scan timestamp. Includes firing time /// offset correction for channel and block /// @param packet_timestamp_ns The timestamp of the current MsopPacket in nanoseconds - /// @param block_id The block index of the point - /// @param channel_id The channel index of the point - uint32_t getPointTimeRelative(uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) + /// @param block_idx The block index of the point + /// @param unit_idx The channel index of the point + /// @param return_mode The currently active return mode + uint32_t getPointTimeRelative( + const typename SensorT::packet_t & packet, size_t block_idx, size_t unit_idx, + ReturnMode return_mode) { - auto point_to_packet_offset_ns = - sensor_.getPacketRelativePointTimeOffset(block_id, channel_id, sensor_configuration_); + const auto point_to_packet_offset_ns = + sensor_.getPacketRelativeTimestamp(packet, block_idx, unit_idx, return_mode); + const auto packet_timestamp_ns = sensor_.getPacketTimestamp(packet); 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; } + void onScanCompleted(size_t packet_id, size_t block_id) + { + std::swap(decode_pc_, output_pc_); + decode_pc_->clear(); + has_scanned_ = true; + output_scan_timestamp_ns_ = decode_scan_timestamp_ns_; + + // A new scan starts within the current packet, so the new scan's timestamp must be + // calculated as the packet timestamp plus the lowest time offset of any point in the + // remainder of the packet + decode_scan_timestamp_ns_ = + sensor_.getPacketTimestamp(decode_group_[packet_id]) + + sensor_.getEarliestPointTimeOffsetForBlock(block_id, sensor_configuration_); + } + public: /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder @@ -190,10 +273,12 @@ class RobosenseDecoder : public RobosenseScanDecoder /// calibration_configuration is set) explicit RobosenseDecoder( const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor) : sensor_configuration_(sensor_configuration), - angle_corrector_(calibration_configuration), - logger_(rclcpp::get_logger("RobosenseDecoder")) + sensor_(*sensor), + decode_group_(decode_group_size_), + logger_(rclcpp::get_logger("RobosenseDecoder")), + clock_(RCL_STEADY_TIME) { logger_.set_level(rclcpp::Logger::Level::Debug); RCLCPP_INFO_STREAM(logger_, sensor_configuration_); @@ -207,12 +292,19 @@ class RobosenseDecoder : public RobosenseScanDecoder int unpack(const robosense_msgs::msg::RobosensePacket & msop_packet) override { + std::lock_guard lock(sensor_configuration_mutex_); + if (sensor_configuration_ == nullptr) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, "Sensor configuration not set, cannot decode packet."); + return false; + } + if (!parsePacket(msop_packet)) { return -1; } if (decode_scan_timestamp_ns_ == 0) { - decode_scan_timestamp_ns_ = robosense_packet::get_timestamp_ns(packet_); + decode_scan_timestamp_ns_ = sensor_.getPacketTimestamp(decode_group_[0]); } if (has_scanned_) { @@ -222,37 +314,31 @@ class RobosenseDecoder : public RobosenseScanDecoder // For the dual return mode, the packet contains two blocks with the same azimuth, one for each // return. For the single return mode, the packet contains only one block per azimuth. // So, if the return mode is dual, we process two blocks per iteration, otherwise one. - const size_t n_returns = robosense_packet::get_n_returns(sensor_configuration_->return_mode); - int current_azimuth; - - for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) { - current_azimuth = - (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS + - packet_.body.blocks[block_id].get_azimuth() - - static_cast( - sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS)) % - (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS); - - bool scan_completed = checkScanCompleted(current_azimuth); - if (scan_completed) { - std::swap(decode_pc_, output_pc_); - decode_pc_->clear(); - has_scanned_ = true; - output_scan_timestamp_ns_ = decode_scan_timestamp_ns_; - - // A new scan starts within the current packet, so the new scan's timestamp must be - // calculated as the packet timestamp plus the lowest time offset of any point in the - // remainder of the packet - decode_scan_timestamp_ns_ = - robosense_packet::get_timestamp_ns(packet_) + - sensor_.getEarliestPointTimeOffsetForBlock(block_id, sensor_configuration_); + const ReturnMode return_mode = sensor_.getReturnMode(decode_group_[0], *sensor_configuration_); + const size_t n_returns = ReturnModeToNReturns(return_mode); + + // 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::transform( + SensorT::RETURN_GROUP_STRIDE.begin(), SensorT::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]) { + 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; + channel_id += advance[2]) { + convertReturnGroup(packet_id, block_id, channel_id, n_returns, return_mode); + } } - - convertReturns(block_id, n_returns); - last_phase_ = current_azimuth; } - return last_phase_; + return 0; // TODO(mojomex): azimuth has no meaning for some sensors, what to return instead? + // #Points decoded? } bool hasScanned() override { return has_scanned_; } @@ -262,6 +348,14 @@ class RobosenseDecoder : public RobosenseScanDecoder double scan_timestamp_s = static_cast(output_scan_timestamp_ns_) * 1e-9; return std::make_pair(output_pc_, scan_timestamp_s); } + + void updateSensorConfiguration( + const std::shared_ptr & sensor_configuration) + override + { + std::lock_guard lock(sensor_configuration_mutex_); + sensor_configuration_ = sensor_configuration; + } }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp index 86b9bc4aa..8e75d8d5e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp @@ -8,6 +8,7 @@ #include #include +#include #include namespace nebula @@ -15,15 +16,16 @@ namespace nebula namespace drivers { -template +template class RobosenseInfoDecoder : public RobosenseInfoDecoderBase { + static_assert(std::is_base_of_v, SensorInfoT>); + protected: - /// @brief The sensor definition, used for return mode and time offset handling - SensorT sensor_{}; + SensorInfoT sensor_{}; /// @brief The last decoded packet - typename SensorT::info_t packet_{}; + typename SensorInfoT::packet_t packet_{}; rclcpp::Logger logger_; @@ -34,14 +36,14 @@ class RobosenseInfoDecoder : public RobosenseInfoDecoderBase bool parsePacket(const std::vector & raw_packet) override { const auto packet_size = raw_packet.size(); - if (packet_size < sizeof(typename SensorT::info_t)) { + if (packet_size < sizeof(typename SensorInfoT::packet_t)) { RCLCPP_ERROR_STREAM( logger_, "Packet size mismatch:" << packet_size << " | Expected at least:" - << sizeof(typename SensorT::info_t)); + << sizeof(typename SensorInfoT::packet_t)); return false; } try { - if (std::memcpy(&packet_, raw_packet.data(), sizeof(typename SensorT::info_t)) == &packet_) { + if (std::memcpy(&packet_, raw_packet.data(), sizeof(typename SensorInfoT::packet_t)) == &packet_) { return true; } } catch (const std::exception & e) { @@ -51,7 +53,6 @@ class RobosenseInfoDecoder : public RobosenseInfoDecoderBase return false; } - /// @brief Constructor RobosenseInfoDecoder() : logger_(rclcpp::get_logger("RobosenseInfoDecoder")) { logger_.set_level(rclcpp::Logger::Level::Debug); @@ -70,7 +71,7 @@ class RobosenseInfoDecoder : public RobosenseInfoDecoderBase /// @brief Get sensor calibration /// @return The sensor calibration - RobosenseCalibrationConfiguration getSensorCalibration() override + std::optional getSensorCalibration() override { return sensor_.getSensorCalibration(packet_); } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp index d97dc4e1c..7c6d40da2 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp @@ -1,5 +1,13 @@ #pragma once +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/robosense/robosense_common.hpp" + +#include +#include +#include +#include + namespace nebula { namespace drivers @@ -31,7 +39,7 @@ class RobosenseInfoDecoderBase /// @brief Get sensor calibration /// @return The sensor calibration - virtual RobosenseCalibrationConfiguration getSensorCalibration() = 0; + virtual std::optional getSensorCalibration() = 0; /// @brief Get the status of time synchronization /// @return True if the sensor's clock is synchronized 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 607c44aac..bd74453b4 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,10 +1,14 @@ #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 #include #include +#include +#include using namespace boost::endian; @@ -17,6 +21,8 @@ namespace robosense_packet #pragma pack(push, 1) +struct RobosensePacket {}; + struct Timestamp { big_uint48_buf_t seconds; @@ -34,6 +40,15 @@ struct Timestamp } }; +template +struct RobosensePacketTimestampMixin : public sensor_mixins::PacketTimestampMixin +{ + uint64_t getPacketTimestamp(const PacketT & packet) const override + { + return packet.header.timestamp.get_time_in_ns(); + } +}; + struct Unit { @@ -226,34 +241,6 @@ size_t get_n_returns(ReturnMode return_mode) return 1; } -/// @brief Get timestamp from packet in nanoseconds -/// @tparam PacketT The packet type -/// @param packet The packet to get the timestamp from -/// @return The timestamp in nanoseconds -template -uint64_t get_timestamp_ns(const PacketT & packet) -{ - return packet.header.timestamp.get_time_in_ns(); -} - -/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, -/// multiplied by this value, yield the distance in meters. -/// @tparam PacketT The packet type -/// @param packet The packet to get the distance unit from -/// @return The distance unit in meters -template -double get_dis_unit(const PacketT & packet) -{ - // Packets define distance unit in millimeters, convert to meters here - const uint8_t range_resolution = packet.header.range_resolution.value(); - if (range_resolution == 0) { - return 0.0050; - } else if (range_resolution == 1) { - return 0.0025; - } - throw std::runtime_error("Unknown range resolution"); -} - /// @brief Convert raw angle value from packet to std::string /// @param raw_angle The raw angle value from the packet /// @return The angle as std::string diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp index c5beaa342..8655419c9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp @@ -36,6 +36,10 @@ class RobosenseScanDecoder /// @brief Returns the point cloud and timestamp of the last scan /// @return A tuple of point cloud and timestamp in nanoseconds virtual std::tuple getPointcloud() = 0; + + /// @brief Updates the internally stored sensor configuration + virtual void updateSensorConfiguration( + const std::shared_ptr & sensor_configuration) = 0; }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp deleted file mode 100644 index 4e8b3c25a..000000000 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp +++ /dev/null @@ -1,124 +0,0 @@ -#pragma once - -#include "nebula_common/nebula_common.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" - -#include -#include - -namespace nebula -{ -namespace drivers -{ - -/// @brief Base class for all sensor definitions -/// @tparam PacketT The packet type of the sensor -template -class RobosenseSensor -{ -private: -public: - typedef PacketT packet_t; - typedef InfoPacketT info_t; - typedef class AngleCorrectorCalibrationBased - angle_corrector_t; - - RobosenseSensor() = default; - virtual ~RobosenseSensor() = default; - - /// @brief Computes the exact relative time between the timestamp of the given packet and the one - /// of the point identified by the given block and channel, in nanoseconds - /// @param block_id The point's block id - /// @param channel_id The point's channel id - /// @param sensor_configuration The sensor configuration - /// @return The relative time offset in nanoseconds - virtual int getPacketRelativePointTimeOffset( - uint32_t block_id, uint32_t channel_id, - const std::shared_ptr & sensor_configuration) = 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) - { - const auto n_returns = robosense_packet::get_n_returns(sensor_configuration->return_mode); - int min_offset_ns = std::numeric_limits::max(); - - for (uint32_t block_id = start_block_id; block_id < start_block_id + n_returns; ++block_id) { - for (uint32_t channel_id = 0; channel_id < PacketT::N_CHANNELS; ++channel_id) { - min_offset_ns = std::min( - min_offset_ns, - getPacketRelativePointTimeOffset(block_id, channel_id, sensor_configuration)); - } - } - - return min_offset_ns; - } - - /// @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. - /// length 2 for dual-return with both units having the same channel but coming from different - /// blocks) - /// @return true if the unit is identical to any other one in return_units, false otherwise - static bool is_duplicate( - uint32_t return_idx, - const std::vector & return_units) - { - for (unsigned int i = 0; i < return_units.size(); ++i) { - if (i == return_idx) { - continue; - } - - if ( - return_units[return_idx]->distance.value() == return_units[i]->distance.value() && - return_units[return_idx]->reflectivity.value() == return_units[i]->reflectivity.value()) { - return true; - } - } - - return false; - } - - /// @brief Get the return type of the point given by return_idx - /// - /// @param return_mode The sensor's currently active return mode - /// @param return_idx The block index of the point within the group of blocks that make up the - /// return group (e.g. either 0 or 1 for dual return) - /// @param return_units The units corresponding to all the returns in the group. These are usually - /// from the same column across adjascent blocks. - /// @return The return type of the point - virtual ReturnType getReturnType( - ReturnMode return_mode, unsigned int return_idx, - const std::vector & return_units) - { - if (is_duplicate(return_idx, return_units)) { - return ReturnType::IDENTICAL; - } - - switch (return_mode) { - case ReturnMode::SINGLE_FIRST: - return ReturnType::FIRST; - case ReturnMode::SINGLE_LAST: - return ReturnType::LAST; - case ReturnMode::SINGLE_STRONGEST: - return ReturnType::STRONGEST; - case ReturnMode::DUAL: - if (return_idx == 0) { - return ReturnType::STRONGEST; - } else { - return ReturnType::LAST; - } - default: - return ReturnType::UNKNOWN; - } - } -}; -} // namespace drivers -} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/sensor_info.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/sensor_info.hpp new file mode 100644 index 000000000..b736e23ab --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/sensor_info.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +template +class SensorInfoBase +{ +private: + static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; + static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x04; + static constexpr uint8_t LAST_RETURN_FLAG = 0x05; + static constexpr uint8_t FIRST_RETURN_FLAG = 0x06; + +public: + typedef InfoPacketT packet_t; + + virtual std::map getSensorInfo(const InfoPacketT & info_packet) const = 0; + + virtual ReturnMode getReturnMode(const InfoPacketT & info_packet) const + { + switch (getFieldValue(info_packet.return_mode)) { + case DUAL_RETURN_FLAG: + return ReturnMode::DUAL; + case STRONGEST_RETURN_FLAG: + return ReturnMode::SINGLE_STRONGEST; + case LAST_RETURN_FLAG: + return ReturnMode::SINGLE_LAST; + case FIRST_RETURN_FLAG: + return ReturnMode::SINGLE_FIRST; + default: + return ReturnMode::UNKNOWN; + } + } + + virtual std::optional getSensorCalibration( + const InfoPacketT & info_packet) const = 0; + + virtual bool getSyncStatus(const InfoPacketT & info_packet) const = 0; +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp index fe9968162..6d482abef 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp @@ -7,9 +7,6 @@ #include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp" -#include "robosense_msgs/msg/robosense_packet.hpp" -#include "robosense_msgs/msg/robosense_scan.hpp" - #include #include @@ -37,8 +34,8 @@ class RobosenseDriver : NebulaDriverBase /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver explicit RobosenseDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr sensor_configuration, + const std::shared_ptr calibration_configuration); /// @brief Get current status of this driver /// @return Current status @@ -55,6 +52,8 @@ class RobosenseDriver : NebulaDriverBase /// @return tuple of Point cloud and timestamp std::tuple ConvertScanToPointcloud( const std::shared_ptr & robosense_scan); + + bool HasScanned(); }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp index bf44a3ce5..55261752e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp @@ -53,7 +53,7 @@ class RobosenseInfoDriver ReturnMode GetReturnMode(); - RobosenseCalibrationConfiguration GetSensorCalibration(); + std::optional GetSensorCalibration(); /// @brief Get the status of time synchronization /// @return True if the sensor's clock is synchronized diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp index c6cb23378..1c97e20b2 100644 --- a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp @@ -10,27 +10,28 @@ namespace drivers { RobosenseDriver::RobosenseDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr sensor_configuration, + const std::shared_ptr calibration_configuration) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; switch (sensor_configuration->sensor_model) { case SensorModel::UNKNOWN: driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; + std::cerr << "Invalid sensor model: " << sensor_configuration->sensor_model << std::endl; break; - case SensorModel::ROBOSENSE_BPEARL_V3: - scan_decoder_.reset( - new RobosenseDecoder(sensor_configuration, calibration_configuration)); - break; - case SensorModel::ROBOSENSE_BPEARL_V4: - scan_decoder_.reset( - new RobosenseDecoder(sensor_configuration, calibration_configuration)); - break; - case SensorModel::ROBOSENSE_HELIOS: - scan_decoder_.reset( - new RobosenseDecoder(sensor_configuration, calibration_configuration)); - break; + case SensorModel::ROBOSENSE_BPEARL_V3: { + std::shared_ptr sensor = std::make_shared(sensor_configuration, calibration_configuration); + scan_decoder_.reset(new RobosenseDecoder(sensor_configuration, sensor)); + } break; + case SensorModel::ROBOSENSE_BPEARL_V4: { + std::shared_ptr sensor = std::make_shared(sensor_configuration, calibration_configuration); + scan_decoder_.reset(new RobosenseDecoder(sensor_configuration, sensor)); + } break; + case SensorModel::ROBOSENSE_HELIOS: { + std::shared_ptr sensor = std::make_shared(sensor_configuration, calibration_configuration); + scan_decoder_.reset(new RobosenseDecoder(sensor_configuration, sensor)); + } break; default: driver_status_ = nebula::Status::NOT_INITIALIZED; throw std::runtime_error("Driver not Implemented for selected sensor."); @@ -42,6 +43,11 @@ Status RobosenseDriver::GetStatus() return driver_status_; } +bool RobosenseDriver::HasScanned() +{ + return scan_decoder_->hasScanned(); +} + Status RobosenseDriver::SetCalibrationConfiguration( const CalibrationConfigurationBase & calibration_configuration) { @@ -70,11 +76,11 @@ std::tuple RobosenseDriver::ConvertScanToP } } - if (cnt == 0) { - RCLCPP_ERROR_STREAM( - logger, "Scanned " << robosense_scan->packets.size() << " packets, but no " - << "pointclouds were generated. Last azimuth: " << last_azimuth); - } + // if (cnt == 0) { + // RCLCPP_ERROR_STREAM( + // logger, "Scanned " << robosense_scan->packets.size() << " packets, but no " + // << "pointclouds were generated. Last azimuth: " << last_azimuth); + // } return pointcloud; } diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp index 4d415afd2..161a791e0 100644 --- a/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp @@ -15,13 +15,13 @@ RobosenseInfoDriver::RobosenseInfoDriver( driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; break; case SensorModel::ROBOSENSE_BPEARL_V3: - info_decoder_.reset(new RobosenseInfoDecoder()); + info_decoder_.reset(new RobosenseInfoDecoder()); break; case SensorModel::ROBOSENSE_BPEARL_V4: - info_decoder_.reset(new RobosenseInfoDecoder()); + info_decoder_.reset(new RobosenseInfoDecoder()); break; case SensorModel::ROBOSENSE_HELIOS: - info_decoder_.reset(new RobosenseInfoDecoder()); + info_decoder_.reset(new RobosenseInfoDecoder()); break; default: @@ -53,7 +53,7 @@ ReturnMode RobosenseInfoDriver::GetReturnMode() return info_decoder_->getReturnMode(); } -RobosenseCalibrationConfiguration RobosenseInfoDriver::GetSensorCalibration() +std::optional RobosenseInfoDriver::GetSensorCalibration() { return info_decoder_->getSensorCalibration(); } diff --git a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp index 8332933e0..b917cbe2a 100644 --- a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp @@ -21,10 +21,11 @@ void RobosenseHwInterface::ReceiveCloudPacketCallback(const std::vector } // Copy data uint32_t buffer_size = buffer.size(); - std::array packet_data{}; + std::array packet_data{0}; std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, packet_data.begin()); robosense_msgs::msg::RobosensePacket msop_packet; msop_packet.data = packet_data; + msop_packet.size = buffer_size; // Add timestamp (Sensor timestamp will be handled by decoder) const auto now = std::chrono::system_clock::now(); @@ -80,8 +81,8 @@ void RobosenseHwInterface::ReceiveInfoPacketCallback(const std::vector return; } - info_buffer_.emplace(buffer); ////// - is_info_received = true; //////// + info_buffer_.emplace(buffer); + is_info_received = true; if (info_reception_callback_) { std::unique_ptr difop_packet = @@ -159,24 +160,28 @@ Status RobosenseHwInterface::SetSensorConfiguration( sensor_configuration_ = std::static_pointer_cast(sensor_configuration); - if ( - sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL || - sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL_V3 || - sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL_V4) { + switch (sensor_configuration->sensor_model) { + case SensorModel::ROBOSENSE_BPEARL: + case SensorModel::ROBOSENSE_BPEARL_V3: + case SensorModel::ROBOSENSE_BPEARL_V4: azimuth_index_ = 44; is_valid_packet_ = [](size_t packet_size) { return (packet_size == BPEARL_PACKET_SIZE); }; is_valid_info_packet_ = [](size_t packet_size) { return (packet_size == BPEARL_INFO_PACKET_SIZE); }; - } else if (sensor_configuration->sensor_model == SensorModel::ROBOSENSE_HELIOS) { + break; + case SensorModel::ROBOSENSE_HELIOS: azimuth_index_ = 44; is_valid_packet_ = [](size_t packet_size) { return (packet_size == HELIOS_PACKET_SIZE); }; is_valid_info_packet_ = [](size_t packet_size) { return (packet_size == HELIOS_INFO_PACKET_SIZE); }; - } else { + break; + default: status = Status::INVALID_SENSOR_MODEL; + break; } + } catch (const std::exception & ex) { status = Status::SENSOR_CONFIG_ERROR; std::cerr << status << std::endl; diff --git a/nebula_messages/robosense_msgs/msg/RobosensePacket.msg b/nebula_messages/robosense_msgs/msg/RobosensePacket.msg index 713682931..fb1efc357 100644 --- a/nebula_messages/robosense_msgs/msg/RobosensePacket.msg +++ b/nebula_messages/robosense_msgs/msg/RobosensePacket.msg @@ -1,2 +1,3 @@ builtin_interfaces/Time stamp -uint8[1248] data \ No newline at end of file +uint8[1248] data +uint32 size \ No newline at end of file diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp index 8f6f713a0..1cab8f2c5 100644 --- a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp @@ -150,8 +150,12 @@ void RobosenseDriverRosWrapper::ReceiveInfoMsgCallback( sensor_cfg_ptr_->return_mode = info_driver_ptr_->GetReturnMode(); sensor_cfg_ptr_->use_sensor_time = info_driver_ptr_->GetSyncStatus(); - *calibration_cfg_ptr_ = info_driver_ptr_->GetSensorCalibration(); - calibration_cfg_ptr_->CreateCorrectedChannels(); + const auto & calibration = info_driver_ptr_->GetSensorCalibration(); + if (calibration) { + *calibration_cfg_ptr_ = (*calibration); + } else { + calibration_cfg_ptr_.reset(); + } RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); From 8cd4751df355584f7aac2e9a271ba05d110c229c Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 5 Dec 2023 16:32:05 +0900 Subject: [PATCH 06/17] fix(robosense): remove leaked M1 dependencies --- transport_drivers | 1 + 1 file changed, 1 insertion(+) create mode 160000 transport_drivers diff --git a/transport_drivers b/transport_drivers new file mode 160000 index 000000000..5b007e205 --- /dev/null +++ b/transport_drivers @@ -0,0 +1 @@ +Subproject commit 5b007e205b60ff3e3f340804dabaccea69f6a917 From 8888106c833a72c88779f4df4e485f0638aaf520 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 5 Dec 2023 18:35:39 +0900 Subject: [PATCH 07/17] fix(robosense_common): add missing Robosense models to GetChannelSize --- .../include/nebula_common/robosense/robosense_common.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp index bbce1779c..ab29c5b84 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -58,7 +58,9 @@ inline ReturnMode ReturnModeFromStringRobosense(const std::string & return_mode) size_t GetChannelSize(const SensorModel & model) { switch (model) { + case SensorModel::ROBOSENSE_BPEARL: case SensorModel::ROBOSENSE_BPEARL_V3: + case SensorModel::ROBOSENSE_BPEARL_V4: return 32; case SensorModel::ROBOSENSE_HELIOS: return 32; From e35d24051eeb3a970d27e2de24a2eeb3cef20b61 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 5 Dec 2023 18:37:35 +0900 Subject: [PATCH 08/17] fix(robosense): fix pointclouds not being output for Helios/Bpearl --- .../src/nebula_decoders_robosense/robosense_driver.cpp | 8 +++----- .../src/robosense/robosense_decoder_ros_wrapper.cpp | 4 ---- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp index 1c97e20b2..90384db36 100644 --- a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp @@ -67,19 +67,17 @@ std::tuple RobosenseDriver::ConvertScanToP return pointcloud; } - int cnt = 0, last_azimuth = 0; for (auto & packet : robosense_scan->packets) { - last_azimuth = scan_decoder_->unpack(packet); + scan_decoder_->unpack(packet); if (scan_decoder_->hasScanned()) { pointcloud = scan_decoder_->getPointcloud(); - cnt++; } } - // if (cnt == 0) { + // if (/* there is no pointcloud produced for a long time */) { // RCLCPP_ERROR_STREAM( // logger, "Scanned " << robosense_scan->packets.size() << " packets, but no " - // << "pointclouds were generated. Last azimuth: " << last_azimuth); + // << "pointclouds were generated."); // } return pointcloud; diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp index 1cab8f2c5..e2e997f08 100644 --- a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp @@ -77,10 +77,6 @@ void RobosenseDriverRosWrapper::ReceiveScanMsgCallback( driver_ptr_->ConvertScanToPointcloud(scan_msg); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - if (!driver_ptr_->HasScanned()) { - return; - }; - if (pointcloud == nullptr) { RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); return; From 4f849d607c91e403bd76c6bb51fb788e8bd4867d Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Wed, 6 Dec 2023 16:04:34 +0900 Subject: [PATCH 09/17] refactor(robosense): use getFieldValue in generic sensor isDuplicate --- .../include/nebula_decoders/nebula_decoders_common/sensor.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 383205317..bf01a36a2 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp @@ -58,8 +58,8 @@ class SensorBase } if ( - return_units[return_idx]->distance.value() == return_units[i]->distance.value() && - return_units[return_idx]->reflectivity.value() == return_units[i]->reflectivity.value()) { + getFieldValue(return_units[return_idx]->distance) == getFieldValue(return_units[i]->distance) && + getFieldValue(return_units[return_idx]->reflectivity) == getFieldValue(return_units[i]->reflectivity)) { return true; } } From cef0cd6aa514ed762ea15b5b6ded977e0f33d554 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Wed, 6 Dec 2023 16:06:08 +0900 Subject: [PATCH 10/17] perf(robosense): replace return group std::vector with statically allocated boost::container::static_vector --- .../decoders/robosense_decoder.hpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) 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 4e4f47829..ea7f4ef64 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 @@ -18,8 +18,10 @@ #include "robosense_msgs/msg/robosense_packet.hpp" #include "robosense_msgs/msg/robosense_scan.hpp" -#include +#include + #include +#include namespace nebula { @@ -127,11 +129,12 @@ 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) { - std::vector return_units( - n_returns); - std::vector packet_idxs(n_returns); - std::vector block_idxs(n_returns); - std::vector unit_idxs(n_returns); + boost::container::static_vector< + const typename SensorT::packet_t::body_t::block_t::unit_t *, SensorT::packet_t::MAX_RETURNS> + return_units{}; + 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. @@ -144,7 +147,7 @@ class RobosenseDecoder : public RobosenseScanDecoder block_idxs[return_idx] = block_idx; unit_idxs[return_idx] = unit_idx; - return_units[return_idx] = &decode_group_[packet_idx].body.blocks[block_idx].units[unit_idx]; + return_units[return_idx] = getUnit(decode_group_[packet_idx], block_idx, unit_idx); } // For each return unit, validate it and check if it is a duplicate of any other From 545872ab16574b687b11a53c68de9719549dc409 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Wed, 6 Dec 2023 16:06:35 +0900 Subject: [PATCH 11/17] perf(robosense): cache packet timestamps instead of re-parsing for every point --- .../decoders/robosense_decoder.hpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) 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 ea7f4ef64..6d04868ad 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 @@ -65,6 +65,7 @@ class RobosenseDecoder : public RobosenseScanDecoder SensorT::RETURN_GROUP_STRIDE[0] ? SensorT::packet_t::MAX_RETURNS : 1; /// @brief The current group of packets being decoded. std::vector decode_group_; + std::vector decode_group_timestamps_; /// @brief The timestamp of the last completed scan in nanoseconds uint64_t output_scan_timestamp_ns_; @@ -94,23 +95,28 @@ class RobosenseDecoder : public RobosenseScanDecoder logger_, "Packet index out of bounds: %lu (expected at most: %lu)", packet_idx, decode_group_size_); decode_group_.clear(); + decode_group_timestamps_.clear(); return false; } if (packet_idx > decode_group_.size()) { RCLCPP_WARN_THROTTLE(logger_, clock_, 1000, "Dropped packets detected"); decode_group_.clear(); + decode_group_timestamps_.clear(); return false; } if (packet_idx == 0) { decode_group_.clear(); + decode_group_timestamps_.clear(); } 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))) { + const auto & parsed_packet = decode_group_[packet_idx]; + decode_group_timestamps_.emplace_back(sensor_.getPacketTimestamp(parsed_packet)); return true; } @@ -154,6 +160,7 @@ class RobosenseDecoder : public RobosenseScanDecoder // unit in the return group. If not, convert it to a point and add it to the point cloud. for (size_t return_idx = 0; return_idx < n_returns; ++return_idx) { const auto & packet = decode_group_[packet_idxs[return_idx]]; + const auto packet_idx = packet_idxs[return_idx]; const auto block_idx = block_idxs[return_idx]; const auto unit_idx = unit_idxs[return_idx]; @@ -209,7 +216,7 @@ class RobosenseDecoder : public RobosenseScanDecoder point.intensity = sensor_.getIntensity(packet, block_idx, unit_idx); // 5. Do timing correction - point.time_stamp = getPointTimeRelative(packet, block_idx, unit_idx, return_mode); + point.time_stamp = getPointTimeRelative(packet_idx, block_idx, unit_idx, return_mode); // 6. Add point metadata point.return_type = static_cast(return_type); @@ -238,17 +245,17 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @brief Get timestamp of point in nanoseconds, relative to scan timestamp. Includes firing time /// offset correction for channel and block - /// @param packet_timestamp_ns The timestamp of the current MsopPacket in nanoseconds + /// @param packet_idx The index of the packet in the current decode group /// @param block_idx The block index of the point /// @param unit_idx The channel index of the point /// @param return_mode The currently active return mode uint32_t getPointTimeRelative( - const typename SensorT::packet_t & packet, size_t block_idx, size_t unit_idx, + size_t packet_idx, size_t block_idx, size_t unit_idx, ReturnMode return_mode) { const auto point_to_packet_offset_ns = - sensor_.getPacketRelativeTimestamp(packet, block_idx, unit_idx, return_mode); - const auto packet_timestamp_ns = sensor_.getPacketTimestamp(packet); + 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_); return packet_to_scan_offset_ns + point_to_packet_offset_ns; @@ -265,7 +272,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_ = - sensor_.getPacketTimestamp(decode_group_[packet_id]) + + decode_group_timestamps_[packet_id] + sensor_.getEarliestPointTimeOffsetForBlock(block_id, sensor_configuration_); } @@ -307,7 +314,7 @@ class RobosenseDecoder : public RobosenseScanDecoder } if (decode_scan_timestamp_ns_ == 0) { - decode_scan_timestamp_ns_ = sensor_.getPacketTimestamp(decode_group_[0]); + decode_scan_timestamp_ns_ = decode_group_timestamps_.back(); } if (has_scanned_) { From 3599a66f8f988b580d47e8fadaeb123468cc15e6 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Fri, 8 Dec 2023 13:21:35 +0900 Subject: [PATCH 12/17] chore: add `manc` to dictionary --- .cspell.json | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.cspell.json b/.cspell.json index e2b0768d9..3574bbfd3 100644 --- a/.cspell.json +++ b/.cspell.json @@ -38,6 +38,7 @@ "Difop", "gptp", "Idat", - "Vdat" + "Vdat", + "manc" ] } From 71c1b0bcf689acf52715d64d0cb19c38a484bc72 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 21 Dec 2023 17:43:58 +0900 Subject: [PATCH 13/17] chore(robosense_msgs): set license --- nebula_messages/robosense_msgs/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_messages/robosense_msgs/package.xml b/nebula_messages/robosense_msgs/package.xml index 29077edb2..ba374bba4 100644 --- a/nebula_messages/robosense_msgs/package.xml +++ b/nebula_messages/robosense_msgs/package.xml @@ -5,7 +5,7 @@ 0.0.0 Robosense message types for Nebula Mehmet Emin BASOGLU - TODO: License declaration + Apache 2 ament_cmake_auto rosidl_default_generators From 795a1e36a194287a7889f31f67109c3c882b34f5 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 21 Dec 2023 17:44:23 +0900 Subject: [PATCH 14/17] fix(robosense_decoder): return number of decoded points in unpack() --- .../decoders/robosense_decoder.hpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) 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 6d04868ad..c2cfc2d89 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 @@ -300,6 +300,9 @@ class RobosenseDecoder : public RobosenseScanDecoder output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); } + /// @brief Parse the raw UDP packet in `msop_packet` and append its points to the decode buffer. + /// @param msop_packet The raw packet + /// @return The number of points decoded from this packet int unpack(const robosense_msgs::msg::RobosensePacket & msop_packet) override { std::lock_guard lock(sensor_configuration_mutex_); @@ -327,6 +330,8 @@ class RobosenseDecoder : public RobosenseScanDecoder const ReturnMode return_mode = sensor_.getReturnMode(decode_group_[0], *sensor_configuration_); const size_t n_returns = ReturnModeToNReturns(return_mode); + const size_t size_before_decode = decode_pc_->size(); + // 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; @@ -347,8 +352,12 @@ class RobosenseDecoder : public RobosenseScanDecoder } } - return 0; // TODO(mojomex): azimuth has no meaning for some sensors, what to return instead? - // #Points decoded? + size_t size_after_decode = decode_pc_->size(); + if (has_scanned_) { + size_after_decode += output_pc_->size(); + } + + return static_cast(size_after_decode - size_before_decode); } bool hasScanned() override { return has_scanned_; } From 83e6cc8a545acfc5987383316a4c87884c10877f Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 26 Dec 2023 17:36:48 +0900 Subject: [PATCH 15/17] 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; } }; From a575ad32cd8ac958ff9084d848c40f3c96db0c4d Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Tue, 26 Dec 2023 17:37:25 +0900 Subject: [PATCH 16/17] fix(robosense): make scan timestamping work with sensor mixins --- .../nebula_decoders_common/sensor.hpp | 12 ------- .../sensor_mixins/timestamp.hpp | 31 ++++++++++++++----- .../decoders/bpearl_v3.hpp | 27 ++++++++-------- .../decoders/bpearl_v4.hpp | 27 ++++++++-------- .../decoders/helios.hpp | 24 +++++++------- .../decoders/robosense_decoder.hpp | 6 ++-- 6 files changed, 65 insertions(+), 62 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 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]) { From 2fcfef10ab41eb8d1e0924e50b26784e8bd956d1 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 28 Dec 2023 14:30:10 +0900 Subject: [PATCH 17/17] chore(robosense): remove todos for reading return mode from DIFOP config file can be used as well, for now. Once setting return modes becomes possible (robosense publishes communication API for sensor) this must be revised --- .../nebula_decoders_robosense/decoders/bpearl_v4.hpp | 2 +- .../nebula_decoders_robosense/decoders/helios.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 f283a7f8c..9dba88d9b 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 @@ -241,7 +241,7 @@ class BpearlV4 : public SensorBase, ReturnMode getReturnMode( const packet_t & /* packet */, const SensorConfigurationBase & config) const override { - return config.return_mode; // TODO(mojomex): add DIFOP packet handling back in + return config.return_mode; } }; 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 24c40a2d5..29bccfcff 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 @@ -298,7 +298,7 @@ class Helios ReturnMode getReturnMode( const packet_t & /* packet */, const SensorConfigurationBase & config) const override { - return config.return_mode; // TODO(mojomex): add DIFOP packet handling back in + return config.return_mode; } };