From c085b156686f1c6b822d9101e84132ee2c1ca450 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 6 Jun 2024 14:53:46 +0900 Subject: [PATCH] feat(ndt_scan_matcher): added score diff to ndt diag (#7227) * Added score diff to ndt diag Signed-off-by: Shintaro Sakoda * Updated README.md Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix --------- Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/README.md | 38 ++++++++++--------- .../src/ndt_scan_matcher_core.cpp | 32 ++++++++++++++++ 2 files changed, 53 insertions(+), 17 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index a44db7bbaa4bf..8049104e8f2f5 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -262,23 +262,27 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num drawing -| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_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 | -| `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. | -| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | -| `is_set_map_points` | whether the map points is set or not | not set | none | yes | -| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | -| `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 | -| `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 | - | +| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_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 | +| `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. | +| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | +| `is_set_map_points` | whether the map points is set or not | not set | none | yes | +| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | +| `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 | +| `transform_probability_diff` | the tp score difference for the current ndt optimization | none | none | no | +| `transform_probability_before` | the tp score before the current ndt optimization | none | none | no | +| `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 | +| `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 | - | ※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/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 8acfe3bd5c1ca..349f924019c28 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -489,6 +489,38 @@ bool NDTScanMatcher::callback_sensor_points_main( return false; } + // check score diff + const std::vector & tp_array = ndt_result.transform_probability_array; + if (static_cast(tp_array.size()) != ndt_result.iteration_num + 1) { + // only publish warning to /diagnostics, not skip publishing pose + std::stringstream message; + message << "transform_probability_array size is not equal to iteration_num + 1." + << " transform_probability_array size: " << tp_array.size() + << ", iteration_num: " << ndt_result.iteration_num; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } else { + const float diff = tp_array.back() - tp_array.front(); + diagnostics_scan_points_->addKeyValue("transform_probability_diff", diff); + diagnostics_scan_points_->addKeyValue("transform_probability_before", tp_array.front()); + } + const std::vector & nvtl_array = ndt_result.nearest_voxel_transformation_likelihood_array; + if (static_cast(nvtl_array.size()) != ndt_result.iteration_num + 1) { + // only publish warning to /diagnostics, not skip publishing pose + std::stringstream message; + message + << "nearest_voxel_transformation_likelihood_array size is not equal to iteration_num + 1." + << " nearest_voxel_transformation_likelihood_array size: " << nvtl_array.size() + << ", iteration_num: " << ndt_result.iteration_num; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } else { + const float diff = nvtl_array.back() - nvtl_array.front(); + diagnostics_scan_points_->addKeyValue("nearest_voxel_transformation_likelihood_diff", diff); + diagnostics_scan_points_->addKeyValue( + "nearest_voxel_transformation_likelihood_before", nvtl_array.front()); + } + bool is_ok_score = (score > score_threshold); if (!is_ok_score) { std::stringstream message;