From 9c5e319c63ffce1b6ccb0270b808f80e0547fdd1 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 16 Jan 2025 12:18:47 +0900 Subject: [PATCH 01/12] chore(hesai_decoders): remove rclcpp logging dependency Signed-off-by: Max SCHMELLER --- .../decoders/hesai_decoder.hpp | 29 ++++++++++--------- .../nebula_decoders_hesai/hesai_driver.hpp | 6 +++- .../nebula_decoders_hesai/hesai_driver.cpp | 19 ++++-------- nebula_examples/CMakeLists.txt | 1 + .../hesai_ros_offline_extract_bag_pcd.cpp | 18 ++++++------ .../hesai/hesai_ros_offline_extract_pcd.cpp | 14 ++++----- nebula_ros/src/hesai/decoder_wrapper.cpp | 11 +++++-- nebula_tests/CMakeLists.txt | 3 ++ nebula_tests/hesai/hesai_ros_decoder_test.cpp | 8 ++--- 9 files changed, 57 insertions(+), 52 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 314c40473..88f5b66eb 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 @@ -20,6 +20,7 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" #include +#include #include #include #include @@ -28,7 +29,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -46,7 +49,7 @@ class HesaiDecoder : public HesaiScanDecoder float scan_emit_angle; }; -protected: +private: /// @brief Configuration for this decoder const std::shared_ptr sensor_configuration_; @@ -74,7 +77,7 @@ class HesaiDecoder : public HesaiScanDecoder ScanCutAngles scan_cut_angles_; uint32_t last_azimuth_ = 0; - rclcpp::Logger logger_; + std::shared_ptr logger_; /// @brief For each channel, its firing offset relative to the block in nanoseconds std::array channel_firing_offset_ns_; @@ -89,9 +92,9 @@ class HesaiDecoder : public HesaiScanDecoder bool parse_packet(const std::vector & packet) { if (packet.size() < sizeof(typename SensorT::packet_t)) { - RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch: " << packet.size() << " | Expected at least: " - << sizeof(typename SensorT::packet_t)); + NEBULA_LOG_STREAM( + logger_->error, "Packet size mismatch: " << packet.size() << " | Expected at least: " + << sizeof(typename SensorT::packet_t)); return false; } if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { @@ -100,7 +103,7 @@ class HesaiDecoder : public HesaiScanDecoder return true; } - RCLCPP_ERROR(logger_, "Packet memcopy failed"); + logger_->error("Packet memcopy failed"); return false; } @@ -246,24 +249,22 @@ class HesaiDecoder : public HesaiScanDecoder explicit HesaiDecoder( const std::shared_ptr & sensor_configuration, const std::shared_ptr & - correction_data) + correction_data, + const std::shared_ptr & logger) : sensor_configuration_(sensor_configuration), angle_corrector_( correction_data, sensor_configuration_->cloud_min_angle, sensor_configuration_->cloud_max_angle, sensor_configuration_->cut_angle), - logger_(rclcpp::get_logger("HesaiDecoder")) + scan_cut_angles_( + {deg2rad(sensor_configuration_->cloud_min_angle), + deg2rad(sensor_configuration_->cloud_max_angle), deg2rad(sensor_configuration_->cut_angle)}), + logger_(logger) { - logger_.set_level(rclcpp::Logger::Level::Debug); - 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); - - scan_cut_angles_ = { - deg2rad(sensor_configuration_->cloud_min_angle), - deg2rad(sensor_configuration_->cloud_max_angle), deg2rad(sensor_configuration_->cut_angle)}; } int unpack(const std::vector & packet) override 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 61992ba16..b256d6f92 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 @@ -20,6 +20,8 @@ #include "nebula_common/point_types.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" +#include + #include #include @@ -34,6 +36,7 @@ class HesaiDriver private: /// @brief Current driver status Status driver_status_; + std::shared_ptr logger_; /// @brief Decoder according to the model std::shared_ptr scan_decoder_; @@ -52,7 +55,8 @@ class HesaiDriver explicit HesaiDriver( const std::shared_ptr & sensor_configuration, const std::shared_ptr & - calibration_configuration); + calibration_configuration, + const std::shared_ptr & logger); /// @brief Get current status of this driver /// @return Current status diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index f14b2938f..2aa2da11f 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -18,13 +18,13 @@ #include #include -// #define WITH_DEBUG_STD_COUT_HESAI_CLIENT // Use std::cout messages for debugging - namespace nebula::drivers { HesaiDriver::HesaiDriver( const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_data) + const std::shared_ptr & calibration_data, + const std::shared_ptr & logger) +: logger_(logger) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; @@ -78,17 +78,17 @@ std::shared_ptr HesaiDriver::initialize_decoder( { using CalibT = typename SensorT::angle_corrector_t::correction_data_t; return std::make_shared>( - sensor_configuration, std::dynamic_pointer_cast(calibration_configuration)); + sensor_configuration, std::dynamic_pointer_cast(calibration_configuration), + logger_->child("Decoder")); } std::tuple HesaiDriver::parse_cloud_packet( const std::vector & packet) { std::tuple pointcloud; - auto logger = rclcpp::get_logger("HesaiDriver"); if (driver_status_ != nebula::Status::OK) { - RCLCPP_ERROR(logger, "Driver not OK."); + logger_->error("Driver not OK."); return pointcloud; } @@ -97,13 +97,6 @@ std::tuple HesaiDriver::parse_cloud_packet pointcloud = scan_decoder_->get_pointcloud(); } - // todo - // if (cnt == 0) { - // RCLCPP_ERROR_STREAM( - // logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " - // << "pointclouds were generated. Last azimuth: " << last_azimuth); - // } - return pointcloud; } diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index 779987293..63d1e6ad1 100644 --- a/nebula_examples/CMakeLists.txt +++ b/nebula_examples/CMakeLists.txt @@ -31,6 +31,7 @@ include_directories( link_libraries( ${nebula_common_TARGETS} + ${nebula_ros_TARGETS} ${rosbag2_cpp_TARGETS} ${PCL_LIBRARIES} ) 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 58ff1af19..9465c5f6e 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 @@ -14,15 +14,15 @@ #include "hesai/hesai_ros_offline_extract_bag_pcd.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/storage_options.hpp" - #include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -76,7 +76,7 @@ Status HesaiRosOfflineExtractBag::initialize_driver( { driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - calibration_configuration); + calibration_configuration, std::make_shared(get_logger())); return driver_ptr_->get_status(); } 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 9b9dff5b6..34da8c049 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -14,14 +14,12 @@ #include "hesai/hesai_ros_offline_extract_pcd.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_storage/storage_options.hpp" - #include +#include +#include +#include +#include +#include #include #include @@ -74,7 +72,7 @@ Status HesaiRosOfflineExtractSample::initialize_driver( // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - calibration_configuration); + calibration_configuration, std::make_shared(get_logger())); return driver_ptr_->get_status(); } diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 19902fd58..169773fc1 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -2,6 +2,8 @@ #include "nebula_ros/hesai/decoder_wrapper.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" + #include #include #include @@ -42,7 +44,8 @@ HesaiDecoderWrapper::HesaiDecoderWrapper( RCLCPP_INFO(logger_, "Starting Decoder"); - driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); + driver_ptr_ = std::make_shared( + config, calibration_cfg_ptr_, std::make_shared(logger_)); status_ = driver_ptr_->get_status(); if (Status::OK != status_) { @@ -81,7 +84,8 @@ void HesaiDecoderWrapper::on_config_change( const std::shared_ptr & new_config) { std::lock_guard lock(mtx_driver_ptr_); - auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); + auto new_driver = std::make_shared( + new_config, calibration_cfg_ptr_, std::make_shared(logger_)); driver_ptr_ = new_driver; sensor_cfg_ = new_config; } @@ -90,7 +94,8 @@ void HesaiDecoderWrapper::on_calibration_change( const std::shared_ptr & new_calibration) { std::lock_guard lock(mtx_driver_ptr_); - auto new_driver = std::make_shared(sensor_cfg_, new_calibration); + auto new_driver = std::make_shared( + sensor_cfg_, new_calibration, std::make_shared(logger_)); driver_ptr_ = new_driver; calibration_cfg_ptr_ = new_calibration; } diff --git a/nebula_tests/CMakeLists.txt b/nebula_tests/CMakeLists.txt index da2481a2f..dc4645660 100644 --- a/nebula_tests/CMakeLists.txt +++ b/nebula_tests/CMakeLists.txt @@ -24,6 +24,7 @@ endif() find_package(ament_cmake_auto REQUIRED) find_package(nebula_common REQUIRED) find_package(nebula_decoders REQUIRED) +find_package(nebula_ros REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(rosbag2_cpp REQUIRED) find_package(diagnostic_updater REQUIRED) @@ -42,6 +43,7 @@ if(BUILD_TESTING) set(NEBULA_TEST_INCLUDE_DIRS ${nebula_common_INCLUDE_DIRS} ${nebula_decoders_INCLUDE_DIRS} + ${nebula_ros_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${rosbag2_cpp_INCLUDE_DIRS} ${diagnostic_updater_INCLUDE_DIRS} @@ -49,6 +51,7 @@ if(BUILD_TESTING) set(NEBULA_TEST_LIBRARIES ${nebula_common_TARGETS} + ${nebula_ros_TARGETS} ${PCL_LIBRARIES} ${rosbag2_cpp_TARGETS} ${diagnostic_updater_TARGETS} diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index b5c77f9b7..e2feb146c 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -2,9 +2,9 @@ #include "hesai_ros_decoder_test.hpp" -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" - +#include +#include +#include #include #include #include @@ -68,7 +68,7 @@ Status HesaiRosDecoderTest::InitializeDriver( { driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - calibration_configuration); + calibration_configuration, std::make_shared(get_logger())); return driver_ptr_->get_status(); } From 96fc447b1c2f8035bb5cfc01a9e78b9626a4f0a4 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 16 Jan 2025 12:27:07 +0900 Subject: [PATCH 02/12] chore(nebula_ros): modernize how file extensions are replaced for calibration files Signed-off-by: Max SCHMELLER --- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 24 ++++++++-------------- 1 file changed, 9 insertions(+), 15 deletions(-) diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index a464ff1fd..5f36e4747 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -460,27 +460,21 @@ HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::get_calibration_data( calib = std::make_shared(); } - std::string calibration_file_path_from_sensor; - - { - int ext_pos = calibration_file_path.find_last_of('.'); - calibration_file_path_from_sensor = calibration_file_path.substr(0, ext_pos); - calibration_file_path_from_sensor += "_from_sensor_" + sensor_cfg_ptr_->sensor_ip; - calibration_file_path_from_sensor += - calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); - } + std::filesystem::path calibration_from_sensor_path{calibration_file_path}; + calibration_from_sensor_path = calibration_from_sensor_path.replace_extension( + "_from_sensor_" + sensor_cfg_ptr_->sensor_ip + + calibration_from_sensor_path.extension().string()); // If a sensor is connected, try to download and save its calibration data if (!ignore_others && launch_hw_) { try { auto raw_data = hw_interface_wrapper_->hw_interface()->get_lidar_calibration_bytes(); RCLCPP_INFO(logger, "Downloaded calibration data from sensor."); - auto status = calib->save_to_file_from_bytes(calibration_file_path_from_sensor, raw_data); + auto status = calib->save_to_file_from_bytes(calibration_from_sensor_path, raw_data); if (status != Status::OK) { RCLCPP_ERROR_STREAM(logger, "Could not save calibration data: " << status); } else { - RCLCPP_INFO_STREAM( - logger, "Saved downloaded data to " << calibration_file_path_from_sensor); + RCLCPP_INFO_STREAM(logger, "Saved downloaded data to " << calibration_from_sensor_path); } } catch (std::runtime_error & e) { RCLCPP_ERROR_STREAM(logger, "Could not download calibration data: " << e.what()); @@ -489,10 +483,10 @@ HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::get_calibration_data( // If saved calibration data from a sensor exists (either just downloaded above, or previously), // try to load it - if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) { - auto status = calib->load_from_file(calibration_file_path_from_sensor); + if (!ignore_others && std::filesystem::exists(calibration_from_sensor_path)) { + auto status = calib->load_from_file(calibration_from_sensor_path); if (status == Status::OK) { - calib->calibration_file = calibration_file_path_from_sensor; + calib->calibration_file = calibration_from_sensor_path; return calib; } From 8a9a92d1a4d461ad9f25d838c1eb579926d1431b Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 16 Jan 2025 12:28:56 +0900 Subject: [PATCH 03/12] chore(hesai): add downsample_mask_path config parameter Signed-off-by: Max SCHMELLER --- .../include/nebula_common/hesai/hesai_common.hpp | 4 +++- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 14 +++++++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index e9a6fce69..96a04a3af 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -50,6 +50,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase PtpTransportType ptp_transport_type; PtpSwitchType ptp_switch_type; uint8_t ptp_lock_threshold; + std::string downsample_mask_path; }; /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator) /// @param os @@ -73,7 +74,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n'; os << "PTP Transport Type: " << arg.ptp_transport_type << '\n'; os << "PTP Switch Type: " << arg.ptp_switch_type << '\n'; - os << "PTP Lock Threshold: " << std::to_string(arg.ptp_lock_threshold); + os << "PTP Lock Threshold: " << std::to_string(arg.ptp_lock_threshold) << '\n'; + os << "Downsample Mask Path: " << arg.downsample_mask_path; return os; } diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 5f36e4747..ff82d934b 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -209,6 +209,9 @@ nebula::Status HesaiRosWrapper::declare_and_get_sensor_config_params() config.ptp_lock_threshold = declare_parameter("ptp_lock_threshold", descriptor); } + config.downsample_mask_path = + declare_parameter("point_filters.downsample_mask.path", "", param_read_write()); + auto new_cfg_ptr = std::make_shared(config); return validate_and_set_config(new_cfg_ptr); } @@ -268,6 +271,14 @@ Status HesaiRosWrapper::validate_and_set_config( return Status::SENSOR_CONFIG_ERROR; } + if ( + !new_config->downsample_mask_path.empty() && + !std::filesystem::exists(new_config->downsample_mask_path)) { + RCLCPP_ERROR_STREAM( + get_logger(), "Downsample mask not found: " << new_config->downsample_mask_path); + return Status::SENSOR_CONFIG_ERROR; + } + if (hw_interface_wrapper_) { hw_interface_wrapper_->on_config_change(new_config); } @@ -346,7 +357,8 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold) | - get_param(p, calibration_parameter_name, new_cfg.calibration_path); + get_param(p, calibration_parameter_name, new_cfg.calibration_path) | + get_param(p, "point_filters.downsample_mask.path", new_cfg.downsample_mask_path); // Currently, all of the sub-wrappers read-only parameters, so they do not be queried for updates From 0c56c3029b4660b544144ceb587d02608bb67acf Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 16 Jan 2025 12:29:17 +0900 Subject: [PATCH 04/12] chore(hesai): add optional point_filters parameter to schema Signed-off-by: Max SCHMELLER --- nebula_ros/schema/Pandar128E4X.schema.json | 3 +++ nebula_ros/schema/Pandar40P.schema.json | 3 +++ nebula_ros/schema/Pandar64.schema.json | 3 +++ nebula_ros/schema/PandarAT128.schema.json | 3 +++ nebula_ros/schema/PandarQT128.schema.json | 3 +++ nebula_ros/schema/PandarQT64.schema.json | 3 +++ nebula_ros/schema/PandarXT32.schema.json | 3 +++ nebula_ros/schema/PandarXT32M.schema.json | 3 +++ 8 files changed, 24 insertions(+) diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index cae88b605..67e98f533 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/nebula_ros/schema/Pandar128E4X.schema.json @@ -107,6 +107,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json index e0b2ef83c..5a7d46618 100644 --- a/nebula_ros/schema/Pandar40P.schema.json +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -98,6 +98,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index c8eea129b..9f7e42083 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -98,6 +98,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json index 7ef9cd4e3..e0db59059 100644 --- a/nebula_ros/schema/PandarAT128.schema.json +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -118,6 +118,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json index 580e0d3ac..ccde699d4 100644 --- a/nebula_ros/schema/PandarQT128.schema.json +++ b/nebula_ros/schema/PandarQT128.schema.json @@ -101,6 +101,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json index 925f51967..2e39101c1 100644 --- a/nebula_ros/schema/PandarQT64.schema.json +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -98,6 +98,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ diff --git a/nebula_ros/schema/PandarXT32.schema.json b/nebula_ros/schema/PandarXT32.schema.json index d8c3e35ef..3d9662518 100644 --- a/nebula_ros/schema/PandarXT32.schema.json +++ b/nebula_ros/schema/PandarXT32.schema.json @@ -101,6 +101,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ diff --git a/nebula_ros/schema/PandarXT32M.schema.json b/nebula_ros/schema/PandarXT32M.schema.json index 9ad61e557..e1f63588b 100644 --- a/nebula_ros/schema/PandarXT32M.schema.json +++ b/nebula_ros/schema/PandarXT32M.schema.json @@ -101,6 +101,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ From 92a65c1a5a0cd0b76a3737b6980f7b5327e1e438 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 16 Jan 2025 12:29:59 +0900 Subject: [PATCH 05/12] chore(hesai): add FoV and resolution information to sensor definitions Signed-off-by: Max SCHMELLER --- .../nebula_decoders_hesai/decoders/pandar_128e3x.hpp | 2 ++ .../nebula_decoders_hesai/decoders/pandar_128e4x.hpp | 2 ++ .../nebula_decoders_hesai/decoders/pandar_40.hpp | 3 +++ .../nebula_decoders_hesai/decoders/pandar_64.hpp | 2 ++ .../nebula_decoders_hesai/decoders/pandar_at128.hpp | 2 ++ .../nebula_decoders_hesai/decoders/pandar_qt128.hpp | 2 ++ .../nebula_decoders_hesai/decoders/pandar_qt64.hpp | 2 ++ .../nebula_decoders_hesai/decoders/pandar_xt32.hpp | 2 ++ .../nebula_decoders_hesai/decoders/pandar_xt32m.hpp | 2 ++ 9 files changed, 19 insertions(+) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp index c6818f9f1..626bf2b23 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp @@ -220,6 +220,8 @@ class Pandar128E3X : public HesaiSensor static constexpr float min_range = 0.1; static constexpr float max_range = 230.0; static constexpr size_t max_scan_buffer_points = 691200; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-25'000, 14'400}}; + static constexpr AnglePair peak_resolution_mdeg{100, 125}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index aec5e4be1..39afab6c7 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -83,6 +83,8 @@ class Pandar128E4X : public HesaiSensor static constexpr float min_range = 0.1; static constexpr float max_range = 230.0; static constexpr size_t max_scan_buffer_points = 691200; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-24'800, 15'000}}; + static constexpr AnglePair peak_resolution_mdeg{100, 125}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp index 4fc108241..164879bbe 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp @@ -14,6 +14,7 @@ #pragma once +#include "nebula_decoders/nebula_decoders_common/angles.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" @@ -74,6 +75,8 @@ class Pandar40 : public HesaiSensor static constexpr float min_range = 0.3f; static constexpr float max_range = 200.f; static constexpr size_t max_scan_buffer_points = 144000; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-25'000, 15'000}}; + static constexpr AnglePair peak_resolution_mdeg{200, 334}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp index bd320064f..897797dc9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp @@ -54,6 +54,8 @@ class Pandar64 : public HesaiSensor static constexpr float min_range = 0.3f; static constexpr float max_range = 200.f; static constexpr size_t max_scan_buffer_points = 230400; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-25'000, 15'000}}; + static constexpr AnglePair peak_resolution_mdeg{200, 167}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp index dc3538b1b..b8a7f3215 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp @@ -80,6 +80,8 @@ class PandarAT128 static constexpr float min_range = 1.f; static constexpr float max_range = 180.0f; static constexpr size_t max_scan_buffer_points = 307200; + static constexpr FieldOfView fov_mdeg{{30'000, 150'000}, {-12'500, 12'900}}; + static constexpr AnglePair peak_resolution_mdeg{100, 200}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp index dd1544507..55c421e38 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp @@ -93,6 +93,8 @@ class PandarQT128 : public HesaiSensor static constexpr float min_range = 0.05; static constexpr float max_range = 50.0; static constexpr size_t max_scan_buffer_points = 172800; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-52'630, 52'630}}; + static constexpr AnglePair peak_resolution_mdeg{400, 100}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp index e828c05e8..900105869 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp @@ -63,6 +63,8 @@ class PandarQT64 : public HesaiSensor static constexpr float min_range = 0.1f; static constexpr float max_range = 60.f; static constexpr size_t max_scan_buffer_points = 76800; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-52'100, 52'100}}; + static constexpr AnglePair peak_resolution_mdeg{600, 1'450}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp index 690dc68e3..84fc80625 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp @@ -56,6 +56,8 @@ class PandarXT32 : public HesaiSensor static constexpr float min_range = 0.05f; static constexpr float max_range = 120.0f; static constexpr size_t max_scan_buffer_points = 256000; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-16'000, 15'000}}; + static constexpr AnglePair peak_resolution_mdeg{180, 1'000}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp index 24a31ca11..cc6a554fc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp @@ -46,6 +46,8 @@ class PandarXT32M : public HesaiSensor static constexpr float min_range = 0.5f; static constexpr float max_range = 300.0f; static constexpr size_t max_scan_buffer_points = 384000; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-20'800, 19'500}}; + static constexpr AnglePair peak_resolution_mdeg{180, 1'300}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override From e39afd74f0e15aea7da84f216a84d4a44b2ccd24 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 16 Jan 2025 12:31:24 +0900 Subject: [PATCH 06/12] feat(hesai): add downsample mask filter to decoder Signed-off-by: Max SCHMELLER --- nebula_decoders/CMakeLists.txt | 2 ++ .../decoders/hesai_decoder.hpp | 16 +++++++++++++++- .../decoders/pandar_128e3x.hpp | 4 ++-- .../decoders/pandar_128e4x.hpp | 4 ++-- .../nebula_decoders_hesai/decoders/pandar_40.hpp | 4 ++-- .../nebula_decoders_hesai/decoders/pandar_64.hpp | 4 ++-- .../decoders/pandar_at128.hpp | 5 +++-- .../decoders/pandar_qt128.hpp | 4 ++-- .../decoders/pandar_qt64.hpp | 4 ++-- .../decoders/pandar_xt16.hpp | 2 ++ .../decoders/pandar_xt32.hpp | 4 ++-- .../decoders/pandar_xt32m.hpp | 4 ++-- 12 files changed, 38 insertions(+), 19 deletions(-) diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index f1267763f..ccf812486 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -53,10 +53,12 @@ add_library(nebula_decoders_hesai SHARED ) target_link_libraries(nebula_decoders_hesai PUBLIC ${pandar_msgs_TARGETS} + ${PNG_LIBRARIES} ) target_include_directories(nebula_decoders_hesai PUBLIC ${pandar_msgs_INCLUDE_DIRS} + ${PNG_INCLUDE_DIRS} ) # Velodyne 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 88f5b66eb..2f63780ce 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 @@ -15,6 +15,7 @@ #pragma once #include "nebula_decoders/nebula_decoders_common/angles.hpp" +#include "nebula_decoders/nebula_decoders_common/point_filters/downsample_mask.hpp" #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" @@ -86,6 +87,8 @@ class HesaiDecoder : public HesaiScanDecoder std::array, SensorT::packet_t::max_returns> block_firing_offset_ns_; + std::optional mask_filter_; + /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully @@ -198,7 +201,7 @@ class HesaiDecoder : public HesaiScanDecoder uint64_t scan_timestamp_ns = in_current_scan ? decode_scan_timestamp_ns_ : output_scan_timestamp_ns_; - NebulaPoint & point = pc->emplace_back(); + NebulaPoint point; point.distance = distance; point.intensity = unit.reflectivity; point.time_stamp = get_point_time_relative( @@ -217,6 +220,10 @@ 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; + + if (!mask_filter_ || !mask_filter_->excluded(point)) { + pc->emplace_back(point); + } } } } @@ -263,6 +270,13 @@ class HesaiDecoder : public HesaiScanDecoder decode_pc_ = std::make_shared(); output_pc_ = std::make_shared(); + if (!sensor_configuration->downsample_mask_path.empty()) { + mask_filter_ = point_filters::DownsampleMaskFilter( + sensor_configuration->downsample_mask_path, SensorT::fov_mdeg.azimuth, + SensorT::peak_resolution_mdeg.azimuth, SensorT::packet_t::n_channels, + logger_->child("Downsample Mask"), true); + } + decode_pc_->reserve(SensorT::max_scan_buffer_points); output_pc_->reserve(SensorT::max_scan_buffer_points); } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp index 626bf2b23..042dabdb3 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp @@ -220,8 +220,8 @@ class Pandar128E3X : public HesaiSensor static constexpr float min_range = 0.1; static constexpr float max_range = 230.0; static constexpr size_t max_scan_buffer_points = 691200; - static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-25'000, 14'400}}; - static constexpr AnglePair peak_resolution_mdeg{100, 125}; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-25'000, 14'400}}; + static constexpr AnglePair peak_resolution_mdeg{100, 125}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 39afab6c7..d695ca74f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -83,8 +83,8 @@ class Pandar128E4X : public HesaiSensor static constexpr float min_range = 0.1; static constexpr float max_range = 230.0; static constexpr size_t max_scan_buffer_points = 691200; - static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-24'800, 15'000}}; - static constexpr AnglePair peak_resolution_mdeg{100, 125}; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-24'800, 14'400}}; + static constexpr AnglePair peak_resolution_mdeg{100, 125}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp index 164879bbe..5e931b956 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp @@ -75,8 +75,8 @@ class Pandar40 : public HesaiSensor static constexpr float min_range = 0.3f; static constexpr float max_range = 200.f; static constexpr size_t max_scan_buffer_points = 144000; - static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-25'000, 15'000}}; - static constexpr AnglePair peak_resolution_mdeg{200, 334}; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-25'000, 15'000}}; + static constexpr AnglePair peak_resolution_mdeg{200, 334}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp index 897797dc9..c9b2cb0f8 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp @@ -54,8 +54,8 @@ class Pandar64 : public HesaiSensor static constexpr float min_range = 0.3f; static constexpr float max_range = 200.f; static constexpr size_t max_scan_buffer_points = 230400; - static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-25'000, 15'000}}; - static constexpr AnglePair peak_resolution_mdeg{200, 167}; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-25'000, 15'000}}; + static constexpr AnglePair peak_resolution_mdeg{200, 167}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp index b8a7f3215..383423c2e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp @@ -80,8 +80,9 @@ class PandarAT128 static constexpr float min_range = 1.f; static constexpr float max_range = 180.0f; static constexpr size_t max_scan_buffer_points = 307200; - static constexpr FieldOfView fov_mdeg{{30'000, 150'000}, {-12'500, 12'900}}; - static constexpr AnglePair peak_resolution_mdeg{100, 200}; + static constexpr FieldOfView fov_mdeg{ + {30'000, 150'000}, {-12'500, 12'900}}; + static constexpr AnglePair peak_resolution_mdeg{100, 200}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp index 55c421e38..c1cf64b3e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp @@ -93,8 +93,8 @@ class PandarQT128 : public HesaiSensor static constexpr float min_range = 0.05; static constexpr float max_range = 50.0; static constexpr size_t max_scan_buffer_points = 172800; - static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-52'630, 52'630}}; - static constexpr AnglePair peak_resolution_mdeg{400, 100}; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-52'630, 52'630}}; + static constexpr AnglePair peak_resolution_mdeg{400, 100}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp index 900105869..ec8407d73 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp @@ -63,8 +63,8 @@ class PandarQT64 : public HesaiSensor static constexpr float min_range = 0.1f; static constexpr float max_range = 60.f; static constexpr size_t max_scan_buffer_points = 76800; - static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-52'100, 52'100}}; - static constexpr AnglePair peak_resolution_mdeg{600, 1'450}; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-52'100, 52'100}}; + static constexpr AnglePair peak_resolution_mdeg{600, 1'450}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt16.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt16.hpp index 60bfd898a..c524f3640 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt16.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt16.hpp @@ -49,6 +49,8 @@ class PandarXT16 : public HesaiSensor static constexpr float min_range = 0.05f; static constexpr float max_range = 120.0f; static constexpr size_t max_scan_buffer_points = 128000; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-15'000, 15'000}}; + static constexpr AnglePair peak_resolution_mdeg{180, 1'000}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp index 84fc80625..4e2ad87e7 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp @@ -56,8 +56,8 @@ class PandarXT32 : public HesaiSensor static constexpr float min_range = 0.05f; static constexpr float max_range = 120.0f; static constexpr size_t max_scan_buffer_points = 256000; - static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-16'000, 15'000}}; - static constexpr AnglePair peak_resolution_mdeg{180, 1'000}; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-16'000, 15'000}}; + static constexpr AnglePair peak_resolution_mdeg{180, 1'000}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp index cc6a554fc..3f4e61488 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp @@ -46,8 +46,8 @@ class PandarXT32M : public HesaiSensor static constexpr float min_range = 0.5f; static constexpr float max_range = 300.0f; static constexpr size_t max_scan_buffer_points = 384000; - static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-20'800, 19'500}}; - static constexpr AnglePair peak_resolution_mdeg{180, 1'300}; + static constexpr FieldOfView fov_mdeg{{0, 360'000}, {-20'800, 19'500}}; + static constexpr AnglePair peak_resolution_mdeg{180, 1'300}; int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override From 5fdca2a9c43050dae903378017754c96e600d9e3 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 16 Jan 2025 12:32:01 +0900 Subject: [PATCH 07/12] docs(point_filters): add compatibility table for hesai Signed-off-by: Max SCHMELLER --- docs/filters.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/filters.md b/docs/filters.md index 6381e3a94..a70c8c6f5 100644 --- a/docs/filters.md +++ b/docs/filters.md @@ -34,7 +34,7 @@ The following filter types are supported: | Filter Name | Filter Type | Hesai | Robosense | Velodyne | | ---------------------- | ----------------- | :---: | :-------: | :------: | -| Downsample Mask Filter | `downsample_mask` | ❌ | ❌ | ❌ | +| Downsample Mask Filter | `downsample_mask` | ✅ | ❌ | ❌ | Compatibility: ✅: compatible From 2ca33147d383503786ec668c96edef1710639cf9 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 18 Feb 2025 19:52:35 +0900 Subject: [PATCH 08/12] docs(downsample_mask): make warnings about color spaces more prominent and detailed Signed-off-by: Max SCHMELLER --- docs/filters.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/docs/filters.md b/docs/filters.md index a70c8c6f5..858b9656a 100644 --- a/docs/filters.md +++ b/docs/filters.md @@ -89,9 +89,16 @@ ros2 param set /_ros_wrapper_node point_filters.downsample_mask.path ""' #### Behavior + +!!! warning + Color spaces in image editors can be confusing and can lead to unexpected results (more/less downsampling than expected). + To work on monochrome images, use the `Grayscale` color mode and use the `Value` in the HSV color space to choose greyscale values. + For example, a `Value` (HSV) of 50% is equal to a downsampling factor of 2, but a `Luminosity` (LCh) of 50% is equal to a downsampling factor of 2.17. + Check the generated `_dithered.png` mask to see whether you are getting the expected results. + + - Greyscale values are quantized to the nearest 10th (yielding 11 quantization levels in total) - Mask resolution is dictated by the sensor's maximum FoV, the number of channels (for rotational LiDARs) and the peak angular resolution: - For a 40-channel LiDAR with `360 deg` FoV and `0.1 deg` peak azimuth resolution, the mask has to be `(360 / 0.1, 40) = (3600, 40)` pixels - Currently, non-rotational LiDARs are not yet supported -- Image editors like GIMP use perceptual color profiles, which can lead to unexpected results (more/less downsampling than expected). Check the generated `_dithered.png` mask to see if you are affected. - Dithering performed by Nebula is spatial only, meaning that it stays constant over time. Decoded points are checked against the nearest pixel in the dithered mask From d0415016d84c5370822f181a2d76d669673ceaf8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 18 Feb 2025 10:53:10 +0000 Subject: [PATCH 09/12] ci(pre-commit): autofix --- docs/filters.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/filters.md b/docs/filters.md index 858b9656a..6095d61ab 100644 --- a/docs/filters.md +++ b/docs/filters.md @@ -91,7 +91,7 @@ ros2 param set /_ros_wrapper_node point_filters.downsample_mask.path ""' !!! warning - Color spaces in image editors can be confusing and can lead to unexpected results (more/less downsampling than expected). + Color spaces in image editors can be confusing and can lead to unexpected results (more/less downsampling than expected). To work on monochrome images, use the `Grayscale` color mode and use the `Value` in the HSV color space to choose greyscale values. For example, a `Value` (HSV) of 50% is equal to a downsampling factor of 2, but a `Luminosity` (LCh) of 50% is equal to a downsampling factor of 2.17. Check the generated `_dithered.png` mask to see whether you are getting the expected results. From 4f72d634d541418eea1fc8039304922c92557f70 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 18 Feb 2025 20:03:06 +0900 Subject: [PATCH 10/12] chore(downsample_mask): change the config item to a std::optional Before, an empty string was used to signal a disabled downsample filter but this was not very readable. Signed-off-by: Max SCHMELLER --- .../nebula_common/hesai/hesai_common.hpp | 9 +++-- .../decoders/hesai_decoder.hpp | 4 +-- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 35 +++++++++++++------ 3 files changed, 33 insertions(+), 15 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 96a04a3af..0ba94c97a 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -50,7 +51,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase PtpTransportType ptp_transport_type; PtpSwitchType ptp_switch_type; uint8_t ptp_lock_threshold; - std::string downsample_mask_path; + std::optional downsample_mask_path; }; /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator) /// @param os @@ -61,7 +62,7 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con os << "Hesai Sensor Configuration:" << '\n'; os << (LidarConfigurationBase)(arg) << '\n'; os << "Multicast: " - << (arg.multicast_ip.empty() ? "disabled" : "enabled, group " + arg.multicast_ip) << '\n'; + << (arg.multicast_ip.empty() ? "disabled" : "enabled, group: " + arg.multicast_ip) << '\n'; os << "GNSS Port: " << arg.gnss_port << '\n'; os << "Rotation Speed: " << arg.rotation_speed << '\n'; os << "Sync Angle: " << arg.sync_angle << '\n'; @@ -75,7 +76,9 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con os << "PTP Transport Type: " << arg.ptp_transport_type << '\n'; os << "PTP Switch Type: " << arg.ptp_switch_type << '\n'; os << "PTP Lock Threshold: " << std::to_string(arg.ptp_lock_threshold) << '\n'; - os << "Downsample Mask Path: " << arg.downsample_mask_path; + os << "Downsample Filter: " + << (arg.downsample_mask_path ? "enabled, path: " + arg.downsample_mask_path.value() + : "disabled"); return os; } 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 2f63780ce..0f4ce3f5b 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 @@ -270,9 +270,9 @@ class HesaiDecoder : public HesaiScanDecoder decode_pc_ = std::make_shared(); output_pc_ = std::make_shared(); - if (!sensor_configuration->downsample_mask_path.empty()) { + if (sensor_configuration->downsample_mask_path) { mask_filter_ = point_filters::DownsampleMaskFilter( - sensor_configuration->downsample_mask_path, SensorT::fov_mdeg.azimuth, + sensor_configuration->downsample_mask_path.value(), SensorT::fov_mdeg.azimuth, SensorT::peak_resolution_mdeg.azimuth, SensorT::packet_t::n_channels, logger_->child("Downsample Mask"), true); } diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index ff82d934b..ee1ba275d 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -209,8 +209,15 @@ nebula::Status HesaiRosWrapper::declare_and_get_sensor_config_params() config.ptp_lock_threshold = declare_parameter("ptp_lock_threshold", descriptor); } - config.downsample_mask_path = - declare_parameter("point_filters.downsample_mask.path", "", param_read_write()); + { + auto downsample_mask_path = + declare_parameter("point_filters.downsample_mask.path", "", param_read_write()); + if (downsample_mask_path.empty()) { + config.downsample_mask_path = std::nullopt; + } else { + config.downsample_mask_path = downsample_mask_path; + } + } auto new_cfg_ptr = std::make_shared(config); return validate_and_set_config(new_cfg_ptr); @@ -272,10 +279,10 @@ Status HesaiRosWrapper::validate_and_set_config( } if ( - !new_config->downsample_mask_path.empty() && - !std::filesystem::exists(new_config->downsample_mask_path)) { + new_config->downsample_mask_path && + !std::filesystem::exists(new_config->downsample_mask_path.value())) { RCLCPP_ERROR_STREAM( - get_logger(), "Downsample mask not found: " << new_config->downsample_mask_path); + get_logger(), "Downsample mask not found: " << new_config->downsample_mask_path.value()); return Status::SENSOR_CONFIG_ERROR; } @@ -345,12 +352,13 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( drivers::HesaiSensorConfiguration new_cfg(*sensor_cfg_ptr_); - std::string _return_mode{}; + std::string return_mode{}; std::string calibration_parameter_name = get_calibration_parameter_name(sensor_cfg_ptr_->sensor_model); + std::string downsample_mask_path = new_cfg.downsample_mask_path.value_or(""); bool got_any = - get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | + get_param(p, "return_mode", return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | get_param(p, "sync_angle", new_cfg.sync_angle) | get_param(p, "cut_angle", new_cfg.cut_angle) | get_param(p, "min_range", new_cfg.min_range) | get_param(p, "max_range", new_cfg.max_range) | get_param(p, "rotation_speed", new_cfg.rotation_speed) | @@ -358,7 +366,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold) | get_param(p, calibration_parameter_name, new_cfg.calibration_path) | - get_param(p, "point_filters.downsample_mask.path", new_cfg.downsample_mask_path); + get_param(p, "point_filters.downsample_mask.path", downsample_mask_path); // Currently, all of the sub-wrappers read-only parameters, so they do not be queried for updates @@ -366,9 +374,16 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( return rcl_interfaces::build().successful(true).reason(""); } - if (_return_mode.length() > 0) + if (return_mode.empty()) { new_cfg.return_mode = - nebula::drivers::return_mode_from_string_hesai(_return_mode, sensor_cfg_ptr_->sensor_model); + nebula::drivers::return_mode_from_string_hesai(return_mode, sensor_cfg_ptr_->sensor_model); + } + + if (!downsample_mask_path.empty()) { + new_cfg.downsample_mask_path = downsample_mask_path; + } else { + new_cfg.downsample_mask_path = std::nullopt; + } // //////////////////////////////////////// // Get and validate new calibration, if any From 2de4297af1dce0606ae6b5421fdb6f1f43594bb2 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 19 Feb 2025 17:05:08 +0900 Subject: [PATCH 11/12] docs: fix rendering issues introduced by code changes Signed-off-by: Max SCHMELLER --- docs/requirements.txt | 2 +- mkdocs.yml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/requirements.txt b/docs/requirements.txt index 31cb03564..9984b1d04 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -5,7 +5,7 @@ markdown==3.3.7 mdx_truly_sane_lists mkdocs-awesome-pages-plugin==2.9.1 mkdocs-exclude -mkdocs-macros-plugin +mkdocs-macros-plugin>=1.2.0 mkdocs-same-dir mkdocs-static-i18n mkdocs-video diff --git a/mkdocs.yml b/mkdocs.yml index c12ef8e40..fa8a60208 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -167,6 +167,7 @@ plugins: force_render_paths: | * !*_source.md + !class*.md - mkdoxy: projects: nebula_common: From 2e8929bb584e363e303e6ce2bad23be018cb379f Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 19 Feb 2025 17:05:42 +0900 Subject: [PATCH 12/12] docs: add a list of downsample mask resolution requirements for Hesai sensor models Signed-off-by: Max SCHMELLER --- docs/filters.md | 19 +++++++++++++++++++ nebula_ros/schema/sub/misc.json | 1 + 2 files changed, 20 insertions(+) diff --git a/docs/filters.md b/docs/filters.md index 6095d61ab..2d56f8ad7 100644 --- a/docs/filters.md +++ b/docs/filters.md @@ -87,6 +87,25 @@ The filter can be disabled by omitting the `downsample_mask` config item, or by ros2 param set /_ros_wrapper_node point_filters.downsample_mask.path ""' ``` +The required resolution of the mask is sensor-dependent: + +| Sensor Model | Required Resolution | +| ------------ | ------------------- | +| Pandar40P | 1800 x 40 | +| Pandar64 | 1800 x 64 | +| PandarXT16 | 2000 x 16 | +| PandarXT32 | 2000 x 32 | +| PandarXT32M | 2000 x 32 | +| PandarAT128 | 1200 x 128 | +| PandarQT64 | 600 x 64 | +| PandarQT128 | 900 x 128 | +| Pandar128E4X | 3600 x 128 | + + +!!! warning + If the mask resolution does not match the required resolution, Nebula will not start (if configured on launch) or reject the setting (if set during runtime). + + #### Behavior diff --git a/nebula_ros/schema/sub/misc.json b/nebula_ros/schema/sub/misc.json index 0cfcea4ef..216d648c2 100644 --- a/nebula_ros/schema/sub/misc.json +++ b/nebula_ros/schema/sub/misc.json @@ -96,6 +96,7 @@ "properties": { "path": { "title": "Path to a grayscale PNG mask (0: filter all, 128: keep 50%, 255: keep all)", + "description": "See [Filters](/filters/) for more information.", "type": "string", "default": "\"\"" }