From 52eb0a2a95f3b68852fd3e912fabcc3cd7b72891 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 20 Dec 2024 15:15:59 +0900 Subject: [PATCH 01/14] add filterd pointcloud counter function --- .../decoders/hesai_decoder.hpp | 64 +++++++++++++++++++ ros2_socketcan | 1 + transport_drivers | 1 + 3 files changed, 66 insertions(+) create mode 160000 ros2_socketcan create mode 160000 transport_drivers 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 314c40473..008131e58 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 @@ -32,10 +32,57 @@ #include #include #include +#include namespace nebula::drivers { +struct HesaiDecodeFilteredInfo +{ + uint16_t distance_counter = 0; + uint16_t fov_counter = 0; + uint16_t timestamp_counter = 0; + float distance_start = 0; + float disntance_end = 0; + float raw_azimuth_start = 0; + float raw_azimuth_end = 0; + std::uint32_t packet_timestamp_start = 0; + std::uint32_t packet_timestamp_end = 0; + NebulaPointCloud point_azimuth_start; + NebulaPointCloud point_azimuth_end; + NebulaPointCloud point_timestamp_start; + NebulaPointCloud point_timestamp_end; + + void clear() + { + distance_counter = 0; + fov_counter = 0; + raw_azimuth_start = 0; + raw_azimuth_end = 0; + packet_timestamp_start = 0; + packet_timestamp_end = 0; + point_azimuth_start = NebulaPointCloud(); + point_azimuth_end = NebulaPointCloud(); + point_timestamp_start = NebulaPointCloud(); + point_timestamp_end = NebulaPointCloud(); + } + + [[nodiscard]] nlohmann::ordered_json to_json() const + { + nlohmann::ordered_json j; + j["distance_counter"] = distance_counter; + j["fov_counter"] = fov_counter; + j["timestamp_counter"] = timestamp_counter; + j["distance_start"] = distance_start; + j["disntance_end"] = disntance_end; + j["raw_azimuth_start"] = raw_azimuth_start; + j["raw_azimuth_end"] = raw_azimuth_end; + j["packet_timestamp_start"] = packet_timestamp_start; + j["packet_timestamp_end"] = packet_timestamp_end; + return j; + } +}; + template class HesaiDecoder : public HesaiScanDecoder { @@ -76,6 +123,9 @@ class HesaiDecoder : public HesaiScanDecoder rclcpp::Logger logger_; + // filtered pointcloud counter + HesaiDecodeFilteredInfo decode_filtered_info_; + /// @brief For each channel, its firing offset relative to the block in nanoseconds std::array channel_firing_offset_ns_; /// @brief For each return mode, the firing offset of each block relative to its packet in @@ -83,6 +133,16 @@ class HesaiDecoder : public HesaiScanDecoder std::array, SensorT::packet_t::max_returns> block_firing_offset_ns_; + void get_minmax_info(const NebulaPoint &point) + { + decode_filtered_info_.raw_azimuth_start = std::min(decode_filtered_info_.raw_azimuth_start, point.azimuth); + decode_filtered_info_.raw_azimuth_end = std::max(decode_filtered_info_.raw_azimuth_end, point.azimuth); + decode_filtered_info_.packet_timestamp_start = std::min(decode_filtered_info_.packet_timestamp_start, point.time_stamp); + decode_filtered_info_.packet_timestamp_end = std::max(decode_filtered_info_.packet_timestamp_end, point.time_stamp); + decode_filtered_info_.distance_start = std::min(decode_filtered_info_.distance_start, point.distance); + decode_filtered_info_.disntance_end = std::max(decode_filtered_info_.disntance_end, point.distance); + } + /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully @@ -138,6 +198,7 @@ class HesaiDecoder : public HesaiScanDecoder distance < SensorT::min_range || SensorT::max_range < distance || distance < sensor_configuration_->min_range || sensor_configuration_->max_range < distance) { + decode_filtered_info_.distance_counter++; continue; } @@ -178,6 +239,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) { + decode_filtered_info_.fov_counter++; continue; } @@ -214,6 +276,8 @@ class HesaiDecoder : public HesaiScanDecoder // The driver wrapper converts to degrees, expects radians point.azimuth = corrected_angle_data.azimuth_rad; point.elevation = corrected_angle_data.elevation_rad; + + get_minmax_info(point); } } } diff --git a/ros2_socketcan b/ros2_socketcan new file mode 160000 index 000000000..4ced5284f --- /dev/null +++ b/ros2_socketcan @@ -0,0 +1 @@ +Subproject commit 4ced5284fdd080274b04530406d887e8330a0cdb diff --git a/transport_drivers b/transport_drivers new file mode 160000 index 000000000..86b9aae85 --- /dev/null +++ b/transport_drivers @@ -0,0 +1 @@ +Subproject commit 86b9aae8555dbb4a3103c21475d7f5e2d49981ab From 4379451e451a6649e7cd7e0db7c667e8be8ca569 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 20 Dec 2024 06:39:29 +0000 Subject: [PATCH 02/14] ci(pre-commit): autofix --- .../decoders/hesai_decoder.hpp | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) 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 008131e58..452eb758c 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 @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -32,7 +33,6 @@ #include #include #include -#include namespace nebula::drivers { @@ -133,14 +133,20 @@ class HesaiDecoder : public HesaiScanDecoder std::array, SensorT::packet_t::max_returns> block_firing_offset_ns_; - void get_minmax_info(const NebulaPoint &point) + void get_minmax_info(const NebulaPoint & point) { - decode_filtered_info_.raw_azimuth_start = std::min(decode_filtered_info_.raw_azimuth_start, point.azimuth); - decode_filtered_info_.raw_azimuth_end = std::max(decode_filtered_info_.raw_azimuth_end, point.azimuth); - decode_filtered_info_.packet_timestamp_start = std::min(decode_filtered_info_.packet_timestamp_start, point.time_stamp); - decode_filtered_info_.packet_timestamp_end = std::max(decode_filtered_info_.packet_timestamp_end, point.time_stamp); - decode_filtered_info_.distance_start = std::min(decode_filtered_info_.distance_start, point.distance); - decode_filtered_info_.disntance_end = std::max(decode_filtered_info_.disntance_end, point.distance); + decode_filtered_info_.raw_azimuth_start = + std::min(decode_filtered_info_.raw_azimuth_start, point.azimuth); + decode_filtered_info_.raw_azimuth_end = + std::max(decode_filtered_info_.raw_azimuth_end, point.azimuth); + decode_filtered_info_.packet_timestamp_start = + std::min(decode_filtered_info_.packet_timestamp_start, point.time_stamp); + decode_filtered_info_.packet_timestamp_end = + std::max(decode_filtered_info_.packet_timestamp_end, point.time_stamp); + decode_filtered_info_.distance_start = + std::min(decode_filtered_info_.distance_start, point.distance); + decode_filtered_info_.disntance_end = + std::max(decode_filtered_info_.disntance_end, point.distance); } /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. From 1e366d65c789443b9b53b5f0e06a9ee63de44dfb Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 20 Dec 2024 15:51:03 +0900 Subject: [PATCH 03/14] del submodule --- ros2_socketcan | 1 - transport_drivers | 1 - 2 files changed, 2 deletions(-) delete mode 160000 ros2_socketcan delete mode 160000 transport_drivers diff --git a/ros2_socketcan b/ros2_socketcan deleted file mode 160000 index 4ced5284f..000000000 --- a/ros2_socketcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 4ced5284fdd080274b04530406d887e8330a0cdb diff --git a/transport_drivers b/transport_drivers deleted file mode 160000 index 86b9aae85..000000000 --- a/transport_drivers +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 86b9aae8555dbb4a3103c21475d7f5e2d49981ab From c20d6bf9721aa220b21cfabdc50058728b4d84e8 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 20 Dec 2024 15:51:11 +0900 Subject: [PATCH 04/14] fix type --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 452eb758c..0176082c4 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 @@ -43,7 +43,7 @@ struct HesaiDecodeFilteredInfo uint16_t fov_counter = 0; uint16_t timestamp_counter = 0; float distance_start = 0; - float disntance_end = 0; + float distance_end = 0; float raw_azimuth_start = 0; float raw_azimuth_end = 0; std::uint32_t packet_timestamp_start = 0; @@ -74,7 +74,7 @@ struct HesaiDecodeFilteredInfo j["fov_counter"] = fov_counter; j["timestamp_counter"] = timestamp_counter; j["distance_start"] = distance_start; - j["disntance_end"] = disntance_end; + j["distance_end"] = distance_end; j["raw_azimuth_start"] = raw_azimuth_start; j["raw_azimuth_end"] = raw_azimuth_end; j["packet_timestamp_start"] = packet_timestamp_start; @@ -145,8 +145,8 @@ class HesaiDecoder : public HesaiScanDecoder std::max(decode_filtered_info_.packet_timestamp_end, point.time_stamp); decode_filtered_info_.distance_start = std::min(decode_filtered_info_.distance_start, point.distance); - decode_filtered_info_.disntance_end = - std::max(decode_filtered_info_.disntance_end, point.distance); + decode_filtered_info_.distance_end = + std::max(decode_filtered_info_.distance_end, point.distance); } /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. From 5cd9da2bb114807b0824e4bb9b743328a92dffda Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 27 Dec 2024 15:27:45 +0900 Subject: [PATCH 05/14] fix reviewed points --- .../decoders/hesai_decoder.hpp | 140 +++++++++++------- ros2_socketcan | 1 + transport_drivers | 1 + 3 files changed, 88 insertions(+), 54 deletions(-) create mode 160000 ros2_socketcan create mode 160000 transport_drivers 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 0176082c4..5296ddc19 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 @@ -18,14 +18,13 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" - #include #include #include #include #include #include - +#include #include #include #include @@ -39,48 +38,87 @@ namespace nebula::drivers struct HesaiDecodeFilteredInfo { - uint16_t distance_counter = 0; - uint16_t fov_counter = 0; - uint16_t timestamp_counter = 0; - float distance_start = 0; - float distance_end = 0; - float raw_azimuth_start = 0; - float raw_azimuth_end = 0; - std::uint32_t packet_timestamp_start = 0; - std::uint32_t packet_timestamp_end = 0; - NebulaPointCloud point_azimuth_start; - NebulaPointCloud point_azimuth_end; - NebulaPointCloud point_timestamp_start; - NebulaPointCloud point_timestamp_end; + uint16_t distance_filtered_count = 0; + uint16_t fov_filtered_count = 0; + uint16_t timestamp_filtered_count = 0; + uint16_t invalid_point_count = 0; + uint16_t identical_return_count = 0; + uint16_t mutliple_return_count = 0; + float cloud_distance_min_m = 0; + float cloud_distance_max_m = 0; + float cloud_azimuth_min_rad = 0; + float cloud_azimuth_max_rad = 0; + uint64_t packet_timestamp_min_ns = 0; + uint64_t packet_timestamp_max_ns = 0; void clear() { - distance_counter = 0; - fov_counter = 0; - raw_azimuth_start = 0; - raw_azimuth_end = 0; - packet_timestamp_start = 0; - packet_timestamp_end = 0; - point_azimuth_start = NebulaPointCloud(); - point_azimuth_end = NebulaPointCloud(); - point_timestamp_start = NebulaPointCloud(); - point_timestamp_end = NebulaPointCloud(); + distance_filtered_count = 0; + fov_filtered_count = 0; + timestamp_filtered_count = 0; + invalid_point_count = 0; + identical_return_count = 0; + mutliple_return_count = 0; + cloud_distance_min_m = 0; + cloud_distance_max_m = 0; + cloud_azimuth_min_rad = 0; + cloud_azimuth_max_rad = 0; + packet_timestamp_min_ns = 0; + packet_timestamp_max_ns = 0; } [[nodiscard]] nlohmann::ordered_json to_json() const { - nlohmann::ordered_json j; - j["distance_counter"] = distance_counter; - j["fov_counter"] = fov_counter; - j["timestamp_counter"] = timestamp_counter; - j["distance_start"] = distance_start; - j["distance_end"] = distance_end; - j["raw_azimuth_start"] = raw_azimuth_start; - j["raw_azimuth_end"] = raw_azimuth_end; - j["packet_timestamp_start"] = packet_timestamp_start; - j["packet_timestamp_end"] = packet_timestamp_end; + nlohmann::json distance_j; + distance_j["filter"] = "distance"; + distance_j["distance_filtered_count"] = distance_filtered_count; + distance_j["cloud_distance_min_m"] = cloud_distance_min_m; + distance_j["cloud_distance_max_m"] = cloud_distance_max_m; + nlohmann::json fov_j; + fov_j["filter"] = "fov"; + fov_j["fov_filtered_count"] = fov_filtered_count; + fov_j["cloud_azimuth_min_rad"] = cloud_azimuth_min_rad; + fov_j["cloud_azimuth_max_rad"] = cloud_azimuth_max_rad; + nlohmann::json timestamp_j; + timestamp_j["filter"] = "timestamp"; + timestamp_j["timestamp_filtered_count"] = timestamp_filtered_count; + timestamp_j["packet_timestamp_min_ns"] = packet_timestamp_min_ns; + timestamp_j["packet_timestamp_max_ns"] = packet_timestamp_max_ns; + nlohmann::json invalid_j; + invalid_j["filter"] = "invalid"; + invalid_j["invalid_point_count"] = invalid_point_count; + nlohmann::json identical_j; + identical_j["filter"] = "identical"; + identical_j["identical_return_count"] = identical_return_count; + nlohmann::json multiple_j; + multiple_j["filter"] = "multiple"; + multiple_j["mutliple_return_count"] = mutliple_return_count; + + nlohmann::json j; + j["filter_pipeline"] = nlohmann::json::array({ + distance_j, + fov_j, + timestamp_j, + }); + return j; } + + void get_minmax_info(const NebulaPoint & point) + { + cloud_azimuth_min_rad = + std::min(cloud_azimuth_min_rad, point.azimuth); + cloud_azimuth_max_rad = + std::max(cloud_azimuth_max_rad, point.azimuth); + packet_timestamp_min_ns = + std::min(packet_timestamp_min_ns, static_cast(point.time_stamp)); + packet_timestamp_max_ns = + std::max(packet_timestamp_max_ns, static_cast(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); + } }; template @@ -133,22 +171,6 @@ class HesaiDecoder : public HesaiScanDecoder std::array, SensorT::packet_t::max_returns> block_firing_offset_ns_; - void get_minmax_info(const NebulaPoint & point) - { - decode_filtered_info_.raw_azimuth_start = - std::min(decode_filtered_info_.raw_azimuth_start, point.azimuth); - decode_filtered_info_.raw_azimuth_end = - std::max(decode_filtered_info_.raw_azimuth_end, point.azimuth); - decode_filtered_info_.packet_timestamp_start = - std::min(decode_filtered_info_.packet_timestamp_start, point.time_stamp); - decode_filtered_info_.packet_timestamp_end = - std::max(decode_filtered_info_.packet_timestamp_end, point.time_stamp); - decode_filtered_info_.distance_start = - std::min(decode_filtered_info_.distance_start, point.distance); - decode_filtered_info_.distance_end = - std::max(decode_filtered_info_.distance_end, point.distance); - } - /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully @@ -195,6 +217,7 @@ class HesaiDecoder : public HesaiScanDecoder auto & unit = *return_units[block_offset]; if (unit.distance == 0) { + decode_filtered_info_.invalid_point_count++; continue; } @@ -204,7 +227,7 @@ class HesaiDecoder : public HesaiScanDecoder distance < SensorT::min_range || SensorT::max_range < distance || distance < sensor_configuration_->min_range || sensor_configuration_->max_range < distance) { - decode_filtered_info_.distance_counter++; + decode_filtered_info_.distance_filtered_count++; continue; } @@ -214,6 +237,7 @@ class HesaiDecoder : public HesaiScanDecoder // Keep only last of multiple identical points if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) { + decode_filtered_info_.identical_return_count++; continue; } @@ -235,6 +259,7 @@ class HesaiDecoder : public HesaiScanDecoder } if (is_below_multi_return_threshold) { + decode_filtered_info_.mutliple_return_count++; continue; } } @@ -245,7 +270,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) { - decode_filtered_info_.fov_counter++; + decode_filtered_info_.fov_filtered_count++; continue; } @@ -283,7 +308,7 @@ class HesaiDecoder : public HesaiScanDecoder point.azimuth = corrected_angle_data.azimuth_rad; point.elevation = corrected_angle_data.elevation_rad; - get_minmax_info(point); + decode_filtered_info_.get_minmax_info(point); } } } @@ -390,6 +415,13 @@ class HesaiDecoder : public HesaiScanDecoder std::swap(decode_pc_, output_pc_); std::swap(decode_scan_timestamp_ns_, output_scan_timestamp_ns_); has_scanned_ = true; + nlohmann::ordered_json j = decode_filtered_info_.to_json(); + std::cout << "=======================" << std::endl; + for (const auto & [key, value] : j.items()) { + std::cout << key << ": " << value << std::endl; + } + std::cout << "=======================" << std::endl; + decode_filtered_info_.clear(); } last_azimuth_ = block_azimuth; diff --git a/ros2_socketcan b/ros2_socketcan new file mode 160000 index 000000000..4ced5284f --- /dev/null +++ b/ros2_socketcan @@ -0,0 +1 @@ +Subproject commit 4ced5284fdd080274b04530406d887e8330a0cdb diff --git a/transport_drivers b/transport_drivers new file mode 160000 index 000000000..86b9aae85 --- /dev/null +++ b/transport_drivers @@ -0,0 +1 @@ +Subproject commit 86b9aae8555dbb4a3103c21475d7f5e2d49981ab From aaa34957e5132b885efeeb24c4c76e5df97bdc2e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 27 Dec 2024 06:28:25 +0000 Subject: [PATCH 06/14] ci(pre-commit): autofix --- .../decoders/hesai_decoder.hpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) 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 5296ddc19..deadc3ef8 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 @@ -18,13 +18,16 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" + #include #include #include #include #include #include + #include + #include #include #include @@ -100,24 +103,20 @@ struct HesaiDecodeFilteredInfo fov_j, timestamp_j, }); - + return j; } void get_minmax_info(const NebulaPoint & point) { - cloud_azimuth_min_rad = - std::min(cloud_azimuth_min_rad, point.azimuth); - cloud_azimuth_max_rad = - std::max(cloud_azimuth_max_rad, point.azimuth); + cloud_azimuth_min_rad = std::min(cloud_azimuth_min_rad, point.azimuth); + cloud_azimuth_max_rad = std::max(cloud_azimuth_max_rad, point.azimuth); packet_timestamp_min_ns = std::min(packet_timestamp_min_ns, static_cast(point.time_stamp)); packet_timestamp_max_ns = std::max(packet_timestamp_max_ns, static_cast(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); + cloud_distance_min_m = std::min(cloud_distance_min_m, point.distance); + cloud_distance_max_m = std::max(cloud_distance_max_m, point.distance); } }; From ff8b7f5e24f2366f79fbda8dfadc79d65aaab469 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 27 Dec 2024 17:58:09 +0900 Subject: [PATCH 07/14] fix reviewed points --- .../decoders/hesai_decoder.hpp | 49 ++++++++++++------- 1 file changed, 32 insertions(+), 17 deletions(-) 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 deadc3ef8..a89056324 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 @@ -18,16 +18,13 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" - #include #include #include #include #include #include - #include - #include #include #include @@ -45,8 +42,10 @@ struct HesaiDecodeFilteredInfo uint16_t fov_filtered_count = 0; uint16_t timestamp_filtered_count = 0; uint16_t invalid_point_count = 0; - uint16_t identical_return_count = 0; - uint16_t mutliple_return_count = 0; + uint16_t identical_return_point_count = 0; + uint16_t mutliple_return_point_count = 0; + uint16_t total_kept_point_count = 0; + uint16_t invalid_packet_count = 0; float cloud_distance_min_m = 0; float cloud_distance_max_m = 0; float cloud_azimuth_min_rad = 0; @@ -60,8 +59,10 @@ struct HesaiDecodeFilteredInfo fov_filtered_count = 0; timestamp_filtered_count = 0; invalid_point_count = 0; - identical_return_count = 0; - mutliple_return_count = 0; + identical_return_point_count = 0; + mutliple_return_point_count = 0; + total_kept_point_count = 0; + invalid_packet_count = 0; cloud_distance_min_m = 0; cloud_distance_max_m = 0; cloud_azimuth_min_rad = 0; @@ -90,33 +91,42 @@ struct HesaiDecodeFilteredInfo nlohmann::json invalid_j; invalid_j["filter"] = "invalid"; invalid_j["invalid_point_count"] = invalid_point_count; + invalid_j["invalid_packet_count"] = invalid_packet_count; nlohmann::json identical_j; identical_j["filter"] = "identical"; - identical_j["identical_return_count"] = identical_return_count; + identical_j["identical_return_point_count"] = identical_return_point_count; nlohmann::json multiple_j; multiple_j["filter"] = "multiple"; - multiple_j["mutliple_return_count"] = mutliple_return_count; + multiple_j["mutliple_return_point_count"] = mutliple_return_point_count; nlohmann::json j; j["filter_pipeline"] = nlohmann::json::array({ distance_j, fov_j, timestamp_j, + invalid_j, + identical_j, + multiple_j, }); - + j["total_kept_point_count"] = total_kept_point_count; + return j; } void get_minmax_info(const NebulaPoint & point) { - cloud_azimuth_min_rad = std::min(cloud_azimuth_min_rad, point.azimuth); - cloud_azimuth_max_rad = std::max(cloud_azimuth_max_rad, point.azimuth); + cloud_azimuth_min_rad = + std::min(cloud_azimuth_min_rad, point.azimuth); + cloud_azimuth_max_rad = + std::max(cloud_azimuth_max_rad, point.azimuth); packet_timestamp_min_ns = std::min(packet_timestamp_min_ns, static_cast(point.time_stamp)); packet_timestamp_max_ns = std::max(packet_timestamp_max_ns, static_cast(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); + cloud_distance_min_m = + std::min(cloud_distance_min_m, point.distance); + cloud_distance_max_m = + std::max(cloud_distance_max_m, point.distance); } }; @@ -236,7 +246,7 @@ class HesaiDecoder : public HesaiScanDecoder // Keep only last of multiple identical points if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) { - decode_filtered_info_.identical_return_count++; + decode_filtered_info_.identical_return_point_count++; continue; } @@ -258,7 +268,7 @@ class HesaiDecoder : public HesaiScanDecoder } if (is_below_multi_return_threshold) { - decode_filtered_info_.mutliple_return_count++; + decode_filtered_info_.mutliple_return_point_count++; continue; } } @@ -308,6 +318,7 @@ class HesaiDecoder : public HesaiScanDecoder point.elevation = corrected_angle_data.elevation_rad; decode_filtered_info_.get_minmax_info(point); + decode_filtered_info_.total_kept_point_count++; } } } @@ -363,6 +374,7 @@ class HesaiDecoder : public HesaiScanDecoder int unpack(const std::vector & packet) override { if (!parse_packet(packet)) { + decode_filtered_info_.invalid_packet_count++; return -1; } @@ -417,7 +429,10 @@ class HesaiDecoder : public HesaiScanDecoder nlohmann::ordered_json j = decode_filtered_info_.to_json(); std::cout << "=======================" << std::endl; for (const auto & [key, value] : j.items()) { - std::cout << key << ": " << value << std::endl; + std::cout << key << ": " << std::endl; + for (const auto & [k, v] : value.items()) { + std::cout << k << ": " << v << std::endl; + } } std::cout << "=======================" << std::endl; decode_filtered_info_.clear(); From 2706c08d78c43b50fb6feda133d02a9aa37f133c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 27 Dec 2024 08:58:49 +0000 Subject: [PATCH 08/14] ci(pre-commit): autofix --- .../decoders/hesai_decoder.hpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) 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 a89056324..228454cad 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 @@ -18,13 +18,16 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" + #include #include #include #include #include #include + #include + #include #include #include @@ -109,24 +112,20 @@ struct HesaiDecodeFilteredInfo multiple_j, }); j["total_kept_point_count"] = total_kept_point_count; - + return j; } void get_minmax_info(const NebulaPoint & point) { - cloud_azimuth_min_rad = - std::min(cloud_azimuth_min_rad, point.azimuth); - cloud_azimuth_max_rad = - std::max(cloud_azimuth_max_rad, point.azimuth); + cloud_azimuth_min_rad = std::min(cloud_azimuth_min_rad, point.azimuth); + cloud_azimuth_max_rad = std::max(cloud_azimuth_max_rad, point.azimuth); packet_timestamp_min_ns = std::min(packet_timestamp_min_ns, static_cast(point.time_stamp)); packet_timestamp_max_ns = std::max(packet_timestamp_max_ns, static_cast(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); + cloud_distance_min_m = std::min(cloud_distance_min_m, point.distance); + cloud_distance_max_m = std::max(cloud_distance_max_m, point.distance); } }; @@ -429,7 +428,7 @@ class HesaiDecoder : public HesaiScanDecoder nlohmann::ordered_json j = decode_filtered_info_.to_json(); std::cout << "=======================" << std::endl; for (const auto & [key, value] : j.items()) { - std::cout << key << ": " << std::endl; + std::cout << key << ": " << std::endl; for (const auto & [k, v] : value.items()) { std::cout << k << ": " << v << std::endl; } From d83d65cbb5730d88813c15ac9ed5201a1cdbec39 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 27 Dec 2024 18:01:15 +0900 Subject: [PATCH 09/14] fix typo --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 228454cad..97af8eff4 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 @@ -45,7 +45,7 @@ struct HesaiDecodeFilteredInfo uint16_t fov_filtered_count = 0; uint16_t timestamp_filtered_count = 0; uint16_t invalid_point_count = 0; - uint16_t identical_return_point_count = 0; + uint16_t multiple_return_point_count = 0; uint16_t mutliple_return_point_count = 0; uint16_t total_kept_point_count = 0; uint16_t invalid_packet_count = 0; @@ -62,7 +62,7 @@ struct HesaiDecodeFilteredInfo fov_filtered_count = 0; timestamp_filtered_count = 0; invalid_point_count = 0; - identical_return_point_count = 0; + multiple_return_point_count = 0; mutliple_return_point_count = 0; total_kept_point_count = 0; invalid_packet_count = 0; @@ -97,7 +97,7 @@ struct HesaiDecodeFilteredInfo invalid_j["invalid_packet_count"] = invalid_packet_count; nlohmann::json identical_j; identical_j["filter"] = "identical"; - identical_j["identical_return_point_count"] = identical_return_point_count; + identical_j["multiple_return_point_count"] = multiple_return_point_count; nlohmann::json multiple_j; multiple_j["filter"] = "multiple"; multiple_j["mutliple_return_point_count"] = mutliple_return_point_count; @@ -245,7 +245,7 @@ class HesaiDecoder : public HesaiScanDecoder // Keep only last of multiple identical points if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) { - decode_filtered_info_.identical_return_point_count++; + decode_filtered_info_.multiple_return_point_count++; continue; } From 411a9baf3fa05773d55129d9172396fa36e11b6a Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 10 Jan 2025 12:43:25 +0900 Subject: [PATCH 10/14] fix reviewed points --- .../decoders/hesai_decoder.hpp | 68 +++++++------------ 1 file changed, 24 insertions(+), 44 deletions(-) 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 97af8eff4..5efaf1b0f 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 @@ -25,8 +25,7 @@ #include #include #include - -#include +#include #include #include @@ -41,38 +40,19 @@ namespace nebula::drivers struct HesaiDecodeFilteredInfo { - uint16_t distance_filtered_count = 0; - uint16_t fov_filtered_count = 0; - uint16_t timestamp_filtered_count = 0; - uint16_t invalid_point_count = 0; - uint16_t multiple_return_point_count = 0; - uint16_t mutliple_return_point_count = 0; - uint16_t total_kept_point_count = 0; - uint16_t invalid_packet_count = 0; - float cloud_distance_min_m = 0; - float cloud_distance_max_m = 0; - float cloud_azimuth_min_rad = 0; - float cloud_azimuth_max_rad = 0; - uint64_t packet_timestamp_min_ns = 0; - uint64_t packet_timestamp_max_ns = 0; - - void clear() - { - distance_filtered_count = 0; - fov_filtered_count = 0; - timestamp_filtered_count = 0; - invalid_point_count = 0; - multiple_return_point_count = 0; - mutliple_return_point_count = 0; - total_kept_point_count = 0; - invalid_packet_count = 0; - cloud_distance_min_m = 0; - cloud_distance_max_m = 0; - cloud_azimuth_min_rad = 0; - cloud_azimuth_max_rad = 0; - packet_timestamp_min_ns = 0; - packet_timestamp_max_ns = 0; - } + 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; + 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::lowest(); + float cloud_azimuth_min_deg = std::numeric_limits::infinity(); + float cloud_azimuth_max_rad = std::numeric_limits::lowest(); + uint64_t packet_timestamp_min_ns = std::numeric_limits::max(); + uint64_t packet_timestamp_max_ns = std::numeric_limits::min(); [[nodiscard]] nlohmann::ordered_json to_json() const { @@ -84,11 +64,10 @@ struct HesaiDecodeFilteredInfo nlohmann::json fov_j; fov_j["filter"] = "fov"; fov_j["fov_filtered_count"] = fov_filtered_count; - fov_j["cloud_azimuth_min_rad"] = cloud_azimuth_min_rad; + fov_j["cloud_azimuth_min_deg"] = cloud_azimuth_min_deg; fov_j["cloud_azimuth_max_rad"] = cloud_azimuth_max_rad; nlohmann::json timestamp_j; timestamp_j["filter"] = "timestamp"; - timestamp_j["timestamp_filtered_count"] = timestamp_filtered_count; timestamp_j["packet_timestamp_min_ns"] = packet_timestamp_min_ns; timestamp_j["packet_timestamp_max_ns"] = packet_timestamp_max_ns; nlohmann::json invalid_j; @@ -97,10 +76,10 @@ struct HesaiDecodeFilteredInfo invalid_j["invalid_packet_count"] = invalid_packet_count; nlohmann::json identical_j; identical_j["filter"] = "identical"; - identical_j["multiple_return_point_count"] = multiple_return_point_count; + identical_j["identical_filtered_count"] = identical_filtered_count; nlohmann::json multiple_j; multiple_j["filter"] = "multiple"; - multiple_j["mutliple_return_point_count"] = mutliple_return_point_count; + multiple_j["multiple_return_filtered_count"] = multiple_return_filtered_count; nlohmann::json j; j["filter_pipeline"] = nlohmann::json::array({ @@ -116,9 +95,9 @@ struct HesaiDecodeFilteredInfo return j; } - void get_minmax_info(const NebulaPoint & point) + void update_pointcloud_bounds(const NebulaPoint & point) { - cloud_azimuth_min_rad = std::min(cloud_azimuth_min_rad, point.azimuth); + cloud_azimuth_min_deg = std::min(cloud_azimuth_min_deg, point.azimuth); cloud_azimuth_max_rad = std::max(cloud_azimuth_max_rad, point.azimuth); packet_timestamp_min_ns = std::min(packet_timestamp_min_ns, static_cast(point.time_stamp)); @@ -245,7 +224,7 @@ class HesaiDecoder : public HesaiScanDecoder // Keep only last of multiple identical points if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) { - decode_filtered_info_.multiple_return_point_count++; + decode_filtered_info_.identical_filtered_count++; continue; } @@ -267,7 +246,7 @@ class HesaiDecoder : public HesaiScanDecoder } if (is_below_multi_return_threshold) { - decode_filtered_info_.mutliple_return_point_count++; + decode_filtered_info_.multiple_return_filtered_count++; continue; } } @@ -316,7 +295,7 @@ class HesaiDecoder : public HesaiScanDecoder point.azimuth = corrected_angle_data.azimuth_rad; point.elevation = corrected_angle_data.elevation_rad; - decode_filtered_info_.get_minmax_info(point); + decode_filtered_info_.update_pointcloud_bounds(point); decode_filtered_info_.total_kept_point_count++; } } @@ -434,7 +413,8 @@ class HesaiDecoder : public HesaiScanDecoder } } std::cout << "=======================" << std::endl; - decode_filtered_info_.clear(); + decode_filtered_info_ = HesaiDecodeFilteredInfo{}; + } last_azimuth_ = block_azimuth; From b5fddfbdcb43db88c9e06bae494bac24bd3025e9 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 10 Jan 2025 12:43:36 +0900 Subject: [PATCH 11/14] del submodule --- ros2_socketcan | 1 - transport_drivers | 1 - 2 files changed, 2 deletions(-) delete mode 160000 ros2_socketcan delete mode 160000 transport_drivers diff --git a/ros2_socketcan b/ros2_socketcan deleted file mode 160000 index 4ced5284f..000000000 --- a/ros2_socketcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 4ced5284fdd080274b04530406d887e8330a0cdb diff --git a/transport_drivers b/transport_drivers deleted file mode 160000 index 86b9aae85..000000000 --- a/transport_drivers +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 86b9aae8555dbb4a3103c21475d7f5e2d49981ab From 6a62c3daf61314d017742d1fe63cd7bc23b694a5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 10 Jan 2025 03:44:42 +0000 Subject: [PATCH 12/14] ci(pre-commit): autofix --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 5efaf1b0f..52c6ff1a2 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 @@ -25,11 +25,11 @@ #include #include #include -#include #include #include #include +#include #include #include #include @@ -414,7 +414,6 @@ class HesaiDecoder : public HesaiScanDecoder } std::cout << "=======================" << std::endl; decode_filtered_info_ = HesaiDecodeFilteredInfo{}; - } last_azimuth_ = block_azimuth; From cf3ddb5d93bf5e65b1b8e9d68898e91562f7471c Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 10 Jan 2025 15:01:24 +0900 Subject: [PATCH 13/14] fix json struct --- .../decoders/hesai_decoder.hpp | 51 +++++++++++-------- 1 file changed, 30 insertions(+), 21 deletions(-) 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 52c6ff1a2..4f34b2065 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 @@ -57,39 +57,48 @@ struct HesaiDecodeFilteredInfo [[nodiscard]] nlohmann::ordered_json to_json() const { nlohmann::json distance_j; - distance_j["filter"] = "distance"; - distance_j["distance_filtered_count"] = distance_filtered_count; - distance_j["cloud_distance_min_m"] = cloud_distance_min_m; - distance_j["cloud_distance_max_m"] = cloud_distance_max_m; + distance_j["name"] = "distance"; + distance_j["filtered_count"] = distance_filtered_count; + // distance_j["cloud_distance_min_m"] = cloud_distance_min_m; + // distance_j["cloud_distance_max_m"] = cloud_distance_max_m; nlohmann::json fov_j; - fov_j["filter"] = "fov"; - fov_j["fov_filtered_count"] = fov_filtered_count; - fov_j["cloud_azimuth_min_deg"] = cloud_azimuth_min_deg; - fov_j["cloud_azimuth_max_rad"] = cloud_azimuth_max_rad; - nlohmann::json timestamp_j; - timestamp_j["filter"] = "timestamp"; - timestamp_j["packet_timestamp_min_ns"] = packet_timestamp_min_ns; - timestamp_j["packet_timestamp_max_ns"] = packet_timestamp_max_ns; + fov_j["name"] = "fov"; + fov_j["filtered_count"] = fov_filtered_count; + // fov_j["cloud_azimuth_min_deg"] = cloud_azimuth_min_deg; + // fov_j["cloud_azimuth_max_rad"] = cloud_azimuth_max_rad; + nlohmann::json identical_j; + identical_j["name"] = "identical"; + identical_j["filtered_count"] = identical_filtered_count; + nlohmann::json multiple_j; + multiple_j["name"] = "multiple"; + multiple_j["filtered_count"] = multiple_return_filtered_count; nlohmann::json invalid_j; - invalid_j["filter"] = "invalid"; + invalid_j["name"] = "invalid"; invalid_j["invalid_point_count"] = invalid_point_count; invalid_j["invalid_packet_count"] = invalid_packet_count; - nlohmann::json identical_j; - identical_j["filter"] = "identical"; - identical_j["identical_filtered_count"] = identical_filtered_count; - nlohmann::json multiple_j; - multiple_j["filter"] = "multiple"; - multiple_j["multiple_return_filtered_count"] = multiple_return_filtered_count; + nlohmann::json pointcloud_bounds_azimuth_j; + pointcloud_bounds_azimuth_j["min"] = cloud_azimuth_min_deg; + pointcloud_bounds_azimuth_j["max"] = cloud_azimuth_max_rad; + nlohmann::json pointcloud_bounds_distance_j; + pointcloud_bounds_distance_j["min"] = cloud_distance_min_m; + pointcloud_bounds_distance_j["max"] = cloud_distance_max_m; + nlohmann::json pointcloud_bounds_timestamp_j; + pointcloud_bounds_timestamp_j["min"] = packet_timestamp_min_ns; + pointcloud_bounds_timestamp_j["max"] = packet_timestamp_max_ns; nlohmann::json j; j["filter_pipeline"] = nlohmann::json::array({ distance_j, fov_j, - timestamp_j, - invalid_j, identical_j, multiple_j, }); + j["pointcloud_bounds"] ={ + j["azimuth_deg"] = pointcloud_bounds_azimuth_j, + j["distance_m"] = pointcloud_bounds_distance_j, + j["timestamp_ns"] = pointcloud_bounds_timestamp_j, + }; + j["invalid_filter"] = invalid_j; j["total_kept_point_count"] = total_kept_point_count; return j; From 92db91d9dbee5aa67331d9cc36f072c0c22f2471 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 10 Jan 2025 06:02:14 +0000 Subject: [PATCH 14/14] ci(pre-commit): autofix --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4f34b2065..ba02a41b3 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 @@ -93,7 +93,7 @@ struct HesaiDecodeFilteredInfo identical_j, multiple_j, }); - j["pointcloud_bounds"] ={ + j["pointcloud_bounds"] = { j["azimuth_deg"] = pointcloud_bounds_azimuth_j, j["distance_m"] = pointcloud_bounds_distance_j, j["timestamp_ns"] = pointcloud_bounds_timestamp_j,