diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md
index 987e4581e867e..d23ce7b07767d 100644
--- a/localization/ndt_scan_matcher/README.md
+++ b/localization/ndt_scan_matcher/README.md
@@ -267,7 +267,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 |
-| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | failed | none | yes |
+| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | faild | 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 |
@@ -290,7 +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 |
+| `is_expected_frame_id` | whether the input frame_id is the same as `frame.map_frame` or not | none | not the same |
### regularization_pose_subscriber_status
@@ -320,7 +320,7 @@ This diagnostic is only published when the service is called, so it becomes stal
| Name | Description | Transition condition to Warning | Transition condition to Error |
| ----------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------- | ----------------------------------------------------------- |
| `service_call_time_stamp` | the time stamp of service calling | none | none |
-| `is_succeed_transform_initial_pose` | whether transform initial pose is succeed or not | failed | none |
+| `is_succeed_transform_initial_pose` | whether transform initial pose is succeed or not | none | failed |
| `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 |
diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp
index 2ad95d4714061..43174a39b797e 100644
--- a/localization/ndt_scan_matcher/src/map_update_module.cpp
+++ b/localization/ndt_scan_matcher/src/map_update_module.cpp
@@ -141,6 +141,7 @@ void MapUpdateModule::update_map(
"properly.";
diagnostics_ptr->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
+ RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, message.str());
last_update_position_ = position;
ndt_ptr_mutex_->unlock();
return;
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 4b91ed4f50a5b..77a88a3e53299 100644
--- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -349,8 +349,8 @@ bool NDTScanMatcher::callback_sensor_points_main(
message << ex.what() << ". Please publish TF " << sensor_frame << " to "
<< param_.frame.base_frame;
diagnostics_scan_points_->updateLevelAndMessage(
- diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
- RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
+ diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
+ RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_scan_points_->addKeyValue("is_succeed_transform_sensor_points", false);
return false;
}
@@ -491,11 +491,10 @@ bool NDTScanMatcher::callback_sensor_points_main(
bool is_ok_score = (score > score_threshold);
if (!is_ok_score) {
std::stringstream message;
- message << "Transform Probability"
- << " is below the threshold. Score: " << score << ", Threshold: " << score_threshold
- << ".";
+ message << "Score is below the threshold. Score: " << score << ", Threshold: " << score_threshold;
diagnostics_scan_points_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+ RCLCPP_WARN_STREAM(this->get_logger(), message.str());
}
// check is_converged
@@ -928,7 +927,8 @@ void NDTScanMatcher::service_ndt_align_main(
std::stringstream message;
message << "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str();
diagnostics_ndt_align_->updateLevelAndMessage(
- diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+ diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
+ RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
res->success = false;
return;
}
@@ -950,7 +950,7 @@ void NDTScanMatcher::service_ndt_align_main(
message << "No InputTarget. Please check the map file and the map_loader service";
diagnostics_ndt_align_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
-
+ RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
res->success = false;
return;
}
@@ -963,7 +963,7 @@ void NDTScanMatcher::service_ndt_align_main(
message << "No InputSource. Please check the input lidar topic";
diagnostics_ndt_align_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
-
+ RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
res->success = false;
return;
}