From a90a1714a0a6138d953914a6254b89db85bbced8 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 5 Jun 2024 14:36:03 +0900 Subject: [PATCH 1/7] rename to sensor_points.timeout_sec Signed-off-by: Yamato Ando --- .../config/ndt_scan_matcher.param.yaml | 6 +++--- .../include/ndt_scan_matcher/hyper_parameters.hpp | 6 +++--- .../ndt_scan_matcher/schema/sub/sensor_points.json | 10 +++++++++- .../ndt_scan_matcher/schema/sub/validation.json | 7 ------- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 4 ++-- 5 files changed, 17 insertions(+), 16 deletions(-) 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..23f55d0517608 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,9 +55,6 @@ 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 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 bc7bf1fe40d36..94084590bf57f 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,7 +55,6 @@ struct HyperParameters 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; @@ -97,6 +97,8 @@ 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,8 +117,6 @@ 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 = diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/ndt_scan_matcher/schema/sub/sensor_points.json index 68a0b40f8f94e..cf28e911851a8 100644 --- a/localization/ndt_scan_matcher/schema/sub/sensor_points.json +++ b/localization/ndt_scan_matcher/schema/sub/sensor_points.json @@ -5,13 +5,21 @@ "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..3886e868d219d 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]", @@ -31,7 +25,6 @@ } }, "required": [ - "lidar_topic_timeout_sec", "initial_pose_timeout_sec", "initial_pose_distance_tolerance_m", "critical_upper_bound_exe_time_ms" 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 8acfe3bd5c1ca..6cbb4aee7d0d2 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -318,11 +318,11 @@ bool NDTScanMatcher::callback_sensor_points_main( (this->now() - sensor_points_msg_in_sensor_frame->header.stamp).seconds(); diagnostics_scan_points_->addKeyValue( "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_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); From 0698d3d44357fa5d3d6eae4de6e3a5131599d07d Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 5 Jun 2024 14:54:31 +0900 Subject: [PATCH 2/7] parameterize skipping_publish_num Signed-off-by: Yamato Ando --- localization/ndt_scan_matcher/README.md | 2 +- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 3 +++ .../include/ndt_scan_matcher/hyper_parameters.hpp | 3 +++ localization/ndt_scan_matcher/schema/sub/validation.json | 9 ++++++++- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 3 +-- 5 files changed, 16 insertions(+), 4 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index a44db7bbaa4bf..f07f63ce74c93 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -278,7 +278,7 @@ 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 | | `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | 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 23f55d0517608..0e861fb5303e7 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -64,6 +64,9 @@ # 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 94084590bf57f..fb4e2293ff767 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 @@ -58,6 +58,7 @@ struct HyperParameters double initial_pose_timeout_sec; double initial_pose_distance_tolerance_m; double critical_upper_bound_exe_time_ms; + int skipping_publish_num; } validation; struct ScoreEstimation @@ -123,6 +124,8 @@ struct HyperParameters 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"); + 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/validation.json b/localization/ndt_scan_matcher/schema/sub/validation.json index 3886e868d219d..1fc34765291f1 100644 --- a/localization/ndt_scan_matcher/schema/sub/validation.json +++ b/localization/ndt_scan_matcher/schema/sub/validation.json @@ -22,12 +22,19 @@ "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": [ "initial_pose_timeout_sec", "initial_pose_distance_tolerance_m", - "critical_upper_bound_exe_time_ms" + "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 6cbb4aee7d0d2..4d8301acf64e2 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -279,11 +279,10 @@ void NDTScanMatcher::callback_sensor_points( // check skipping_publish_num static size_t skipping_publish_num = 0; - const size_t error_skipping_publish_num = 5; skipping_publish_num = ((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1)); diagnostics_scan_points_->addKeyValue("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_->updateLevelAndMessage( From 403a9f12b07a5a5a09ef6c539e104ae50aefd3b8 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 5 Jun 2024 14:55:23 +0900 Subject: [PATCH 3/7] fix readme Signed-off-by: Yamato Ando --- localization/ndt_scan_matcher/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index f07f63ce74c93..79b6c1d3059be 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. | From 909edc1d8cf203939e1bf39b2a4e736d4a83ee3e Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 5 Jun 2024 15:12:53 +0900 Subject: [PATCH 4/7] parameterize sinitial_to_result_distance_tolerance_m Signed-off-by: Yamato Ando --- localization/ndt_scan_matcher/README.md | 2 +- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 3 +++ .../include/ndt_scan_matcher/hyper_parameters.hpp | 7 +++++-- localization/ndt_scan_matcher/schema/sub/validation.json | 7 +++++++ .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 3 +-- 5 files changed, 17 insertions(+), 5 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 79b6c1d3059be..d4ce47edf2da8 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -276,7 +276,7 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num | `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | | `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | | `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 | -| `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 `validation.skipping_publish_num` or more | none | - | 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 0e861fb5303e7..f62329b8bdb6d 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -61,6 +61,9 @@ # 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 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 fb4e2293ff767..f63feab5f8d15 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 @@ -57,8 +57,9 @@ struct HyperParameters { 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; - int skipping_publish_num; + int64_t skipping_publish_num; } validation; struct ScoreEstimation @@ -122,10 +123,12 @@ struct HyperParameters 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"); + 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/validation.json b/localization/ndt_scan_matcher/schema/sub/validation.json index 1fc34765291f1..c1168d733ff9d 100644 --- a/localization/ndt_scan_matcher/schema/sub/validation.json +++ b/localization/ndt_scan_matcher/schema/sub/validation.json @@ -17,6 +17,12 @@ "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]", @@ -33,6 +39,7 @@ "required": [ "initial_pose_timeout_sec", "initial_pose_distance_tolerance_m", + "initial_to_result_distance_tolerance_m", "critical_upper_bound_exe_time_ms", "skipping_publish_num" ], 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 4d8301acf64e2..683c40c9bf715 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -521,8 +521,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_->addKeyValue("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])."; From ca3e6da73aab24d4e45a7c2ab615c6da01e742e3 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 5 Jun 2024 15:36:33 +0900 Subject: [PATCH 5/7] fix build error Signed-off-by: Yamato Ando --- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 683c40c9bf715..fe89d09c5e37b 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -278,7 +278,7 @@ 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; + static int64_t skipping_publish_num = 0; skipping_publish_num = ((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1)); diagnostics_scan_points_->addKeyValue("skipping_publish_num", skipping_publish_num); From eb8dab7c4d6d4db72f62e702c61527fe0318c843 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 5 Jun 2024 06:53:50 +0000 Subject: [PATCH 6/7] style(pre-commit): autofix --- localization/ndt_scan_matcher/README.md | 6 +++--- .../include/ndt_scan_matcher/hyper_parameters.hpp | 3 +-- localization/ndt_scan_matcher/schema/sub/sensor_points.json | 4 +--- 3 files changed, 5 insertions(+), 8 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index d4ce47edf2da8..021f85bcfed33 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 `sensor_points.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. | @@ -276,9 +276,9 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num | `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | | `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | | `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 | -| `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 | +| `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 `validation.skipping_publish_num` 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/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index f63feab5f8d15..4beb1c3eb1700 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 @@ -99,8 +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.timeout_sec = node->declare_parameter("sensor_points.timeout_sec"); sensor_points.required_distance = node->declare_parameter("sensor_points.required_distance"); diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/ndt_scan_matcher/schema/sub/sensor_points.json index cf28e911851a8..12bda6fb38d24 100644 --- a/localization/ndt_scan_matcher/schema/sub/sensor_points.json +++ b/localization/ndt_scan_matcher/schema/sub/sensor_points.json @@ -17,9 +17,7 @@ "default": "10.0" } }, - "required": ["timeout_sec", - "required_distance" - ], + "required": ["timeout_sec", "required_distance"], "additionalProperties": false } } From 055e77f106bef04d95c6666538d630bbf7b9d8a2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 13 Jun 2024 08:25:12 +0000 Subject: [PATCH 7/7] style(pre-commit): autofix --- localization/ndt_scan_matcher/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index d1e70a31260e6..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 `sensor_points.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 `validation.initial_to_result_distance_tolerance_m` | 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 `validation.skipping_publish_num` 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.