feat(ekf_localizer): add diagnostics

[YamatoAndo]

Description

Add ekf_localizer's diagnostics.

Please refer to the readme for the conditions that trigger WARN/ERROR states.

Related links

autowarefoundation/autoware_launch#554

Tests performed

Test with autowarefoundation/autoware_launch#554

I have confirmed the following

• The logging simulator works with the sample rosbag
• The diagnostic state triggers WARN/ERROR under the conditions mentioned in the readme
• The unit test is passed

Notes for reviewers

After merging this pr, I will modify system_error_monitor.param.yaml and diagnostic_aggregator/localization.param.yaml to notify ekf_localizer's state to the autoware system.

Interface changes

• add a new published topic
  • /diagnostics
• add parameters
  • pose_no_update_count_threshold_warn
  • pose_no_update_count_threshold_error
  • twist_no_update_count_threshold_warn
  • twist_no_update_count_threshold_error

Effects on system behavior

N/A

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Patch coverage: 30.03% and project coverage change: +0.03% 🎉

Comparison is base (2e6bc3f) 15.92% compared to head (39f35d5) 15.96%.
Report is 58 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4914      +/-   ##
==========================================
+ Coverage   15.92%   15.96%   +0.03%     
==========================================
  Files        1579     1581       +2     
  Lines      108886   109155     +269     
  Branches    33584    33753     +169     
==========================================
+ Hits        17344    17426      +82     
- Misses      72944    73014      +70     
- Partials    18598    18715     +117     

Flag	Coverage Δ		*Carryforward flag
differential	28.40% <30.03%> (?)
total	15.89% <ø> (-0.04%)	⬇️	Carriedforward from f4455d9

*This pull request uses carry forward flags. Click here to find out more.

Files Changed	Coverage Δ
..._localizer/include/ekf_localizer/ekf_localizer.hpp	0.00% <ø> (ø)
...calizer/include/ekf_localizer/hyper_parameters.hpp	0.00% <0.00%> (ø)
localization/ekf_localizer/src/ekf_localizer.cpp	0.00% <0.00%> (ø)
...calization/ekf_localizer/test/test_diagnostics.cpp	31.81% <31.81%> (ø)
localization/ekf_localizer/src/diagnostics.cpp	52.08% <52.08%> (ø)

... and 1 file with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[Motsu-san]

Big commit.
Could you separate the commit fc1dd2f to feature changes, tests and docs(ex. README)?

[KYabuuchi]

Thank you for creating the pull request. I have confirmed that the diagnostics works well in my environment too. 👍
Please wait a bit for the implementation review.

[SakodaShintaro]

I am not familiar with C++, so I simply have a question.
Take checkProcessActivated as an example.

autoware.universe/localization/ekf_localizer/src/diagnostics.cpp

Lines 21 to 38 in 67ae6f9

	void checkProcessActivated(
	diagnostic_updater::DiagnosticStatusWrapper & stat, const bool * const is_activated_ptr)
	{
	if (is_activated_ptr == nullptr) {
	return;
	}
	stat.add("is_activated", (*is_activated_ptr));
	int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
	std::string diag_message;
	if (!(*is_activated_ptr)) {
	diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
	diag_message = "[WARN]process is not activated";
	}
	stat.summary(diag_level, diag_message);
	}

autoware.universe/localization/ekf_localizer/src/ekf_localizer.cpp

Lines 106 to 107 in 67ae6f9

	diag_process_activated_.reset(new diagnostic_updater::FunctionDiagnosticTask(
	"process activated", std::bind(&checkProcessActivated, std::placeholders::_1, &is_activated_)));

When the EKFLocalizer instance is destroyed (when the destructor is called), what happens if the is_activated_ptr_ variable is deleted before the diag_process_activated_?
I think that since &is_activated_ptr_ does not disappear synchronously with is_activated_ptr_, it would not be caught by the nullptr check and would refer to the destroyed area.

And it seems that member variables in C++ classes are destroyed in the reverse order of their declaration.

https://stackoverflow.com/questions/2254263/order-of-member-constructor-and-destructor-calls

