diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 0f4ce3f5b..42a8d64e5 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -31,6 +32,8 @@ #include #include #include +#include +#include #include #include #include @@ -43,6 +46,104 @@ namespace nebula::drivers template class HesaiDecoder : public HesaiScanDecoder { + struct PointcloudDecodeStatistics + { + explicit PointcloudDecodeStatistics(bool has_downsample_mask_filter) + : downsample_mask_filtered_count( + has_downsample_mask_filter ? std::make_optional(0) : std::nullopt) + { + } + + uint64_t distance_filtered_count = 0; + uint64_t fov_filtered_count = 0; + uint64_t invalid_point_count = 0; + uint64_t identical_filtered_count = 0; + uint64_t multiple_return_filtered_count = 0; + std::optional downsample_mask_filtered_count = 0; + + uint64_t total_kept_point_count = 0; + uint64_t invalid_packet_count = 0; + + float cloud_distance_min_m = std::numeric_limits::infinity(); + float cloud_distance_max_m = -std::numeric_limits::infinity(); + float cloud_azimuth_min_deg = std::numeric_limits::infinity(); + float cloud_azimuth_max_deg = -std::numeric_limits::infinity(); + uint32_t packet_timestamp_min_ns = std::numeric_limits::max(); + uint32_t packet_timestamp_max_ns = std::numeric_limits::min(); + + [[nodiscard]] nlohmann::ordered_json to_json() const + { + nlohmann::ordered_json distance_j; + distance_j["name"] = "distance"; + distance_j["filtered_count"] = distance_filtered_count; + + nlohmann::ordered_json fov_j; + fov_j["name"] = "fov"; + fov_j["filtered_count"] = fov_filtered_count; + + nlohmann::ordered_json identical_j; + identical_j["name"] = "identical"; + identical_j["filtered_count"] = identical_filtered_count; + + nlohmann::ordered_json multiple_j; + multiple_j["name"] = "multiple"; + multiple_j["filtered_count"] = multiple_return_filtered_count; + + nlohmann::ordered_json invalid_j; + invalid_j["name"] = "invalid"; + invalid_j["filtered_count"] = invalid_point_count; + + nlohmann::ordered_json pointcloud_bounds_azimuth_j; + pointcloud_bounds_azimuth_j["min"] = cloud_azimuth_min_deg; + pointcloud_bounds_azimuth_j["max"] = cloud_azimuth_max_deg; + + nlohmann::ordered_json pointcloud_bounds_distance_j; + pointcloud_bounds_distance_j["min"] = cloud_distance_min_m; + pointcloud_bounds_distance_j["max"] = cloud_distance_max_m; + + nlohmann::ordered_json pointcloud_bounds_timestamp_j; + pointcloud_bounds_timestamp_j["min"] = packet_timestamp_min_ns; + pointcloud_bounds_timestamp_j["max"] = packet_timestamp_max_ns; + + nlohmann::ordered_json j; + j["filter_pipeline"] = nlohmann::ordered_json::array({ + invalid_j, + distance_j, + fov_j, + identical_j, + multiple_j, + }); + + if (downsample_mask_filtered_count.has_value()) { + nlohmann::ordered_json downsample_j; + downsample_j["name"] = "downsample_mask"; + downsample_j["filtered_count"] = downsample_mask_filtered_count.value(); + j["filter_pipeline"].push_back(downsample_j); + } + + j["pointcloud_bounds"] = { + {"azimuth_deg", pointcloud_bounds_azimuth_j}, + {"distance_m", pointcloud_bounds_distance_j}, + {"timestamp_ns", pointcloud_bounds_timestamp_j}, + }; + + j["invalid_packet_count"] = invalid_packet_count; + j["total_kept_point_count"] = total_kept_point_count; + + return j; + } + + void update_pointcloud_bounds(const NebulaPoint & point) + { + cloud_azimuth_min_deg = std::min(cloud_azimuth_min_deg, rad2deg(point.azimuth)); + cloud_azimuth_max_deg = std::max(cloud_azimuth_max_deg, rad2deg(point.azimuth)); + packet_timestamp_min_ns = std::min(packet_timestamp_min_ns, point.time_stamp); + packet_timestamp_max_ns = std::max(packet_timestamp_max_ns, point.time_stamp); + cloud_distance_min_m = std::min(cloud_distance_min_m, point.distance); + cloud_distance_max_m = std::max(cloud_distance_max_m, point.distance); + } + }; + struct ScanCutAngles { float fov_min; @@ -89,6 +190,8 @@ class HesaiDecoder : public HesaiScanDecoder std::optional mask_filter_; + PointcloudDecodeStatistics pointcloud_decode_statistics_; + /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully @@ -135,6 +238,7 @@ class HesaiDecoder : public HesaiScanDecoder auto & unit = *return_units[block_offset]; if (unit.distance == 0) { + pointcloud_decode_statistics_.invalid_point_count++; continue; } @@ -144,6 +248,7 @@ class HesaiDecoder : public HesaiScanDecoder distance < SensorT::min_range || SensorT::max_range < distance || distance < sensor_configuration_->min_range || sensor_configuration_->max_range < distance) { + pointcloud_decode_statistics_.distance_filtered_count++; continue; } @@ -153,6 +258,7 @@ class HesaiDecoder : public HesaiScanDecoder // Keep only last of multiple identical points if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) { + pointcloud_decode_statistics_.identical_filtered_count++; continue; } @@ -174,6 +280,7 @@ class HesaiDecoder : public HesaiScanDecoder } if (is_below_multi_return_threshold) { + pointcloud_decode_statistics_.multiple_return_filtered_count++; continue; } } @@ -184,6 +291,7 @@ class HesaiDecoder : public HesaiScanDecoder bool in_fov = angle_is_between(scan_cut_angles_.fov_min, scan_cut_angles_.fov_max, azimuth); if (!in_fov) { + pointcloud_decode_statistics_.fov_filtered_count++; continue; } @@ -221,9 +329,14 @@ class HesaiDecoder : public HesaiScanDecoder point.azimuth = corrected_angle_data.azimuth_rad; point.elevation = corrected_angle_data.elevation_rad; - if (!mask_filter_ || !mask_filter_->excluded(point)) { - pc->emplace_back(point); + if (mask_filter_ && mask_filter_->excluded(point)) { + (*pointcloud_decode_statistics_.downsample_mask_filtered_count)++; + continue; } + + pc->emplace_back(point); + pointcloud_decode_statistics_.update_pointcloud_bounds(point); + pointcloud_decode_statistics_.total_kept_point_count++; } } } @@ -265,10 +378,13 @@ class HesaiDecoder : public HesaiScanDecoder scan_cut_angles_( {deg2rad(sensor_configuration_->cloud_min_angle), deg2rad(sensor_configuration_->cloud_max_angle), deg2rad(sensor_configuration_->cut_angle)}), - logger_(logger) + logger_(logger), + pointcloud_decode_statistics_(false) { decode_pc_ = std::make_shared(); output_pc_ = std::make_shared(); + decode_pc_->reserve(SensorT::max_scan_buffer_points); + output_pc_->reserve(SensorT::max_scan_buffer_points); if (sensor_configuration->downsample_mask_path) { mask_filter_ = point_filters::DownsampleMaskFilter( @@ -277,13 +393,13 @@ class HesaiDecoder : public HesaiScanDecoder logger_->child("Downsample Mask"), true); } - decode_pc_->reserve(SensorT::max_scan_buffer_points); - output_pc_->reserve(SensorT::max_scan_buffer_points); + pointcloud_decode_statistics_ = PointcloudDecodeStatistics(mask_filter_.has_value()); } int unpack(const std::vector & packet) override { if (!parse_packet(packet)) { + pointcloud_decode_statistics_.invalid_packet_count++; return -1; } @@ -295,6 +411,7 @@ class HesaiDecoder : public HesaiScanDecoder if (has_scanned_) { output_pc_->clear(); + pointcloud_decode_statistics_ = PointcloudDecodeStatistics(mask_filter_.has_value()); has_scanned_ = false; } @@ -345,10 +462,10 @@ class HesaiDecoder : public HesaiScanDecoder bool has_scanned() override { return has_scanned_; } - std::tuple get_pointcloud() override + std::tuple get_pointcloud() override { double scan_timestamp_s = static_cast(output_scan_timestamp_ns_) * 1e-9; - return std::make_pair(output_pc_, scan_timestamp_s); + return {output_pc_, scan_timestamp_s, pointcloud_decode_statistics_.to_json()}; } }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp index c4ac84299..9ea631117 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -46,7 +47,8 @@ class HesaiScanDecoder /// @brief Returns the point cloud and timestamp of the last scan /// @return A tuple of point cloud and timestamp in nanoseconds - virtual std::tuple get_pointcloud() = 0; + virtual std::tuple + get_pointcloud() = 0; }; } // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index b256d6f92..479925ccb 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -21,6 +21,7 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" #include +#include #include @@ -33,6 +34,10 @@ namespace nebula::drivers /// @brief Hesai driver class HesaiDriver { +public: + using parse_result_t = + std::optional>; + private: /// @brief Current driver status Status driver_status_; @@ -71,8 +76,7 @@ class HesaiDriver /// @brief Convert raw packet to pointcloud /// @param packet Packet to convert /// @return Tuple of pointcloud and timestamp - std::tuple parse_cloud_packet( - const std::vector & packet); + parse_result_t parse_cloud_packet(const std::vector & packet); }; } // namespace nebula::drivers diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 2aa2da11f..ef2d3fa24 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -82,22 +82,19 @@ std::shared_ptr HesaiDriver::initialize_decoder( logger_->child("Decoder")); } -std::tuple HesaiDriver::parse_cloud_packet( - const std::vector & packet) +HesaiDriver::parse_result_t HesaiDriver::parse_cloud_packet(const std::vector & packet) { - std::tuple pointcloud; - if (driver_status_ != nebula::Status::OK) { logger_->error("Driver not OK."); - return pointcloud; + return {}; } scan_decoder_->unpack(packet); if (scan_decoder_->has_scanned()) { - pointcloud = scan_decoder_->get_pointcloud(); + return scan_decoder_->get_pointcloud(); } - return pointcloud; + return {}; } Status HesaiDriver::set_calibration_configuration( diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 9465c5f6e..8a58397fc 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -271,8 +271,8 @@ Status HesaiRosOfflineExtractBag::read_bag() nebula_msg.header = extracted_msg.header; for (auto & pkt : extracted_msg.packets) { std::vector pkt_data(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size)); - auto pointcloud_ts = driver_ptr_->parse_cloud_packet(pkt_data); - auto pointcloud = std::get<0>(pointcloud_ts); + auto parse_result = driver_ptr_->parse_cloud_packet(pkt_data); + const auto & [pointcloud, timestamp_s, decode_stats] = parse_result.value(); nebula_msgs::msg::NebulaPacket nebula_pkt; nebula_pkt.stamp = pkt.stamp; diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index 34da8c049..b66b50266 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -208,9 +208,9 @@ Status HesaiRosOfflineExtractSample::read_bag() << bag_message->time_stamp << std::endl; for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->parse_cloud_packet( + auto parse_result = driver_ptr_->parse_cloud_packet( std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); - auto pointcloud = std::get<0>(pointcloud_ts); + const auto & [pointcloud, timestamp_s, decode_stats] = parse_result.value(); if (!pointcloud) { continue; diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 169773fc1..022c2050c 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -2,6 +2,7 @@ #include "nebula_ros/hesai/decoder_wrapper.hpp" +#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_ros/common/rclcpp_logger.hpp" #include @@ -118,23 +119,23 @@ void HesaiDecoderWrapper::process_cloud_packet( current_scan_msg_->packets.emplace_back(std::move(pandar_packet_msg)); } - std::tuple pointcloud_ts{}; - nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; + drivers::HesaiDriver::parse_result_t parse_result{}; { std::lock_guard lock(mtx_driver_ptr_); - pointcloud_ts = driver_ptr_->parse_cloud_packet(packet_msg->data); - pointcloud = std::get<0>(pointcloud_ts); + parse_result = driver_ptr_->parse_cloud_packet(packet_msg->data); } // A pointcloud is only emitted when a scan completes (e.g. 3599 packets do not emit, the 3600th // emits one) - if (pointcloud == nullptr) { + if (!parse_result) { // Since this ends the function early, the `cloud_watchdog_` will not be updated. // Thus, if pointclouds are not emitted for too long (e.g. when decoder settings are wrong or no // packets come in), the watchdog will log a warning automatically return; } + const auto & [pointcloud, timestamp_s, decode_stats] = parse_result.value(); + cloud_watchdog_->update(); // Publish scan message only if it has been written to @@ -149,7 +150,7 @@ void HesaiDecoderWrapper::process_cloud_packet( auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + rclcpp::Time(seconds_to_chrono_nano_seconds(timestamp_s).count()); publish_cloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); } if ( @@ -160,18 +161,18 @@ void HesaiDecoderWrapper::process_cloud_packet( auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + rclcpp::Time(seconds_to_chrono_nano_seconds(timestamp_s).count()); publish_cloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); } if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = nebula::drivers::convert_point_xyzircaedt_to_point_xyziradt( - pointcloud, std::get<1>(pointcloud_ts)); + const auto autoware_ex_cloud = + nebula::drivers::convert_point_xyzircaedt_to_point_xyziradt(pointcloud, timestamp_s); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + rclcpp::Time(seconds_to_chrono_nano_seconds(timestamp_s).count()); publish_cloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); } } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index e2feb146c..62b6e98b7 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -146,7 +146,7 @@ Status HesaiRosDecoderTest::GetParameters( } void HesaiRosDecoderTest::read_bag( - std::function scan_callback) + std::function scan_callback) { rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; @@ -188,16 +188,15 @@ void HesaiRosDecoderTest::read_bag( auto extracted_msg_ptr = std::make_shared(extracted_msg); for (auto & pkt : extracted_msg_ptr->packets) { - auto pointcloud_ts = driver_ptr_->parse_cloud_packet( + auto decode_result = driver_ptr_->parse_cloud_packet( std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); - auto pointcloud = std::get<0>(pointcloud_ts); - auto scan_timestamp = std::get<1>(pointcloud_ts); - if (!pointcloud) { + if (!decode_result) { continue; } + const auto & [pointcloud, timestamp_s, decode_stats] = decode_result.value(); - scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); + scan_callback(bag_message->time_stamp, timestamp_s, pointcloud); } } } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index 75caf10de..43f322cac 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -130,7 +130,7 @@ class HesaiRosDecoderTest final : public rclcpp::Node /// @brief Read the specified bag file and compare the constructed point clouds with the /// corresponding PCD files void read_bag( - std::function scan_callback); + std::function scan_callback); HesaiRosDecoderTestParams params_; }; diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index e6402e098..8d3163bcd 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -55,7 +55,7 @@ TEST_P(DecoderTest, TestPcd) int check_cnt = 0; auto scan_callback = [&]( - uint64_t msg_timestamp, uint64_t /*scan_timestamp*/, + uint64_t msg_timestamp, double /*scan_timestamp*/, nebula::drivers::NebulaPointCloudPtr pointcloud) { if (!pointcloud) return; @@ -89,7 +89,7 @@ TEST_P(DecoderTest, TestTimezone) std::vector decoded_timestamps; auto scan_callback = [&]( - uint64_t /*msg_timestamp*/, uint64_t scan_timestamp, + uint64_t /*msg_timestamp*/, double scan_timestamp, nebula::drivers::NebulaPointCloudPtr pointcloud) { if (!pointcloud) return; decoded_timestamps.push_back(scan_timestamp);