Skip to content

Commit

Permalink
output to terminal
Browse files Browse the repository at this point in the history 
Signed-off-by: Yamato Ando <[email protected]>
  • Loading branch information
@YamatoAndo
YamatoAndo committed Apr 26, 2024
1 parent c33adc9 commit 2e60bef
Show file tree
Hide file tree
Showing 3 changed files with 12 additions and 11 deletions.
6 changes: 3 additions & 3 deletions localization/ndt_scan_matcher/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 |

Check warning on line 270 in localization/ndt_scan_matcher/README.md

View workflow job for this annotation

GitHub Actions / spell-check-partial 

Unknown word (faild)
| `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. <br> (1) the size of `initial_pose_buffer_` is **smaller** than 2. <br> (2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`. <br> (3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes |
Expand All @@ -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

Expand Down Expand Up @@ -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 |
Expand Down
1 change: 1 addition & 0 deletions localization/ndt_scan_matcher/src/map_update_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
16 changes: 8 additions & 8 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand Down

0 comments on commit 2e60bef

Please sign in to comment.