In EKFLocalizer, diag_process_activated_ is constructed before is_activated_ptr_. Since is_activated_ptr_ is discarded before diag_process_activated_, I think it is possible that undefined behavior may be created for a very short moment.

autoware.universe/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

Lines 111 to 184 in 67ae6f9

	//!< @brief ekf estimated pose publisher
	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose_;
	//!< @brief estimated ekf pose with covariance publisher
	rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_pose_cov_;
	//!< @brief estimated ekf odometry publisher
	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom_;
	//!< @brief ekf estimated twist publisher
	rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_twist_;
	//!< @brief ekf estimated twist with covariance publisher
	rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr pub_twist_cov_;
	//!< @brief debug info publisher
	rclcpp::Publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr pub_debug_;
	//!< @brief debug measurement pose publisher
	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_measured_pose_;
	//!< @brief ekf estimated yaw bias publisher
	rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_yaw_bias_;
	//!< @brief ekf estimated yaw bias publisher
	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_biased_pose_;
	//!< @brief ekf estimated yaw bias publisher
	rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_biased_pose_cov_;
	//!< @brief initial pose subscriber
	rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initialpose_;
	//!< @brief measurement pose with covariance subscriber
	rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_pose_with_cov_;
	//!< @brief measurement twist with covariance subscriber
	rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
	sub_twist_with_cov_;
	//!< @brief time for ekf calculation callback
	rclcpp::TimerBase::SharedPtr timer_control_;
	//!< @brief last predict time
	std::shared_ptr<const rclcpp::Time> last_predict_time_;
	//!< @brief trigger_node service
	rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_trigger_node_;
	//!< @brief timer to send transform
	rclcpp::TimerBase::SharedPtr timer_tf_;
	//!< @brief tf broadcaster
	std::shared_ptr<tf2_ros::TransformBroadcaster> tf_br_;
	//!< @brief diagnostic updater
	std::shared_ptr<diagnostic_updater::Updater> diag_updater_;
	std::shared_ptr<diagnostic_updater::CompositeDiagnosticTask> diag_composite_task_;
	std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_process_activated_;
	std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_pose_updated_;
	std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_pose_queue_size_;
	std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_pose_delay_gate_;
	std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_pose_mahalanobis_gate_;
	std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_twist_updated_;
	std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_twist_queue_size_;
	std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_twist_delay_gate_;
	std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_twist_mahalanobis_gate_;
	//!< @brief extended kalman filter instance.
	TimeDelayKalmanFilter ekf_;
	Simple1DFilter z_filter_;
	Simple1DFilter roll_filter_;
	Simple1DFilter pitch_filter_;
	const HyperParameters params_;
	double ekf_rate_;
	double ekf_dt_;
	/* parameters */
	int dim_x_; //!< @brief dimension of EKF state
	/* process noise variance for discrete model */
	double proc_cov_yaw_d_; //!< @brief discrete yaw process noise
	double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise
	double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0
	double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0
	bool is_activated_;

Is this a needless concern?

[YamatoAndo]

@SakodaShintaro
This node is single-threaded, so parallel processing does not occur.
So I believe that checkProcessActivated will not be called in parallel while the destructor is running.

[YamatoAndo]

If my understanding is incorrect, please let me know.

[SakodaShintaro]

@YamatoAndo
Oh, I see. I understood that the diag is a one of single-threaded timer callback which runs on the same thread as ekf. Then it looks fine.

I would like to check the diagnostics working, but how did you get the image shown at the beginning of the Description?

[YamatoAndo]

@SakodaShintaro I used following commnad to get that image.
$ ros2 run rqt_runtime_monitor rqt_runtime_monitor

[YamatoAndo]

Since there is a possibility of making significant changes, I will temporarily convert to draft.

[YamatoAndo]

@KYabuuchi @SakodaShintaro I have reopened the PR. Please review again.

[SakodaShintaro]

Looks Good To Me

[KYabuuchi]

👍

[YamatoAndo]

@Motsu-san Could you please approve this pr?

[kminoda]

LGTM

[Motsu-san]

I approve this pull-request even without organizing the commits due to the circumstances of the squash merge rule. I hope that the pull-request will become smaller in size, which is a prerequisite for squash merge in the future.

[kminoda]

@Motsu-san Thank you!