From 52eb0a2a95f3b68852fd3e912fabcc3cd7b72891 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 20 Dec 2024 15:15:59 +0900 Subject: [PATCH 01/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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, From 915dc3227713e8030a2d055f8377f52bc09f2323 Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 28 Feb 2025 16:36:58 +0900 Subject: [PATCH 15/25] fix output layout --- .../decoders/hesai_decoder.hpp | 49 ++++++++----------- 1 file changed, 21 insertions(+), 28 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 ba02a41b3..10cf684fe 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 @@ -48,57 +48,55 @@ struct HesaiDecodeFilteredInfo 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_distance_max_m = -std::numeric_limits::infinity(); float cloud_azimuth_min_deg = std::numeric_limits::infinity(); - float cloud_azimuth_max_rad = std::numeric_limits::lowest(); + float cloud_azimuth_max_deg = -std::numeric_limits::infinity(); 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 { - nlohmann::json distance_j; + nlohmann::ordered_json distance_j; 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; + nlohmann::ordered_json fov_j; 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; + nlohmann::ordered_json identical_j; identical_j["name"] = "identical"; identical_j["filtered_count"] = identical_filtered_count; - nlohmann::json multiple_j; + nlohmann::ordered_json multiple_j; multiple_j["name"] = "multiple"; multiple_j["filtered_count"] = multiple_return_filtered_count; - nlohmann::json invalid_j; + nlohmann::ordered_json invalid_j; invalid_j["name"] = "invalid"; invalid_j["invalid_point_count"] = invalid_point_count; - invalid_j["invalid_packet_count"] = invalid_packet_count; - nlohmann::json pointcloud_bounds_azimuth_j; + nlohmann::ordered_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_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::json pointcloud_bounds_timestamp_j; + 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::json j; - j["filter_pipeline"] = nlohmann::json::array({ + nlohmann::ordered_json j; + j["filter_pipeline"] = nlohmann::ordered_json::array({ + invalid_j, distance_j, fov_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, + {"azimuth_deg", pointcloud_bounds_azimuth_j}, + {"distance_m", pointcloud_bounds_distance_j}, + {"timestamp_ns", pointcloud_bounds_timestamp_j}, }; - j["invalid_filter"] = invalid_j; + j["invalid_packet_count"] = invalid_packet_count; j["total_kept_point_count"] = total_kept_point_count; return j; @@ -106,8 +104,8 @@ struct HesaiDecodeFilteredInfo void update_pointcloud_bounds(const NebulaPoint & point) { - cloud_azimuth_min_deg = std::min(cloud_azimuth_min_deg, point.azimuth); - cloud_azimuth_max_rad = std::max(cloud_azimuth_max_rad, point.azimuth); + cloud_azimuth_min_deg = static_cast(std::min(cloud_azimuth_min_deg, point.azimuth * (180.0f / static_cast(M_PI)))); + cloud_azimuth_max_deg = static_cast(std::max(cloud_azimuth_max_deg, point.azimuth * (180.0f / static_cast(M_PI)))); packet_timestamp_min_ns = std::min(packet_timestamp_min_ns, static_cast(point.time_stamp)); packet_timestamp_max_ns = @@ -415,12 +413,7 @@ class HesaiDecoder : public HesaiScanDecoder 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 << ": " << std::endl; - for (const auto & [k, v] : value.items()) { - std::cout << k << ": " << v << std::endl; - } - } + std::cout << j.dump(2) << std::endl; std::cout << "=======================" << std::endl; decode_filtered_info_ = HesaiDecodeFilteredInfo{}; } From 46753217e8cf1571069f7db6b2ced0d714f18dcc Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 28 Feb 2025 16:39:53 +0900 Subject: [PATCH 16/25] del comment-out --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 2 -- 1 file changed, 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 10cf684fe..78b6b13f4 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 @@ -59,8 +59,6 @@ struct HesaiDecodeFilteredInfo nlohmann::ordered_json distance_j; 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::ordered_json fov_j; fov_j["name"] = "fov"; fov_j["filtered_count"] = fov_filtered_count; From c2ea81766836e183f36fbbf9d01a65bcbe55cdcc Mon Sep 17 00:00:00 2001 From: ike-kazu Date: Fri, 28 Feb 2025 16:45:45 +0900 Subject: [PATCH 17/25] change json name --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 2 +- nebula_ros/config/lidar/hesai/Pandar40P.param.yaml | 2 +- ros2_socketcan | 1 + transport_drivers | 1 + 4 files changed, 4 insertions(+), 2 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 78b6b13f4..1f4657134 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 @@ -70,7 +70,7 @@ struct HesaiDecodeFilteredInfo multiple_j["filtered_count"] = multiple_return_filtered_count; nlohmann::ordered_json invalid_j; invalid_j["name"] = "invalid"; - invalid_j["invalid_point_count"] = invalid_point_count; + 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; diff --git a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml index b74c26272..58d69b61f 100644 --- a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml @@ -12,7 +12,7 @@ frame_id: hesai diag_span: 1000 min_range: 0.3 - max_range: 300.0 + max_range: 5.0 cloud_min_angle: 0 cloud_max_angle: 360 sync_angle: 0 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 6cc219ec97589ec34caa7d1d623f0e31360624a6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 5 Mar 2025 02:00:46 +0000 Subject: [PATCH 18/25] ci(pre-commit): autofix --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 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 9139574d8..37feb15bb 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 @@ -105,8 +105,10 @@ struct HesaiDecodeFilteredInfo void update_pointcloud_bounds(const NebulaPoint & point) { - cloud_azimuth_min_deg = static_cast(std::min(cloud_azimuth_min_deg, point.azimuth * (180.0f / static_cast(M_PI)))); - cloud_azimuth_max_deg = static_cast(std::max(cloud_azimuth_max_deg, point.azimuth * (180.0f / static_cast(M_PI)))); + cloud_azimuth_min_deg = static_cast( + std::min(cloud_azimuth_min_deg, point.azimuth * (180.0f / static_cast(M_PI)))); + cloud_azimuth_max_deg = static_cast( + std::max(cloud_azimuth_max_deg, point.azimuth * (180.0f / static_cast(M_PI)))); packet_timestamp_min_ns = std::min(packet_timestamp_min_ns, static_cast(point.time_stamp)); packet_timestamp_max_ns = From 0fa5a9b831b94b14feab26265678b3b4f1821bba Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 5 Mar 2025 11:38:58 +0900 Subject: [PATCH 19/25] chore: remove wrongly committed submodules Signed-off-by: Max SCHMELLER --- 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 f77a7212ca480e34a321973b8772d16fef17133d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 5 Mar 2025 11:41:16 +0900 Subject: [PATCH 20/25] chore: clean up decode filter info, add integraion for mask filter Signed-off-by: Max SCHMELLER --- .../decoders/hesai_decoder.hpp | 189 ++++++++++-------- 1 file changed, 107 insertions(+), 82 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 37feb15bb..e40e1be32 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,6 +32,8 @@ #include #include #include +#include +#include #include #include #include @@ -41,86 +43,107 @@ namespace nebula::drivers { -struct HesaiDecodeFilteredInfo +template +class HesaiDecoder : public HesaiScanDecoder { - 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::infinity(); - float cloud_azimuth_min_deg = std::numeric_limits::infinity(); - float cloud_azimuth_max_deg = -std::numeric_limits::infinity(); - 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 + struct HesaiDecodeFilteredInfo { - 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, - }); - 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; - } + explicit HesaiDecodeFilteredInfo(bool has_downsample_mask_filter) + : downsample_mask_filtered_count( + has_downsample_mask_filter ? std::make_optional(0) : std::nullopt) + { + } - void update_pointcloud_bounds(const NebulaPoint & point) - { - cloud_azimuth_min_deg = static_cast( - std::min(cloud_azimuth_min_deg, point.azimuth * (180.0f / static_cast(M_PI)))); - cloud_azimuth_max_deg = static_cast( - std::max(cloud_azimuth_max_deg, point.azimuth * (180.0f / static_cast(M_PI)))); - 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); - } -}; + 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); + } + }; -template -class HesaiDecoder : public HesaiScanDecoder -{ struct ScanCutAngles { float fov_min; @@ -158,9 +181,6 @@ class HesaiDecoder : public HesaiScanDecoder std::shared_ptr 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 @@ -170,6 +190,9 @@ class HesaiDecoder : public HesaiScanDecoder std::optional mask_filter_; + // filtered pointcloud counter + HesaiDecodeFilteredInfo decode_filtered_info_; + /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully @@ -308,6 +331,7 @@ class HesaiDecoder : public HesaiScanDecoder point.elevation = corrected_angle_data.elevation_rad; if (mask_filter_ && mask_filter_->excluded(point)) { + *decode_filtered_info_.downsample_mask_filtered_count++; continue; } @@ -359,6 +383,8 @@ class HesaiDecoder : public HesaiScanDecoder { 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( @@ -367,8 +393,7 @@ 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); + decode_filtered_info_ = HesaiDecodeFilteredInfo(mask_filter_.has_value()); } int unpack(const std::vector & packet) override @@ -430,7 +455,7 @@ class HesaiDecoder : public HesaiScanDecoder std::cout << "=======================" << std::endl; std::cout << j.dump(2) << std::endl; std::cout << "=======================" << std::endl; - decode_filtered_info_ = HesaiDecodeFilteredInfo{}; + decode_filtered_info_ = HesaiDecodeFilteredInfo(mask_filter_.has_value()); } last_azimuth_ = block_azimuth; From 1fdf864b545260d4a39d50aaae8a730bf7d73560 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 5 Mar 2025 11:41:49 +0900 Subject: [PATCH 21/25] chore: revert wrongly committed Pandar40P range setting Signed-off-by: Max SCHMELLER --- nebula_ros/config/lidar/hesai/Pandar40P.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml index f6fcb09f6..850f52e1d 100644 --- a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml @@ -12,7 +12,7 @@ frame_id: hesai diag_span: 1000 min_range: 0.3 - max_range: 5.0 + max_range: 300.0 cloud_min_angle: 0 cloud_max_angle: 360 sync_angle: 0 From d0e82a8413ef07f47faf8a6a11dcca7a981a008f Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 5 Mar 2025 11:58:35 +0900 Subject: [PATCH 22/25] chore: fix mask filter integration Signed-off-by: Max SCHMELLER --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 5 +++-- 1 file changed, 3 insertions(+), 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 e40e1be32..0e33a0ce7 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 @@ -331,7 +331,7 @@ class HesaiDecoder : public HesaiScanDecoder point.elevation = corrected_angle_data.elevation_rad; if (mask_filter_ && mask_filter_->excluded(point)) { - *decode_filtered_info_.downsample_mask_filtered_count++; + (*decode_filtered_info_.downsample_mask_filtered_count)++; continue; } @@ -379,7 +379,8 @@ 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), + decode_filtered_info_(false) { decode_pc_ = std::make_shared(); output_pc_ = std::make_shared(); From 247e9ac83f856f53c6c42a2b3ae5c3e472b055ab Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 5 Mar 2025 18:35:55 +0900 Subject: [PATCH 23/25] chore: rename to PointcloudDecodeStatistics Signed-off-by: Max SCHMELLER --- .../decoders/hesai_decoder.hpp | 33 +++++++++---------- 1 file changed, 16 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 0e33a0ce7..9717fc4bf 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 @@ -46,9 +46,9 @@ namespace nebula::drivers template class HesaiDecoder : public HesaiScanDecoder { - struct HesaiDecodeFilteredInfo + struct PointcloudDecodeStatistics { - explicit HesaiDecodeFilteredInfo(bool has_downsample_mask_filter) + explicit PointcloudDecodeStatistics(bool has_downsample_mask_filter) : downsample_mask_filtered_count( has_downsample_mask_filter ? std::make_optional(0) : std::nullopt) { @@ -190,8 +190,7 @@ class HesaiDecoder : public HesaiScanDecoder std::optional mask_filter_; - // filtered pointcloud counter - HesaiDecodeFilteredInfo decode_filtered_info_; + PointcloudDecodeStatistics pointcloud_decode_statistics_; /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. /// @param packet The incoming PandarPacket @@ -239,7 +238,7 @@ class HesaiDecoder : public HesaiScanDecoder auto & unit = *return_units[block_offset]; if (unit.distance == 0) { - decode_filtered_info_.invalid_point_count++; + pointcloud_decode_statistics_.invalid_point_count++; continue; } @@ -249,7 +248,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_filtered_count++; + pointcloud_decode_statistics_.distance_filtered_count++; continue; } @@ -259,7 +258,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_filtered_count++; + pointcloud_decode_statistics_.identical_filtered_count++; continue; } @@ -281,7 +280,7 @@ class HesaiDecoder : public HesaiScanDecoder } if (is_below_multi_return_threshold) { - decode_filtered_info_.multiple_return_filtered_count++; + pointcloud_decode_statistics_.multiple_return_filtered_count++; continue; } } @@ -292,7 +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) { - decode_filtered_info_.fov_filtered_count++; + pointcloud_decode_statistics_.fov_filtered_count++; continue; } @@ -331,13 +330,13 @@ class HesaiDecoder : public HesaiScanDecoder point.elevation = corrected_angle_data.elevation_rad; if (mask_filter_ && mask_filter_->excluded(point)) { - (*decode_filtered_info_.downsample_mask_filtered_count)++; + (*pointcloud_decode_statistics_.downsample_mask_filtered_count)++; continue; } pc->emplace_back(point); - decode_filtered_info_.update_pointcloud_bounds(point); - decode_filtered_info_.total_kept_point_count++; + pointcloud_decode_statistics_.update_pointcloud_bounds(point); + pointcloud_decode_statistics_.total_kept_point_count++; } } } @@ -380,7 +379,7 @@ class HesaiDecoder : public HesaiScanDecoder {deg2rad(sensor_configuration_->cloud_min_angle), deg2rad(sensor_configuration_->cloud_max_angle), deg2rad(sensor_configuration_->cut_angle)}), logger_(logger), - decode_filtered_info_(false) + pointcloud_decode_statistics_(false) { decode_pc_ = std::make_shared(); output_pc_ = std::make_shared(); @@ -394,13 +393,13 @@ class HesaiDecoder : public HesaiScanDecoder logger_->child("Downsample Mask"), true); } - decode_filtered_info_ = HesaiDecodeFilteredInfo(mask_filter_.has_value()); + pointcloud_decode_statistics_ = PointcloudDecodeStatistics(mask_filter_.has_value()); } int unpack(const std::vector & packet) override { if (!parse_packet(packet)) { - decode_filtered_info_.invalid_packet_count++; + pointcloud_decode_statistics_.invalid_packet_count++; return -1; } @@ -452,11 +451,11 @@ 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(); + nlohmann::ordered_json j = pointcloud_decode_statistics_.to_json(); std::cout << "=======================" << std::endl; std::cout << j.dump(2) << std::endl; std::cout << "=======================" << std::endl; - decode_filtered_info_ = HesaiDecodeFilteredInfo(mask_filter_.has_value()); + pointcloud_decode_statistics_ = PointcloudDecodeStatistics(mask_filter_.has_value()); } last_azimuth_ = block_azimuth; From 24bc2e9070225c8f166a9e55c870b8e69869e3bc Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 5 Mar 2025 18:39:32 +0900 Subject: [PATCH 24/25] chore: remove print and pass decode_statistics along with pointcloud and timestamp Signed-off-by: Max SCHMELLER --- .../decoders/hesai_decoder.hpp | 10 +++------ .../decoders/hesai_scan_decoder.hpp | 4 +++- .../nebula_decoders_hesai/hesai_driver.hpp | 8 +++++-- .../nebula_decoders_hesai/hesai_driver.cpp | 11 ++++------ .../hesai_ros_offline_extract_bag_pcd.cpp | 4 ++-- .../hesai/hesai_ros_offline_extract_pcd.cpp | 4 ++-- nebula_ros/src/hesai/decoder_wrapper.cpp | 21 ++++++++++--------- nebula_tests/hesai/hesai_ros_decoder_test.cpp | 9 ++++---- 8 files changed, 35 insertions(+), 36 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 9717fc4bf..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 @@ -411,6 +411,7 @@ class HesaiDecoder : public HesaiScanDecoder if (has_scanned_) { output_pc_->clear(); + pointcloud_decode_statistics_ = PointcloudDecodeStatistics(mask_filter_.has_value()); has_scanned_ = false; } @@ -451,11 +452,6 @@ 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 = pointcloud_decode_statistics_.to_json(); - std::cout << "=======================" << std::endl; - std::cout << j.dump(2) << std::endl; - std::cout << "=======================" << std::endl; - pointcloud_decode_statistics_ = PointcloudDecodeStatistics(mask_filter_.has_value()); } last_azimuth_ = block_azimuth; @@ -466,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..5a77e049c 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -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); } } } From b030c223c66bde0be84c8bad51f84dff47c324e9 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 5 Mar 2025 18:39:50 +0900 Subject: [PATCH 25/25] chore: fix wrong timestamp type in Hesai tests Signed-off-by: Max SCHMELLER --- nebula_tests/hesai/hesai_ros_decoder_test.cpp | 2 +- nebula_tests/hesai/hesai_ros_decoder_test.hpp | 2 +- nebula_tests/hesai/hesai_ros_decoder_test_main.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index 5a77e049c..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; 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);