Skip to content

Commit f914182

Browse files
Motsu-sanMotsu-san
committed
feat: Set diagnostic level STALE as INVALID
when the node is NOT activated and initial pose is NOT set Signed-off-by: Motsu-san <83898149+Motsu-san@users.noreply.github.com>
1 parent d7621ef commit f914182

1 file changed

Lines changed: 118 additions & 99 deletions

File tree

‎localization/autoware_ekf_localizer/src/ekf_localizer.cpp‎

Lines changed: 118 additions & 99 deletions
Original file line numberDiff line numberDiff line change
@@ -389,120 +389,139 @@ void EKFLocalizer::publish_diagnostics(
389389
diag_msg.status.push_back(status);
390390
}
391391

392-
if (is_activated_ && is_set_initialpose_) {
393-
// 3. Pose no update count diagnostic
394-
{
395-
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_updated(
396-
"pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn,
397-
params_.pose_no_update_count_threshold_error);
398-
status.name = "localization: " + std::string(this->get_name()) + ": pose_no_update_count";
399-
status.hardware_id = this->get_name();
400-
diag_msg.status.push_back(status);
401-
}
392+
// 3. Pose no update count diagnostic
393+
{
394+
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_updated(
395+
"pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn,
396+
params_.pose_no_update_count_threshold_error);
397+
status.name = "localization: " + std::string(this->get_name()) + ": pose_no_update_count";
398+
status.hardware_id = this->get_name();
399+
diag_msg.status.push_back(status);
400+
}
402401

403-
// 4. Pose queue size diagnostic
404-
{
405-
diagnostic_msgs::msg::DiagnosticStatus status =
406-
check_measurement_queue_size("pose", pose_diag_info_.queue_size);
407-
status.name = "localization: " + std::string(this->get_name()) + ": pose_queue_size";
408-
status.hardware_id = this->get_name();
409-
diag_msg.status.push_back(status);
410-
}
402+
// 4. Pose queue size diagnostic
403+
{
404+
diagnostic_msgs::msg::DiagnosticStatus status =
405+
check_measurement_queue_size("pose", pose_diag_info_.queue_size);
406+
status.name = "localization: " + std::string(this->get_name()) + ": pose_queue_size";
407+
status.hardware_id = this->get_name();
408+
diag_msg.status.push_back(status);
409+
}
411410

412-
// 5. Pose delay time diagnostic
413-
{
414-
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_delay_gate(
415-
"pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time,
416-
pose_diag_info_.delay_time_threshold);
417-
status.name = "localization: " + std::string(this->get_name()) + ": pose_delay_time";
418-
status.hardware_id = this->get_name();
419-
diag_msg.status.push_back(status);
411+
// 5. Pose delay time diagnostic
412+
{
413+
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_delay_gate(
414+
"pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time,
415+
pose_diag_info_.delay_time_threshold);
416+
status.name = "localization: " + std::string(this->get_name()) + ": pose_delay_time";
417+
status.hardware_id = this->get_name();
418+
// STALE as INVALID when the node is NOT activated and initial pose is NOT set
419+
if (!(is_activated_ && is_set_initialpose_)) {
420+
status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
420421
}
422+
diag_msg.status.push_back(status);
423+
}
421424

422-
// 6. Pose mahalanobis distance diagnostic
423-
{
424-
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_mahalanobis_gate(
425-
"pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance,
426-
params_.pose_gate_dist);
427-
status.name =
428-
"localization: " + std::string(this->get_name()) + ": pose_mahalanobis_distance";
429-
status.hardware_id = this->get_name();
430-
diag_msg.status.push_back(status);
425+
// 6. Pose mahalanobis distance diagnostic
426+
{
427+
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_mahalanobis_gate(
428+
"pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance,
429+
params_.pose_gate_dist);
430+
status.name = "localization: " + std::string(this->get_name()) + ": pose_mahalanobis_distance";
431+
status.hardware_id = this->get_name();
432+
// STALE as INVALID when the node is NOT activated and initial pose is NOT set
433+
if (!(is_activated_ && is_set_initialpose_)) {
434+
status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
431435
}
436+
diag_msg.status.push_back(status);
437+
}
432438

433-
// 7. Twist no update count diagnostic
434-
{
435-
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_updated(
436-
"twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn,
437-
params_.twist_no_update_count_threshold_error);
438-
status.name = "localization: " + std::string(this->get_name()) + ": twist_no_update_count";
439-
status.hardware_id = this->get_name();
440-
diag_msg.status.push_back(status);
441-
}
439+
// 7. Twist no update count diagnostic
440+
{
441+
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_updated(
442+
"twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn,
443+
params_.twist_no_update_count_threshold_error);
444+
status.name = "localization: " + std::string(this->get_name()) + ": twist_no_update_count";
445+
status.hardware_id = this->get_name();
446+
diag_msg.status.push_back(status);
447+
}
442448

443-
// 8. Twist queue size diagnostic
444-
{
445-
diagnostic_msgs::msg::DiagnosticStatus status =
446-
check_measurement_queue_size("twist", twist_diag_info_.queue_size);
447-
status.name = "localization: " + std::string(this->get_name()) + ": twist_queue_size";
448-
status.hardware_id = this->get_name();
449-
diag_msg.status.push_back(status);
450-
}
449+
// 8. Twist queue size diagnostic
450+
{
451+
diagnostic_msgs::msg::DiagnosticStatus status =
452+
check_measurement_queue_size("twist", twist_diag_info_.queue_size);
453+
status.name = "localization: " + std::string(this->get_name()) + ": twist_queue_size";
454+
status.hardware_id = this->get_name();
455+
diag_msg.status.push_back(status);
456+
}
451457

452-
// 9. Twist delay time diagnostic
453-
{
454-
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_delay_gate(
455-
"twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time,
456-
twist_diag_info_.delay_time_threshold);
457-
status.name = "localization: " + std::string(this->get_name()) + ": twist_delay_time";
458-
status.hardware_id = this->get_name();
459-
diag_msg.status.push_back(status);
458+
// 9. Twist delay time diagnostic
459+
{
460+
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_delay_gate(
461+
"twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time,
462+
twist_diag_info_.delay_time_threshold);
463+
status.name = "localization: " + std::string(this->get_name()) + ": twist_delay_time";
464+
status.hardware_id = this->get_name();
465+
// STALE as INVALID when the node is NOT activated and initial pose is NOT set
466+
if (!(is_activated_ && is_set_initialpose_)) {
467+
status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
460468
}
469+
diag_msg.status.push_back(status);
470+
}
461471

462-
// 10. Twist mahalanobis distance diagnostic
463-
{
464-
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_mahalanobis_gate(
465-
"twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance,
466-
params_.twist_gate_dist);
467-
status.name =
468-
"localization: " + std::string(this->get_name()) + ": twist_mahalanobis_distance";
469-
status.hardware_id = this->get_name();
470-
diag_msg.status.push_back(status);
472+
// 10. Twist mahalanobis distance diagnostic
473+
{
474+
diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_mahalanobis_gate(
475+
"twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance,
476+
params_.twist_gate_dist);
477+
status.name = "localization: " + std::string(this->get_name()) + ": twist_mahalanobis_distance";
478+
status.hardware_id = this->get_name();
479+
// STALE as INVALID when the node is NOT activated and initial pose is NOT set
480+
if (!(is_activated_ && is_set_initialpose_)) {
481+
status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
471482
}
483+
diag_msg.status.push_back(status);
484+
}
472485

473-
// 11. Covariance ellipse long axis diagnostic
474-
{
475-
geometry_msgs::msg::PoseWithCovariance pose_cov;
476-
pose_cov.pose = current_ekf_pose.pose;
477-
pose_cov.covariance = ekf_module_->get_current_pose_covariance();
478-
const autoware::localization_util::Ellipse ellipse =
479-
autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale);
480-
481-
diagnostic_msgs::msg::DiagnosticStatus status = check_covariance_ellipse(
482-
"cov_ellipse_long_axis", ellipse.long_radius, params_.warn_ellipse_size,
483-
params_.error_ellipse_size);
484-
status.name =
485-
"localization: " + std::string(this->get_name()) + ": cov_ellipse_long_axis_size";
486-
status.hardware_id = this->get_name();
487-
diag_msg.status.push_back(status);
486+
// 11. Covariance ellipse long axis diagnostic
487+
{
488+
geometry_msgs::msg::PoseWithCovariance pose_cov;
489+
pose_cov.pose = current_ekf_pose.pose;
490+
pose_cov.covariance = ekf_module_->get_current_pose_covariance();
491+
const autoware::localization_util::Ellipse ellipse =
492+
autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale);
493+
494+
diagnostic_msgs::msg::DiagnosticStatus status = check_covariance_ellipse(
495+
"cov_ellipse_long_axis", ellipse.long_radius, params_.warn_ellipse_size,
496+
params_.error_ellipse_size);
497+
status.name = "localization: " + std::string(this->get_name()) + ": cov_ellipse_long_axis_size";
498+
status.hardware_id = this->get_name();
499+
// STALE as INVALID when the node is NOT activated and initial pose is NOT set
500+
if (!(is_activated_ && is_set_initialpose_)) {
501+
status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
488502
}
503+
diag_msg.status.push_back(status);
504+
}
489505

490-
// 12. Covariance ellipse lateral direction diagnostic
491-
{
492-
geometry_msgs::msg::PoseWithCovariance pose_cov;
493-
pose_cov.pose = current_ekf_pose.pose;
494-
pose_cov.covariance = ekf_module_->get_current_pose_covariance();
495-
const autoware::localization_util::Ellipse ellipse =
496-
autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale);
497-
498-
diagnostic_msgs::msg::DiagnosticStatus status = check_covariance_ellipse(
499-
"cov_ellipse_lateral_direction", ellipse.size_lateral_direction,
500-
params_.warn_ellipse_size_lateral_direction, params_.error_ellipse_size_lateral_direction);
501-
status.name =
502-
"localization: " + std::string(this->get_name()) + ": cov_ellipse_lateral_direction_size";
503-
status.hardware_id = this->get_name();
504-
diag_msg.status.push_back(status);
506+
// 12. Covariance ellipse lateral direction diagnostic
507+
{
508+
geometry_msgs::msg::PoseWithCovariance pose_cov;
509+
pose_cov.pose = current_ekf_pose.pose;
510+
pose_cov.covariance = ekf_module_->get_current_pose_covariance();
511+
const autoware::localization_util::Ellipse ellipse =
512+
autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale);
513+
514+
diagnostic_msgs::msg::DiagnosticStatus status = check_covariance_ellipse(
515+
"cov_ellipse_lateral_direction", ellipse.size_lateral_direction,
516+
params_.warn_ellipse_size_lateral_direction, params_.error_ellipse_size_lateral_direction);
517+
status.name =
518+
"localization: " + std::string(this->get_name()) + ": cov_ellipse_lateral_direction_size";
519+
status.hardware_id = this->get_name();
520+
// STALE as INVALID when the node is NOT activated and initial pose is NOT set
521+
if (!(is_activated_ && is_set_initialpose_)) {
522+
status.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
505523
}
524+
diag_msg.status.push_back(status);
506525
}
507526

508527
pub_diag_->publish(diag_msg);

0 commit comments

Comments
 (0)