diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 1d1d725908f5d..36f29885443c9 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -265,22 +265,22 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num | 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 | -| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | no | -| `is_set_sensor_points` | whether the sensor points is set or not | not set | none | yes | -| `is_set_map_points` | whether the map points is set or not | not set | none | yes | | `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 | no | +| `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 | failed | none | 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 | yes | | `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) | | 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) | | 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 **larger** than 0 and **smaller** than 5 | the number of times is **larger** than 5 | - | +| `skipping_publish_num` | the number of times rejected estimation results consecutively | none | the number of times is **larger** than 5 | - | -※The `sensor_points_callback` shares the same callback group as the `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale. +※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. ### initial_pose_callback @@ -290,6 +290,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 | | `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | +| `is_expected_frame_id` | whether the input frame_id is the same as `frame.map_frame` or not | not the same | none | ### regularization_pose_callback @@ -299,6 +300,19 @@ 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 | +### trigger_node_service + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| ------------------------------------- | ---------------------------------------------------------- | ---------------------------------------------------------------------------------- | ----------------------------- | +| `service_call_time_stamp` | the time stamp of service calling | none | none | +| `is_activated` | whether the node is in the "activate" state or not | none | none | +| `is_succeed_service` | whether the process of service is succeed or not | none | none | + +※ +This diagnostic is only published when the service is called, so it becomes stale after the initial pose estimation is completed. + ### ndt_align_service drawing @@ -306,10 +320,19 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num | Name | Description | Transition condition to Warning | Transition condition to Error | | ------------------------------------- | ---------------------------------------------------------- | ---------------------------------------------------------------------------------- | ----------------------------- | | `service_call_time_stamp` | the time stamp of service calling | none | none | -| `is_succeed_ndt_align_service` | whether the process of ndt_align_service is succeed or not | failed.
(1) The map points is not set.
(2) The sensor points is not set. | none | -| `is_set_sensor_points` | whether the sensor points is set or not | not set | none | +| `is_succeed_transform_initial_pose` | whether tramsform initial pose is succeed or not | failed | none | +| `is_need_rebuild` | whether it need to rebuild the map. If the map has not been loaded yet or if `distance_last_update_position_to_current_position encounters` is an Error state, it is considered necessary to reconstruct the map, and `is_need_rebuild` becomes `True`. | none | none | +| `maps_size_before` | the number of maps before update map | none | none | +| `is_succeed_call_pcd_loader` | whether call pcd_loader service is succeed or not | failed | none | +| `maps_to_add_size` | the number of maps to be added | none | none | +| `maps_to_remove_size` | the number of maps to be removed | none | none | +| `maps_size_after` | the number of maps after update map | none | none | +| `map_update_execution_time` | the time for map updating | none | none | +| `is_updated_map` | whether map is updated. If the map update couldn't be performed or there was no need to update the map, it becomes `False` | none | `is_updated_map` is `False` but `is_need_rebuild` is `True` | | `is_set_map_points` | whether the map points is set or not | not set | none | -| `latest_ndt_align_service_best_score` | the best score of particle | none | none | +| `is_set_sensor_points` | whether the sensor points is set or not | not set | none | +| `best_particle_score` | the best score of particle | none | none | +| `is_succeed_service` | whether the process of service is succeed or not | failed | none | ※ This diagnostic is only published when the service is called, so it becomes stale after the initial pose estimation is completed. @@ -325,9 +348,10 @@ This diagnostic is only published when the service is called, so it becomes stal | `is_set_last_update_position` | whether the `last_update_position` is set or not | not set | none | | `distance_last_update_position_to_current_position` | the distance of `last_update_position` to current position | none | (the distance + `dynamic_map_loading.lidar_radius`) is **larger** than `dynamic_map_loading.map_radius` | | `is_need_rebuild` | whether it need to rebuild the map. If the map has not been loaded yet or if `distance_last_update_position_to_current_position encounters` is an Error state, it is considered necessary to reconstruct the map, and `is_need_rebuild` becomes `True`. | none | none | -| `is_updated_map` | whether map is updated. If the map update couldn't be performed or there was no need to update the map, it becomes `False` | none | `is_updated_map` is `False` but `is_need_rebuild` is `True` | +| `maps_size_before` | the number of maps before update map | none | none | | `is_succeed_call_pcd_loader` | whether call pcd_loader service is succeed or not | failed | none | | `maps_to_add_size` | the number of maps to be added | none | none | | `maps_to_remove_size` | the number of maps to be removed | none | none | -| `maps_size` | the number of maps | none | none | -| `latest_update_execution_time` | the time for map updating | none | none | +| `maps_size_after` | the number of maps after update map | none | none | +| `map_update_execution_time` | the time for map updating | none | none | +| `is_updated_map` | whether map is updated. If the map update couldn't be performed or there was no need to update the map, it becomes `False` | none | `is_updated_map` is `False` but `is_need_rebuild` is `True` | diff --git a/localization/ndt_scan_matcher/media/diagnostic_initial_pose_callback.png b/localization/ndt_scan_matcher/media/diagnostic_initial_pose_callback.png index 80ebcc0599cff..41c90ceca7541 100644 Binary files a/localization/ndt_scan_matcher/media/diagnostic_initial_pose_callback.png and b/localization/ndt_scan_matcher/media/diagnostic_initial_pose_callback.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_map_update_module.png b/localization/ndt_scan_matcher/media/diagnostic_map_update_module.png index 026b0b33655f3..6ba9c1245dbef 100644 Binary files a/localization/ndt_scan_matcher/media/diagnostic_map_update_module.png and b/localization/ndt_scan_matcher/media/diagnostic_map_update_module.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_ndt_align_service.png b/localization/ndt_scan_matcher/media/diagnostic_ndt_align_service.png index b2ce0bb485381..1b72eb7700756 100644 Binary files a/localization/ndt_scan_matcher/media/diagnostic_ndt_align_service.png and b/localization/ndt_scan_matcher/media/diagnostic_ndt_align_service.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_regularization_pose_callback.png b/localization/ndt_scan_matcher/media/diagnostic_regularization_pose_callback.png index e2c524175965d..28dbfb42ca650 100644 Binary files a/localization/ndt_scan_matcher/media/diagnostic_regularization_pose_callback.png and b/localization/ndt_scan_matcher/media/diagnostic_regularization_pose_callback.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_sensor_points_callback.png b/localization/ndt_scan_matcher/media/diagnostic_sensor_points_callback.png index b789c21a48b1c..c829afcea19fb 100644 Binary files a/localization/ndt_scan_matcher/media/diagnostic_sensor_points_callback.png and b/localization/ndt_scan_matcher/media/diagnostic_sensor_points_callback.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_trigger_node_service.png b/localization/ndt_scan_matcher/media/diagnostic_trigger_node_service.png new file mode 100644 index 0000000000000..55343f5b3cf39 Binary files /dev/null and b/localization/ndt_scan_matcher/media/diagnostic_trigger_node_service.png differ