From 20049d1903da15d21dae1015409f3094b871c002 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 1 Feb 2024 16:28:07 +0900 Subject: [PATCH] refactor(ndt_scan_matcher): hierarchize parameters (#6208) * refactor(ndt_scan_matcher): hierarchize parameters Signed-off-by: Yamato Ando * style(pre-commit): autofix * add new lines Signed-off-by: Yamato Ando * fix typo Signed-off-by: Yamato Ando --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/ndt_scan_matcher.param.yaml | 158 +++++++------- .../ndt_scan_matcher/hyper_parameters.hpp | 173 +++++++++++++++ .../ndt_scan_matcher/map_update_module.hpp | 9 +- .../ndt_scan_matcher_core.hpp | 31 +-- .../src/map_update_module.cpp | 14 +- .../src/ndt_scan_matcher_core.cpp | 197 ++++++------------ 6 files changed, 334 insertions(+), 248 deletions(-) create mode 100644 localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index d4c49b7e8eec5..144449ce75084 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,102 +1,114 @@ /**: ros__parameters: - # Vehicle reference frame - base_frame: "base_link" + frame: + # Vehicle reference frame + base_frame: "base_link" - # NDT reference frame - ndt_base_frame: "ndt_base_link" + # NDT reference frame + ndt_base_frame: "ndt_base_link" - # map frame - map_frame: "map" + # Map frame + map_frame: "map" - # Subscriber queue size - input_sensor_points_queue_size: 1 - # The maximum difference between two consecutive - # transformations in order to consider convergence - trans_epsilon: 0.01 + ndt: + # The maximum difference between two consecutive + # transformations in order to consider convergence + trans_epsilon: 0.01 - # The newton line search maximum step length - step_size: 0.1 + # The newton line search maximum step length + step_size: 0.1 - # The ND voxel grid resolution - resolution: 2.0 + # The ND voxel grid resolution + resolution: 2.0 - # The number of iterations required to calculate alignment - max_iterations: 30 + # The number of iterations required to calculate alignment + max_iterations: 30 - # Converged param type - # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD - converged_param_type: 1 + # Number of threads used for parallel computing + num_threads: 4 - # If converged_param_type is 0 - # Threshold for deciding whether to trust the estimation result - converged_param_transform_probability: 3.0 + regularization: + enable: false - # If converged_param_type is 1 - # Threshold for deciding whether to trust the estimation result - converged_param_nearest_voxel_transformation_likelihood: 2.3 + # Regularization scale factor + scale_factor: 0.01 - # The number of particles to estimate initial pose - initial_estimate_particles_num: 200 - # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). - # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. - # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. - n_startup_trials: 20 + initial_pose_estimation: + # The number of particles to estimate initial pose + particles_num: 200 - # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] - lidar_topic_timeout_sec: 1.0 + # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). + # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. + # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. + n_startup_trials: 20 - # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] - initial_pose_timeout_sec: 1.0 - # Tolerance of distance difference between two initial poses used for linear interpolation. [m] - initial_pose_distance_tolerance_m: 10.0 + validation: + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + lidar_topic_timeout_sec: 1.0 - # The execution time which means probably NDT cannot matches scans properly. [ms] - critical_upper_bound_exe_time_ms: 100.0 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 - # Number of threads used for parallel computing - num_threads: 4 + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 - # The covariance of output pose - # Note that this covariance matrix is empirically derived - output_pose_covariance: - [ - 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, - ] + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100.0 - # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) - use_covariance_estimation: false - # Offset arrangement in covariance estimation [m] - # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. - initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] - initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + score_estimation: + # Converged param type + # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD + converged_param_type: 1 - # Regularization switch - regularization_enabled: false + # If converged_param_type is 0 + # Threshold for deciding whether to trust the estimation result + converged_param_transform_probability: 3.0 - # Regularization scale factor - regularization_scale_factor: 0.01 + # If converged_param_type is 1 + # Threshold for deciding whether to trust the estimation result + converged_param_nearest_voxel_transformation_likelihood: 2.3 - # Dynamic map loading distance - dynamic_map_loading_update_distance: 20.0 + # Scan matching score based on no ground LiDAR scan + no_ground_points: + enable: false - # Dynamic map loading loading radius - dynamic_map_loading_map_radius: 150.0 + # If lidar_point.z - base_link.z <= this threshold , the point will be removed + z_margin_for_ground_removal: 0.8 - # Radius of input LiDAR range (used for diagnostics of dynamic map loading) - lidar_radius: 100.0 - # A flag for using scan matching score based on no ground LiDAR scan - estimate_scores_by_no_ground_points: false + covariance: + # The covariance of output pose + # Note that this covariance matrix is empirically derived + output_pose_covariance: + [ + 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, + ] - # If lidar_point.z - base_link.z <= this threshold , the point will be removed - z_margin_for_ground_removal: 0.8 + # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) + covariance_estimation: + enable: false + + # Offset arrangement in covariance estimation [m] + # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. + initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] + initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + + + dynamic_map_loading: + # Dynamic map loading distance + update_distance: 20.0 + + # Dynamic map loading loading radius + map_radius: 150.0 + + # Radius of input LiDAR range (used for diagnostics of dynamic map loading) + lidar_radius: 100.0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp new file mode 100644 index 0000000000000..2955e3cb9a5f7 --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -0,0 +1,173 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use node file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#define NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ + +#include + +#include + +#include +#include +#include + +enum class ConvergedParamType { + TRANSFORM_PROBABILITY = 0, + NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 +}; + +struct HyperParameters +{ + struct Frame + { + std::string base_frame; + std::string ndt_base_frame; + std::string map_frame; + } frame; + + pclomp::NdtParams ndt; + bool ndt_regularization_enable; + + struct InitialPoseEstimation + { + int64_t particles_num; + int64_t n_startup_trials; + } initial_pose_estimation; + + struct Validation + { + double lidar_topic_timeout_sec; + double initial_pose_timeout_sec; + double initial_pose_distance_tolerance_m; + double critical_upper_bound_exe_time_ms; + } validation; + + struct ScoreEstimation + { + ConvergedParamType converged_param_type; + double converged_param_transform_probability; + double converged_param_nearest_voxel_transformation_likelihood; + struct NoGroundPoints + { + bool enable; + double z_margin_for_ground_removal; + } no_ground_points; + } score_estimation; + + struct Covariance + { + std::array output_pose_covariance; + + struct CovarianceEstimation + { + bool enable; + std::vector initial_pose_offset_model; + } covariance_estimation; + } covariance; + + struct DynamicMapLoading + { + double update_distance; + double map_radius; + double lidar_radius; + } dynamic_map_loading; + +public: + explicit HyperParameters(rclcpp::Node * node) + { + frame.base_frame = node->declare_parameter("frame.base_frame"); + frame.ndt_base_frame = node->declare_parameter("frame.ndt_base_frame"); + frame.map_frame = node->declare_parameter("frame.map_frame"); + + ndt.trans_epsilon = node->declare_parameter("ndt.trans_epsilon"); + ndt.step_size = node->declare_parameter("ndt.step_size"); + ndt.resolution = node->declare_parameter("ndt.resolution"); + ndt.max_iterations = static_cast(node->declare_parameter("ndt.max_iterations")); + ndt.num_threads = static_cast(node->declare_parameter("ndt.num_threads")); + ndt.num_threads = std::max(ndt.num_threads, 1); + ndt_regularization_enable = node->declare_parameter("ndt.regularization.enable"); + ndt.regularization_scale_factor = + static_cast(node->declare_parameter("ndt.regularization.scale_factor")); + + initial_pose_estimation.particles_num = + node->declare_parameter("initial_pose_estimation.particles_num"); + initial_pose_estimation.n_startup_trials = + node->declare_parameter("initial_pose_estimation.n_startup_trials"); + + validation.lidar_topic_timeout_sec = + node->declare_parameter("validation.lidar_topic_timeout_sec"); + validation.initial_pose_timeout_sec = + node->declare_parameter("validation.initial_pose_timeout_sec"); + validation.initial_pose_distance_tolerance_m = + node->declare_parameter("validation.initial_pose_distance_tolerance_m"); + validation.critical_upper_bound_exe_time_ms = + node->declare_parameter("validation.critical_upper_bound_exe_time_ms"); + + const int64_t converged_param_type_tmp = + node->declare_parameter("score_estimation.converged_param_type"); + score_estimation.converged_param_type = + static_cast(converged_param_type_tmp); + score_estimation.converged_param_transform_probability = + node->declare_parameter("score_estimation.converged_param_transform_probability"); + score_estimation.converged_param_nearest_voxel_transformation_likelihood = + node->declare_parameter( + "score_estimation.converged_param_nearest_voxel_transformation_likelihood"); + score_estimation.no_ground_points.enable = + node->declare_parameter("score_estimation.no_ground_points.enable"); + score_estimation.no_ground_points.z_margin_for_ground_removal = node->declare_parameter( + "score_estimation.no_ground_points.z_margin_for_ground_removal"); + + std::vector output_pose_covariance = + node->declare_parameter>("covariance.output_pose_covariance"); + for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { + covariance.output_pose_covariance[i] = output_pose_covariance[i]; + } + covariance.covariance_estimation.enable = + node->declare_parameter("covariance.covariance_estimation.enable"); + if (covariance.covariance_estimation.enable) { + std::vector initial_pose_offset_model_x = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_x"); + std::vector initial_pose_offset_model_y = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_y"); + + if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { + const size_t size = initial_pose_offset_model_x.size(); + covariance.covariance_estimation.initial_pose_offset_model.resize(size); + for (size_t i = 0; i < size; i++) { + covariance.covariance_estimation.initial_pose_offset_model[i].x() = + initial_pose_offset_model_x[i]; + covariance.covariance_estimation.initial_pose_offset_model[i].y() = + initial_pose_offset_model_y[i]; + } + } else { + RCLCPP_WARN( + node->get_logger(), + "Invalid initial pose offset model parameters. Disable covariance estimation."); + covariance.covariance_estimation.enable = false; + } + } + + dynamic_map_loading.update_distance = + node->declare_parameter("dynamic_map_loading.update_distance"); + dynamic_map_loading.map_radius = + node->declare_parameter("dynamic_map_loading.map_radius"); + dynamic_map_loading.lidar_radius = + node->declare_parameter("dynamic_map_loading.lidar_radius"); + } +}; + +#endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 8b192b0186b42..157421fc3ccb1 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -16,6 +16,7 @@ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #include "localization_util/util_func.hpp" +#include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/particle.hpp" #include @@ -47,7 +48,8 @@ class MapUpdateModule public: MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr); + std::shared_ptr ndt_ptr, + HyperParameters::DynamicMapLoading param); private: friend class NDTScanMatcher; @@ -70,9 +72,8 @@ class MapUpdateModule rclcpp::Clock::SharedPtr clock_; std::optional last_update_position_ = std::nullopt; - const double dynamic_map_loading_update_distance_; - const double dynamic_map_loading_map_radius_; - const double lidar_radius_; + + HyperParameters::DynamicMapLoading param_; }; #endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 2883aa761444d..ca69576061e21 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -18,6 +18,7 @@ #define FMT_HEADER_ONLY #include "localization_util/smart_pose_buffer.hpp" +#include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/map_update_module.hpp" #include @@ -65,11 +66,6 @@ #include #include -enum class ConvergedParamType { - TRANSFORM_PROBABILITY = 0, - NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 -}; - class NDTScanMatcher : public rclcpp::Node { using PointSource = pcl::PointXYZ; @@ -187,44 +183,21 @@ class NDTScanMatcher : public rclcpp::Node std::shared_ptr> state_ptr_; Eigen::Matrix4f base_to_sensor_matrix_; - std::string base_frame_; - std::string ndt_base_frame_; - std::string map_frame_; - - ConvergedParamType converged_param_type_; - double converged_param_transform_probability_; - double converged_param_nearest_voxel_transformation_likelihood_; - - int64_t initial_estimate_particles_num_; - int64_t n_startup_trials_; - double lidar_topic_timeout_sec_; - double initial_pose_timeout_sec_; - double initial_pose_distance_tolerance_m_; - bool use_cov_estimation_; - std::vector initial_pose_offset_model_; - std::array output_pose_covariance_; std::mutex ndt_ptr_mtx_; std::unique_ptr initial_pose_buffer_; // Keep latest position for dynamic map loading - // This variable is only used when use_dynamic_map_loading is true std::mutex latest_ekf_position_mtx_; std::optional latest_ekf_position_ = std::nullopt; - // variables for regularization - const bool regularization_enabled_; // whether to use longitudinal regularization std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; - bool estimate_scores_by_no_ground_points_; - double z_margin_for_ground_removal_; - - // The execution time which means probably NDT cannot matches scans properly - double critical_upper_bound_exe_time_ms_; + HyperParameters param_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 39b92fe248660..402ec9da32782 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -16,16 +16,12 @@ MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr) + std::shared_ptr ndt_ptr, HyperParameters::DynamicMapLoading param) : ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex), logger_(node->get_logger()), clock_(node->get_clock()), - dynamic_map_loading_update_distance_( - node->declare_parameter("dynamic_map_loading_update_distance")), - dynamic_map_loading_map_radius_( - node->declare_parameter("dynamic_map_loading_map_radius")), - lidar_radius_(node->declare_parameter("lidar_radius")) + param_(param) { loaded_pcd_pub_ = node->create_publisher( "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); @@ -42,10 +38,10 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi const double dx = position.x - last_update_position_.value().x; const double dy = position.y - last_update_position_.value().y; const double distance = std::hypot(dx, dy); - if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { + if (distance + param_.lidar_radius > param_.map_radius) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Dynamic map loading is not keeping up."); } - return distance > dynamic_map_loading_update_distance_; + return distance > param_.update_distance; } void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) @@ -53,7 +49,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) auto request = std::make_shared(); request->area.center_x = static_cast(position.x); request->area.center_y = static_cast(position.y); - request->area.radius = static_cast(dynamic_map_loading_map_radius_); + request->area.radius = static_cast(param_.map_radius); request->cached_ids = ndt_ptr_->getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 0278a62341981..cd637791f04b6 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -71,99 +71,11 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), - output_pose_covariance_({}), - regularization_enabled_(declare_parameter("regularization_enabled")), - is_activated_(false) + is_activated_(false), + param_(this) { (*state_ptr_)["state"] = "Initializing"; - int64_t points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); - points_queue_size = std::max(points_queue_size, (int64_t)0); - RCLCPP_INFO(get_logger(), "points_queue_size: %ld", points_queue_size); - - base_frame_ = this->declare_parameter("base_frame"); - RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str()); - - ndt_base_frame_ = this->declare_parameter("ndt_base_frame"); - RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str()); - - map_frame_ = this->declare_parameter("map_frame"); - RCLCPP_INFO(get_logger(), "map_frame_id: %s", map_frame_.c_str()); - - pclomp::NdtParams ndt_params{}; - ndt_params.trans_epsilon = this->declare_parameter("trans_epsilon"); - ndt_params.step_size = this->declare_parameter("step_size"); - ndt_params.resolution = this->declare_parameter("resolution"); - ndt_params.max_iterations = static_cast(this->declare_parameter("max_iterations")); - ndt_params.num_threads = static_cast(this->declare_parameter("num_threads")); - ndt_params.num_threads = std::max(ndt_params.num_threads, 1); - ndt_params.regularization_scale_factor = - static_cast(this->declare_parameter("regularization_scale_factor")); - ndt_ptr_->setParams(ndt_params); - - RCLCPP_INFO( - get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", - ndt_params.trans_epsilon, ndt_params.step_size, ndt_params.resolution, - ndt_params.max_iterations); - - const int64_t converged_param_type_tmp = this->declare_parameter("converged_param_type"); - converged_param_type_ = static_cast(converged_param_type_tmp); - - converged_param_transform_probability_ = - this->declare_parameter("converged_param_transform_probability"); - converged_param_nearest_voxel_transformation_likelihood_ = - this->declare_parameter("converged_param_nearest_voxel_transformation_likelihood"); - - lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); - - critical_upper_bound_exe_time_ms_ = - this->declare_parameter("critical_upper_bound_exe_time_ms"); - - initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); - - initial_pose_distance_tolerance_m_ = - this->declare_parameter("initial_pose_distance_tolerance_m"); - - initial_pose_buffer_ = std::make_unique( - this->get_logger(), initial_pose_timeout_sec_, initial_pose_distance_tolerance_m_); - - use_cov_estimation_ = this->declare_parameter("use_covariance_estimation"); - if (use_cov_estimation_) { - std::vector initial_pose_offset_model_x = - this->declare_parameter>("initial_pose_offset_model_x"); - std::vector initial_pose_offset_model_y = - this->declare_parameter>("initial_pose_offset_model_y"); - - if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { - const size_t size = initial_pose_offset_model_x.size(); - initial_pose_offset_model_.resize(size); - for (size_t i = 0; i < size; i++) { - initial_pose_offset_model_[i].x() = initial_pose_offset_model_x[i]; - initial_pose_offset_model_[i].y() = initial_pose_offset_model_y[i]; - } - } else { - RCLCPP_WARN( - get_logger(), - "Invalid initial pose offset model parameters. Disable covariance estimation."); - use_cov_estimation_ = false; - } - } - - std::vector output_pose_covariance = - this->declare_parameter>("output_pose_covariance"); - for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { - output_pose_covariance_[i] = output_pose_covariance[i]; - } - - initial_estimate_particles_num_ = - this->declare_parameter("initial_estimate_particles_num"); - n_startup_trials_ = this->declare_parameter("n_startup_trials"); - - estimate_scores_by_no_ground_points_ = - this->declare_parameter("estimate_scores_by_no_ground_points"); - - z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); - timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -186,12 +98,12 @@ NDTScanMatcher::NDTScanMatcher() std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1), initial_pose_sub_opt); sensor_points_sub_ = this->create_subscription( - "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), + "points_raw", rclcpp::SensorDataQoS().keep_last(1), std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), sensor_sub_opt); // Only if regularization is enabled, subscribe to the regularization base pose - if (regularization_enabled_) { + if (param_.ndt_regularization_enable) { // NOTE: The reason that the regularization subscriber does not belong to the // sensor_callback_group is to ensure that the regularization callback is called even if // sensor_callback takes long time to process. @@ -263,7 +175,14 @@ NDTScanMatcher::NDTScanMatcher() &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); + ndt_ptr_->setParams(param_.ndt); + + initial_pose_buffer_ = std::make_unique( + this->get_logger(), param_.validation.initial_pose_timeout_sec, + param_.validation.initial_pose_distance_tolerance_m); + + map_update_module_ = + std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); logger_configure_ = std::make_unique(this); } @@ -289,7 +208,8 @@ void NDTScanMatcher::publish_diagnostic() } if ( state_ptr_->count("lidar_topic_delay_time_sec") && - std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) { + std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > + param_.validation.lidar_topic_timeout_sec) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; } @@ -309,13 +229,13 @@ void NDTScanMatcher::publish_diagnostic() if ( state_ptr_->count("nearest_voxel_transformation_likelihood") && std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) < - converged_param_nearest_voxel_transformation_likelihood_) { + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT score is unreliably low. "; } if ( - state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { + state_ptr_->count("execution_time") && std::stod((*state_ptr_)["execution_time"]) >= + param_.validation.critical_upper_bound_exe_time_ms) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; @@ -360,13 +280,13 @@ void NDTScanMatcher::callback_initial_pose( { if (!is_activated_) return; - if (initial_pose_msg_ptr->header.frame_id == map_frame_) { + if (initial_pose_msg_ptr->header.frame_id == param_.frame.map_frame) { initial_pose_buffer_->push_back(initial_pose_msg_ptr); } else { RCLCPP_ERROR_STREAM_THROTTLE( get_logger(), *this->get_clock(), 1000, "Received initial pose message with frame_id " - << initial_pose_msg_ptr->header.frame_id << ", but expected " << map_frame_ + << initial_pose_msg_ptr->header.frame_id << ", but expected " << param_.frame.map_frame << ". Please check the frame_id in the input topic and ensure it is correct."); } @@ -395,12 +315,12 @@ void NDTScanMatcher::callback_sensor_points( const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds(); (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec); - if (lidar_topic_delay_time_sec > lidar_topic_timeout_sec_) { + if (lidar_topic_delay_time_sec > param_.validation.lidar_topic_timeout_sec) { RCLCPP_WARN( this->get_logger(), "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is " "%lf[sec])", - lidar_topic_delay_time_sec, lidar_topic_timeout_sec_); + lidar_topic_delay_time_sec, param_.validation.lidar_topic_timeout_sec); // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, // even if further processing continues, the estimated result will be rejected by ekf_localizer. @@ -424,7 +344,8 @@ void NDTScanMatcher::callback_sensor_points( pcl::fromROSMsg(*sensor_points_msg_in_sensor_frame, *sensor_points_in_sensor_frame); transform_sensor_measurement( - sensor_frame, base_frame_, sensor_points_in_sensor_frame, sensor_points_in_baselink_frame); + sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, + sensor_points_in_baselink_frame); ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); if (!is_activated_) return; @@ -440,7 +361,7 @@ void NDTScanMatcher::callback_sensor_points( interpolation_result_opt.value(); // if regularization is enabled and available, set pose to NDT for regularization - if (regularization_enabled_) { + if (param_.ndt_regularization_enable) { add_regularization_pose(sensor_ros_time); } @@ -491,9 +412,9 @@ void NDTScanMatcher::callback_sensor_points( map_to_base_link_quat.normalized().toRotationMatrix(); std::array ndt_covariance = - rotate_covariance(output_pose_covariance_, map_to_base_link_rotation); + rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation); - if (is_converged && use_cov_estimation_) { + if (is_converged && param_.covariance.covariance_estimation.enable) { const auto estimated_covariance = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); ndt_covariance = estimated_covariance; @@ -523,16 +444,18 @@ void NDTScanMatcher::callback_sensor_points( new pcl::PointCloud); tier4_autoware_utils::transformPointCloud( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); - publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_in_map_ptr); + publish_point_cloud(sensor_ros_time, param_.frame.map_frame, sensor_points_in_map_ptr); // whether use no ground points to calculate score - if (estimate_scores_by_no_ground_points_) { + if (param_.score_estimation.no_ground_points.enable) { // remove ground pcl::shared_ptr> no_ground_points_in_map_ptr( new pcl::PointCloud); for (std::size_t i = 0; i < sensor_points_in_map_ptr->size(); i++) { const float point_z = sensor_points_in_map_ptr->points[i].z; // NOLINT - if (point_z - matrix4f_to_pose(ndt_result.pose).position.z > z_margin_for_ground_removal_) { + if ( + point_z - matrix4f_to_pose(ndt_result.pose).position.z > + param_.score_estimation.no_ground_points.z_margin_for_ground_removal) { no_ground_points_in_map_ptr->points.push_back(sensor_points_in_map_ptr->points[i]); } } @@ -540,7 +463,7 @@ void NDTScanMatcher::callback_sensor_points( sensor_msgs::msg::PointCloud2 no_ground_points_msg_in_map; pcl::toROSMsg(*no_ground_points_in_map_ptr, no_ground_points_msg_in_map); no_ground_points_msg_in_map.header.stamp = sensor_ros_time; - no_ground_points_msg_in_map.header.frame_id = map_frame_; + no_ground_points_msg_in_map.header.frame_id = param_.frame.map_frame; no_ground_points_aligned_pose_pub_->publish(no_ground_points_msg_in_map); // calculate score const auto no_ground_transform_probability = static_cast( @@ -603,10 +526,10 @@ void NDTScanMatcher::publish_tf( { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; - result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; tf2_broadcaster_.sendTransform( - tier4_autoware_utils::pose2transform(result_pose_stamped_msg, ndt_base_frame_)); + tier4_autoware_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); } void NDTScanMatcher::publish_pose( @@ -615,12 +538,12 @@ void NDTScanMatcher::publish_pose( { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; - result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = sensor_ros_time; - result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = result_pose_msg; result_pose_with_cov_msg.pose.covariance = ndt_covariance; @@ -647,7 +570,7 @@ void NDTScanMatcher::publish_marker( visualization_msgs::msg::MarkerArray marker_array; visualization_msgs::msg::Marker marker; marker.header.stamp = sensor_ros_time; - marker.header.frame_id = map_frame_; + marker.header.frame_id = param_.frame.map_frame; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); @@ -681,7 +604,7 @@ void NDTScanMatcher::publish_initial_to_result( initial_to_result_relative_pose_stamped.pose = tier4_autoware_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; - initial_to_result_relative_pose_stamped.header.frame_id = map_frame_; + initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); const auto initial_to_result_distance = @@ -729,13 +652,16 @@ bool NDTScanMatcher::validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood) { bool is_ok_converged_param = false; - if (converged_param_type_ == ConvergedParamType::TRANSFORM_PROBABILITY) { + if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { is_ok_converged_param = validate_score( - transform_probability, converged_param_transform_probability_, "Transform Probability"); - } else if (converged_param_type_ == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { + transform_probability, param_.score_estimation.converged_param_transform_probability, + "Transform Probability"); + } else if ( + param_.score_estimation.converged_param_type == + ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { is_ok_converged_param = validate_score( nearest_voxel_transformation_likelihood, - converged_param_nearest_voxel_transformation_likelihood_, + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood, "Nearest Voxel Transformation Likelihood"); } else { is_ok_converged_param = false; @@ -804,11 +730,12 @@ std::array NDTScanMatcher::estimate_covariance( ndt_result.hessian.inverse().block(0, 0, 2, 2)); } catch (const std::exception & e) { RCLCPP_WARN(get_logger(), "Error in Eigen solver: %s", e.what()); - return output_pose_covariance_; + return param_.covariance.output_pose_covariance; } // first result is added to mean - const int n = static_cast(initial_pose_offset_model_.size()) + 1; + const int n = + static_cast(param_.covariance.covariance_estimation.initial_pose_offset_model.size()) + 1; const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); Eigen::Vector2d mean = ndt_pose_2d; std::vector ndt_pose_2d_vec; @@ -818,14 +745,15 @@ std::array NDTScanMatcher::estimate_covariance( geometry_msgs::msg::PoseArray multi_ndt_result_msg; geometry_msgs::msg::PoseArray multi_initial_pose_msg; multi_ndt_result_msg.header.stamp = sensor_ros_time; - multi_ndt_result_msg.header.frame_id = map_frame_; + multi_ndt_result_msg.header.frame_id = param_.frame.map_frame; multi_initial_pose_msg.header.stamp = sensor_ros_time; - multi_initial_pose_msg.header.frame_id = map_frame_; + multi_initial_pose_msg.header.frame_id = param_.frame.map_frame; multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(ndt_result.pose)); multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); // multiple searches - for (const auto & pose_offset : initial_pose_offset_model_) { + for (const auto & pose_offset : + param_.covariance.covariance_estimation.initial_pose_offset_model) { const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); @@ -854,7 +782,7 @@ std::array NDTScanMatcher::estimate_covariance( } pca_covariance /= (n - 1); // unbiased covariance - std::array ndt_covariance = output_pose_covariance_; + std::array ndt_covariance = param_.covariance.output_pose_covariance; ndt_covariance[0 + 6 * 0] += pca_covariance(0, 0); ndt_covariance[1 + 6 * 0] += pca_covariance(1, 0); ndt_covariance[0 + 6 * 1] += pca_covariance(0, 1); @@ -898,7 +826,7 @@ void NDTScanMatcher::service_ndt_align( tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { // get TF from pose_frame to map_frame - const std::string & target_frame = map_frame_; + const std::string & target_frame = param_.frame.map_frame; const std::string & source_frame = req->pose_with_covariance.header.frame_id; geometry_msgs::msg::TransformStamped transform_s2t; @@ -982,7 +910,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // the ego vehicle is aligned with the ground to some extent about roll and pitch. const std::vector is_loop_variable = {false, false, false, false, false, true}; TreeStructuredParzenEstimator tpe( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials_, is_loop_variable); + TreeStructuredParzenEstimator::Direction::MAXIMIZE, + param_.initial_pose_estimation.n_startup_trials, is_loop_variable); std::vector particle_array; auto output_cloud = std::make_shared>(); @@ -990,9 +919,9 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // publish the estimated poses in 20 times to see the progress and to avoid dropping data visualization_msgs::msg::MarkerArray marker_array; constexpr int64_t publish_num = 20; - const int64_t publish_interval = initial_estimate_particles_num_ / publish_num; + const int64_t publish_interval = param_.initial_pose_estimation.particles_num / publish_num; - for (int64_t i = 0; i < initial_estimate_particles_num_; i++) { + for (int64_t i = 0; i < param_.initial_pose_estimation.particles_num; i++) { const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); geometry_msgs::msg::Pose initial_pose; @@ -1018,8 +947,9 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, ndt_result.iteration_num); particle_array.push_back(particle); - push_debug_markers(marker_array, get_clock()->now(), map_frame_, particle, i); - if ((i + 1) % publish_interval == 0 || (i + 1) == initial_estimate_particles_num_) { + push_debug_markers(marker_array, get_clock()->now(), param_.frame.map_frame, particle, i); + if ( + (i + 1) % publish_interval == 0 || (i + 1) == param_.initial_pose_estimation.particles_num) { ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); marker_array.markers.clear(); } @@ -1048,7 +978,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( auto sensor_points_in_map_ptr = std::make_shared>(); tier4_autoware_utils::transformPointCloud( *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); - publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_in_map_ptr); + publish_point_cloud( + initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); } auto best_particle_ptr = std::max_element( @@ -1057,7 +988,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; - result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg);