From 166e2bc2f6f51b858bd370019d72a20c8f36adaa Mon Sep 17 00:00:00 2001 From: Autum60 Date: Wed, 28 Oct 2026 21:25:39 +0900 Subject: [PATCH] fix(hesai_decoder): add handling for fault message packets and corresponding tests Signed-off-by: Autum60 --- src/nebula_hesai/nebula_hesai/CMakeLists.txt | 5 + .../tests/test_hesai_fault_message.cpp | 93 +++++++++++++++++++ .../decoders/hesai_decoder.hpp | 11 +++ .../decoders/hesai_scan_decoder.hpp | 1 + 4 files changed, 110 insertions(+) create mode 100644 src/nebula_hesai/nebula_hesai/tests/test_hesai_fault_message.cpp diff --git a/src/nebula_hesai/nebula_hesai/CMakeLists.txt b/src/nebula_hesai/nebula_hesai/CMakeLists.txt index 82ab3ea5f..31099bf8a 100644 --- a/src/nebula_hesai/nebula_hesai/CMakeLists.txt +++ b/src/nebula_hesai/nebula_hesai/CMakeLists.txt @@ -85,6 +85,11 @@ if(BUILD_TESTING) -D_SRC_CALIBRATION_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../nebula_hesai_decoders/calibration/" ) + # Fault message handling test + ament_add_gtest(test_hesai_fault_message tests/test_hesai_fault_message.cpp) + target_link_libraries(test_hesai_fault_message + nebula_hesai_decoders::nebula_hesai_decoders) + # Functional safety test ament_add_gtest(test_hesai_functional_safety tests/test_hesai_functional_safety.cpp) diff --git a/src/nebula_hesai/nebula_hesai/tests/test_hesai_fault_message.cpp b/src/nebula_hesai/nebula_hesai/tests/test_hesai_fault_message.cpp new file mode 100644 index 000000000..e6e60151a --- /dev/null +++ b/src/nebula_hesai/nebula_hesai/tests/test_hesai_fault_message.cpp @@ -0,0 +1,93 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nebula_hesai_decoders/decoders/hesai_decoder.hpp" +#include "nebula_hesai_decoders/decoders/pandar_at128.hpp" + +#include + +#include + +#include +#include +#include + +using nebula::Status; +using nebula::drivers::DecodeError; +using nebula::drivers::HesaiCorrection; +using nebula::drivers::HesaiDecoder; +using nebula::drivers::HesaiSensorConfiguration; +using nebula::drivers::PandarAT128; + +namespace +{ + +const char g_correction_path[] = _SRC_CALIBRATION_DIR_PATH "PandarAT128.dat"; + +class CountingLogger : public nebula::drivers::loggers::Logger +{ +public: + void debug(const std::string &) override {} + void info(const std::string &) override {} + void warn(const std::string &) override {} + void error(const std::string &) override { ++n_errors; } + std::shared_ptr child(const std::string &) override + { + return std::make_shared(); + } + + size_t n_errors{0}; +}; + +std::shared_ptr make_config() +{ + auto config = std::make_shared(); + config->sensor_model = nebula::drivers::SensorModel::HESAI_PANDARAT128; + config->return_mode = nebula::drivers::ReturnMode::DUAL_LAST_STRONGEST; + config->min_range = 0.3; + config->max_range = 300; + config->cloud_min_angle = 30; + config->cloud_max_angle = 150; + config->cut_angle = 150; + return config; +} + +} // namespace + +TEST(TestHesaiDecoder, IgnoresFaultMessagePackets) +{ + auto correction = std::make_shared(); + ASSERT_EQ(correction->load_from_file(g_correction_path), Status::OK); + auto logger = std::make_shared(); + // The decoder holds large angle lookup tables, so keep it off the stack + auto decoder = std::make_shared>( + make_config(), correction, logger, nullptr, nullptr, nullptr); + + std::vector fault_message(99, 0); + fault_message[0] = 0xCD; + fault_message[1] = 0xDC; + auto fault_result = decoder->unpack(fault_message); + ASSERT_FALSE(fault_result.metadata_or_error.has_value()); + EXPECT_EQ(fault_result.metadata_or_error.error(), DecodeError::PACKET_IGNORED); + EXPECT_EQ(logger->n_errors, 0U); + + // Other truncated packets are still reported + std::vector truncated_packet(99, 0); + truncated_packet[0] = 0xEE; + truncated_packet[1] = 0xFF; + auto truncated_result = decoder->unpack(truncated_packet); + ASSERT_FALSE(truncated_result.metadata_or_error.has_value()); + EXPECT_EQ(truncated_result.metadata_or_error.error(), DecodeError::PACKET_PARSE_FAILED); + EXPECT_EQ(logger->n_errors, 1U); +} diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_decoder.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_decoder.hpp index 5d442f36c..066579c6d 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_decoder.hpp +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_decoder.hpp @@ -96,6 +96,13 @@ class HesaiDecoder : public HesaiScanDecoder std::array frame_buffers_{initialize_frame(), initialize_frame()}; + /// @brief Whether the packet is a fault message (SOP 0xCDDC). Some sensors (e.g. the AT128P) + /// send these to the data port alongside point cloud packets. + static bool is_fault_message(const std::vector & packet) + { + return packet.size() >= 2 && packet[0] == 0xCD && packet[1] == 0xDC; + } + /// @brief Validates and parse PandarPacket. Checks size and, if present, CRC checksums. /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully @@ -366,6 +373,10 @@ class HesaiDecoder : public HesaiScanDecoder callback_time_ns_ = 0; did_scan_complete_ = false; + if (is_fault_message(packet)) { + return {PerformanceCounters{decode_watch.elapsed_ns()}, DecodeError::PACKET_IGNORED}; + } + if (!parse_packet(packet)) { return {PerformanceCounters{decode_watch.elapsed_ns()}, DecodeError::PACKET_PARSE_FAILED}; } diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_scan_decoder.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_scan_decoder.hpp index 655b40714..064097d4a 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_scan_decoder.hpp +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_scan_decoder.hpp @@ -29,6 +29,7 @@ enum class DecodeError : uint8_t { PACKET_PARSE_FAILED, ///< Failed to parse packet structure DRIVER_NOT_OK, ///< Driver status is not OK INVALID_PACKET_SIZE, ///< Packet size is invalid + PACKET_IGNORED, ///< Valid packet without point cloud data (e.g. IMU or fault message) }; /// @brief Metadata for a decoded packet