diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 8049104e8f2f5..48ce49d2bcaa9 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -266,7 +266,7 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num | ------------------------------------------------ | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- | | `topic_time_stamp` | the time stamp of input topic | none | none | no | | `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | -| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | +| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `sensor_points.timeout_sec` | none | yes | | `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | | `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | | `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. | @@ -280,9 +280,9 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num | `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | | `nearest_voxel_transformation_likelihood_diff` | the nvtl score difference for the current ndt optimization | none | none | no | | `nearest_voxel_transformation_likelihood_before` | the nvtl score before the current ndt optimization | none | none | no | -| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | +| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than `validation.initial_to_result_distance_tolerance_m` | none | no | | `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | -| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - | +| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is `validation.skipping_publish_num` or more | none | - | ※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale. 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 ec80a0ef79c69..f62329b8bdb6d 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -12,6 +12,9 @@ sensor_points: + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + timeout_sec: 1.0 + # Required distance of input sensor points. [m] # If the max distance of input sensor points is lower than this value, the scan matching will not be performed. required_distance: 10.0 @@ -52,18 +55,21 @@ validation: - # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] - lidar_topic_timeout_sec: 1.0 - # 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 + # Tolerance of distance difference from initial pose to result pose. [m] + initial_to_result_distance_tolerance_m: 3.0 + # The execution time which means probably NDT cannot matches scans properly. [ms] critical_upper_bound_exe_time_ms: 100.0 + # Tolerance for the number of times rejected estimation results consecutively + skipping_publish_num: 5 + score_estimation: # Converged param type 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 index 7002b4bf43a73..e488b49393d48 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -40,6 +40,7 @@ struct HyperParameters struct SensorPoints { + double timeout_sec{}; double required_distance{}; } sensor_points{}; @@ -54,10 +55,11 @@ struct HyperParameters struct Validation { - double lidar_topic_timeout_sec{}; double initial_pose_timeout_sec{}; double initial_pose_distance_tolerance_m{}; + double initial_to_result_distance_tolerance_m{}; double critical_upper_bound_exe_time_ms{}; + int64_t skipping_publish_num{}; } validation{}; struct ScoreEstimation @@ -97,6 +99,7 @@ struct HyperParameters frame.ndt_base_frame = node->declare_parameter("frame.ndt_base_frame"); frame.map_frame = node->declare_parameter("frame.map_frame"); + sensor_points.timeout_sec = node->declare_parameter("sensor_points.timeout_sec"); sensor_points.required_distance = node->declare_parameter("sensor_points.required_distance"); @@ -115,14 +118,16 @@ struct HyperParameters 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.initial_to_result_distance_tolerance_m = + node->declare_parameter("validation.initial_to_result_distance_tolerance_m"); validation.critical_upper_bound_exe_time_ms = node->declare_parameter("validation.critical_upper_bound_exe_time_ms"); + validation.skipping_publish_num = + node->declare_parameter("validation.skipping_publish_num"); const int64_t converged_param_type_tmp = node->declare_parameter("score_estimation.converged_param_type"); diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/ndt_scan_matcher/schema/sub/sensor_points.json index 68a0b40f8f94e..12bda6fb38d24 100644 --- a/localization/ndt_scan_matcher/schema/sub/sensor_points.json +++ b/localization/ndt_scan_matcher/schema/sub/sensor_points.json @@ -5,13 +5,19 @@ "sensor_points": { "type": "object", "properties": { + "timeout_sec": { + "type": "number", + "description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]", + "default": 1.0, + "minimum": 0.0 + }, "required_distance": { "type": "number", "description": "Required distance of input sensor points [m]. If the max distance of input sensor points is lower than this value, the scan matching will not be performed.", "default": "10.0" } }, - "required": ["required_distance"], + "required": ["timeout_sec", "required_distance"], "additionalProperties": false } } diff --git a/localization/ndt_scan_matcher/schema/sub/validation.json b/localization/ndt_scan_matcher/schema/sub/validation.json index 5ad40ceb99577..c1168d733ff9d 100644 --- a/localization/ndt_scan_matcher/schema/sub/validation.json +++ b/localization/ndt_scan_matcher/schema/sub/validation.json @@ -5,12 +5,6 @@ "validation": { "type": "object", "properties": { - "lidar_topic_timeout_sec": { - "type": "number", - "description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]", - "default": 1.0, - "minimum": 0.0 - }, "initial_pose_timeout_sec": { "type": "number", "description": "Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]", @@ -23,18 +17,31 @@ "default": 10.0, "minimum": 0.0 }, + "initial_to_result_distance_tolerance_m": { + "type": "number", + "description": "Tolerance of distance difference from initial pose to result pose. [m]", + "default": 3.0, + "minimum": 0.0 + }, "critical_upper_bound_exe_time_ms": { "type": "number", "description": "The execution time which means probably NDT cannot matches scans properly. [ms]", "default": 100.0, "minimum": 0.0 + }, + "skipping_publish_num": { + "type": "number", + "description": "Tolerance for the number for times rejected estimation results consecutively.", + "default": 5, + "minimum": 1 } }, "required": [ - "lidar_topic_timeout_sec", "initial_pose_timeout_sec", "initial_pose_distance_tolerance_m", - "critical_upper_bound_exe_time_ms" + "initial_to_result_distance_tolerance_m", + "critical_upper_bound_exe_time_ms", + "skipping_publish_num" ], "additionalProperties": false } 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 f87b1cd8e98a8..31f58f94ed6e8 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -300,12 +300,11 @@ void NDTScanMatcher::callback_sensor_points( callback_sensor_points_main(sensor_points_msg_in_sensor_frame); // check skipping_publish_num - static size_t skipping_publish_num = 0; - const size_t error_skipping_publish_num = 5; + static int64_t skipping_publish_num = 0; skipping_publish_num = ((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1)); diagnostics_scan_points_->add_key_value("skipping_publish_num", skipping_publish_num); - if (skipping_publish_num >= error_skipping_publish_num) { + if (skipping_publish_num >= param_.validation.skipping_publish_num) { std::stringstream message; message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times)."; diagnostics_scan_points_->update_level_and_message( @@ -340,11 +339,11 @@ bool NDTScanMatcher::callback_sensor_points_main( (this->now() - sensor_points_msg_in_sensor_frame->header.stamp).seconds(); diagnostics_scan_points_->add_key_value( "sensor_points_delay_time_sec", sensor_points_delay_time_sec); - if (sensor_points_delay_time_sec > param_.validation.lidar_topic_timeout_sec) { + if (sensor_points_delay_time_sec > param_.sensor_points.timeout_sec) { std::stringstream message; message << "sensor points is experiencing latency." << "The delay time is " << sensor_points_delay_time_sec << "[sec] " - << "(the tolerance is " << param_.validation.lidar_topic_timeout_sec << "[sec])."; + << "(the tolerance is " << param_.sensor_points.timeout_sec << "[sec])."; diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); @@ -578,8 +577,7 @@ bool NDTScanMatcher::callback_sensor_points_main( const auto distance_initial_to_result = static_cast( norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position)); diagnostics_scan_points_->add_key_value("distance_initial_to_result", distance_initial_to_result); - const double warn_distance_initial_to_result = 3.0; - if (distance_initial_to_result > warn_distance_initial_to_result) { + if (distance_initial_to_result > param_.validation.initial_to_result_distance_tolerance_m) { std::stringstream message; message << "distance_initial_to_result is too large (" << distance_initial_to_result << " [m]).";