From 795a1e36a194287a7889f31f67109c3c882b34f5 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 21 Dec 2023 17:44:23 +0900 Subject: [PATCH] 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_; }