From 1bab84e75e6a6b750b5781212b04702b7646616f Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 3 Mar 2022 04:19:34 +0900 Subject: [PATCH] feat(ndt_scan_matcher): add tolerance of initial pose (#408) * feat(ndt_scan_matcher): add tolerance of initial pose Signed-off-by: Yamato Ando * move codes Signed-off-by: YamatoAndo * modify the default value Signed-off-by: YamatoAndo * change the variable names Signed-off-by: YamatoAndo * ci(pre-commit): autofix * fix typo Signed-off-by: YamatoAndo * add depend fmt Signed-off-by: YamatoAndo * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/ndt_scan_matcher.param.yaml | 6 ++ .../config/ndt_scan_matcher.param.yaml | 6 ++ .../ndt_scan_matcher_core.hpp | 9 +++ localization/ndt_scan_matcher/package.xml | 1 + .../src/ndt_scan_matcher_core.cpp | 60 ++++++++++++++++++- 5 files changed, 81 insertions(+), 1 deletion(-) diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml index ae5a39dd99ceb..fd21436b3909b 100644 --- a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml +++ b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml @@ -29,6 +29,12 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 100 + # 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 + # neighborhood search method in OMP # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 omp_neighborhood_search_method: 0 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 9b044ec409934..7b62785f7f11a 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -30,6 +30,12 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 100 + # 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 + # neighborhood search method in OMP # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 omp_neighborhood_search_method: 0 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 f4407819a0ae0..a74c5b47b51e7 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 @@ -113,6 +113,13 @@ class NDTScanMatcher : public rclcpp::Node const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr); + bool validateTimeStampDifference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec); + bool validatePositionDifference( + const geometry_msgs::msg::Point & target_point, + const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_); + void timerDiagnostic(); rclcpp::Subscription::SharedPtr initial_pose_sub_; @@ -154,6 +161,8 @@ class NDTScanMatcher : public rclcpp::Node std::string map_frame_; double converged_param_transform_probability_; int initial_estimate_particles_num_; + double initial_pose_timeout_sec_; + double initial_pose_distance_tolerance_m_; float inversion_vector_threshold_; float oscillation_threshold_; std::array output_pose_covariance_; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index c83659f337958..9b6bb71d60426 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -8,6 +8,7 @@ ament_cmake_auto diagnostic_msgs + fmt geometry_msgs nav_msgs ndt 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 855b7c9750623..dca714376599a 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -104,6 +104,8 @@ NDTScanMatcher::NDTScanMatcher() map_frame_("map"), converged_param_transform_probability_(4.5), initial_estimate_particles_num_(100), + initial_pose_timeout_sec_(1.0), + initial_pose_distance_tolerance_m_(10.0), inversion_vector_threshold_(-0.9), oscillation_threshold_(10) { @@ -169,6 +171,12 @@ NDTScanMatcher::NDTScanMatcher() initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_); + initial_pose_timeout_sec_ = + this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); + + initial_pose_distance_tolerance_m_ = this->declare_parameter( + "initial_pose_distance_tolerance_m", initial_pose_distance_tolerance_m_); + std::vector output_pose_covariance = this->declare_parameter>("output_pose_covariance"); for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { @@ -433,9 +441,27 @@ void NDTScanMatcher::callbackSensorPoints( initial_pose_msg_ptr_array_, sensor_ros_time, initial_pose_old_msg_ptr, initial_pose_new_msg_ptr); popOldPose(initial_pose_msg_ptr_array_, sensor_ros_time); - // TODO(Tier IV): check pose_timestamp - sensor_ros_time + + // check the time stamp + bool valid_old_timestamp = validateTimeStampDifference( + initial_pose_old_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_); + bool valid_new_timestamp = validateTimeStampDifference( + initial_pose_new_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_); + + // check the position jumping (ex. immediately after the initial pose estimation) + bool valid_new_to_old_distance = validatePositionDifference( + initial_pose_old_msg_ptr->pose.pose.position, initial_pose_new_msg_ptr->pose.pose.position, + initial_pose_distance_tolerance_m_); + + // must all validations are true + if (!(valid_old_timestamp && valid_new_timestamp && valid_new_to_old_distance)) { + RCLCPP_WARN(get_logger(), "Validation error."); + return; + } + const auto initial_pose_msg = interpolatePose(*initial_pose_old_msg_ptr, *initial_pose_new_msg_ptr, sensor_ros_time); + // enf of critical section for initial_pose_msg_ptr_array_ initial_pose_array_lock.unlock(); @@ -688,3 +714,35 @@ bool NDTScanMatcher::getTransform( } return true; } + +bool NDTScanMatcher::validateTimeStampDifference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) +{ + const double dt = std::abs((target_time - reference_time).seconds()); + if (dt > time_tolerance_sec) { + RCLCPP_WARN( + get_logger(), + "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " + "difference is %lf[sec] (the tolerance is %lf[sec]).", + reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); + return false; + } + return true; +} + +bool NDTScanMatcher::validatePositionDifference( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, + const double distance_tolerance_m_) +{ + double distance = norm(target_point, reference_point); + if (distance > distance_tolerance_m_) { + RCLCPP_WARN( + get_logger(), + "Validation error. The distance from reference position to target position is %lf[m] (the " + "tolerance is %lf[m]).", + distance, distance_tolerance_m_); + return false; + } + return true; +}