diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp index 120aed7c7548e..db370eb46ef43 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp @@ -24,10 +24,10 @@ namespace autoware::universe_utils { -class DiagnosticInterface +class DiagnosticsInterface { public: - DiagnosticInterface(rclcpp::Node * node, const std::string & diagnostic_name); + DiagnosticsInterface(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template @@ -48,7 +48,7 @@ class DiagnosticInterface }; template -void DiagnosticInterface::add_key_value(const std::string & key, const T & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; diff --git a/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp index 978af27f202d4..e4d1ec8494113 100644 --- a/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp +++ b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp @@ -23,7 +23,7 @@ namespace autoware::universe_utils { -DiagnosticInterface::DiagnosticInterface(rclcpp::Node * node, const std::string & diagnostic_name) +DiagnosticsInterface::DiagnosticsInterface(rclcpp::Node * node, const std::string & diagnostic_name) : clock_(node->get_clock()) { diagnostics_pub_ = @@ -34,7 +34,7 @@ DiagnosticInterface::DiagnosticInterface(rclcpp::Node * node, const std::string diagnostics_status_msg_.hardware_id = node->get_name(); } -void DiagnosticInterface::clear() +void DiagnosticsInterface::clear() { diagnostics_status_msg_.values.clear(); diagnostics_status_msg_.values.shrink_to_fit(); @@ -43,7 +43,7 @@ void DiagnosticInterface::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), @@ -56,7 +56,7 @@ void DiagnosticInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & k } } -void DiagnosticInterface::add_key_value(const std::string & key, const std::string & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -64,7 +64,7 @@ void DiagnosticInterface::add_key_value(const std::string & key, const std::stri add_key_value(key_value); } -void DiagnosticInterface::add_key_value(const std::string & key, bool value) +void DiagnosticsInterface::add_key_value(const std::string & key, bool value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -72,7 +72,7 @@ void DiagnosticInterface::add_key_value(const std::string & key, bool value) add_key_value(key_value); } -void DiagnosticInterface::update_level_and_message(const int8_t level, const std::string & message) +void DiagnosticsInterface::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -85,12 +85,12 @@ void DiagnosticInterface::update_level_and_message(const int8_t level, const std } } -void DiagnosticInterface::publish(const rclcpp::Time & publish_time_stamp) +void DiagnosticsInterface::publish(const rclcpp::Time & publish_time_stamp) { diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticInterface::create_diagnostics_array( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsInterface::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; diff --git a/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp index a0683694cc2b7..6ec4fccf78b43 100644 --- a/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp +++ b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp @@ -25,9 +25,9 @@ #include #include -using autoware::universe_utils::DiagnosticInterface; +using autoware::universe_utils::DiagnosticsInterface; -class TestDiagnosticInterface : public ::testing::Test +class TestDiagnosticsInterface : public ::testing::Test { protected: void SetUp() override @@ -44,9 +44,9 @@ class TestDiagnosticInterface : public ::testing::Test * Test clear(): * Verify that calling clear() resets DiagnosticStatus values, level, and message. */ -TEST_F(TestDiagnosticInterface, ClearTest) +TEST_F(TestDiagnosticsInterface, ClearTest) { - DiagnosticInterface module(node_.get(), "test_diagnostic"); + DiagnosticsInterface module(node_.get(), "test_diagnostic"); // Add some key-value pairs and update level/message module.add_key_value("param1", 42); @@ -87,9 +87,9 @@ TEST_F(TestDiagnosticInterface, ClearTest) * Test add_key_value(): * Verify that adding the same key updates its value instead of adding a duplicate. */ -TEST_F(TestDiagnosticInterface, AddKeyValueTest) +TEST_F(TestDiagnosticsInterface, AddKeyValueTest) { - DiagnosticInterface module(node_.get(), "test_diagnostic"); + DiagnosticsInterface module(node_.get(), "test_diagnostic"); // Call the template version of add_key_value() with different types module.add_key_value("string_key", std::string("initial_value")); @@ -139,9 +139,9 @@ TEST_F(TestDiagnosticInterface, AddKeyValueTest) * Verify that the level is updated to the highest severity and * that messages are concatenated if level > OK. */ -TEST_F(TestDiagnosticInterface, UpdateLevelAndMessageTest) +TEST_F(TestDiagnosticsInterface, UpdateLevelAndMessageTest) { - DiagnosticInterface module(node_.get(), "test_diagnostic"); + DiagnosticsInterface module(node_.get(), "test_diagnostic"); // Initial status is level=OK(0), message="" module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::OK, "Initial OK"); diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index d52fb5e798b58..0a85f75b74ad7 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -73,7 +73,7 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) "twist_with_covariance", rclcpp::QoS{10}); diagnostics_ = - std::make_unique(this, "gyro_odometer_status"); + std::make_unique(this, "gyro_odometer_status"); // TODO(YamatoAndo) createTimer } diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index 70334738e9ce3..b59e6af341ec2 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node std::deque vehicle_twist_queue_; std::deque gyro_queue_; - std::unique_ptr diagnostics_; + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp index bc7dbbcfc9ca1..34fc61797231b 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -125,7 +125,7 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti tf_listener_ = std::make_shared(*tf_buffer_, this, false); diagnostics_interface_.reset( - new autoware::universe_utils::DiagnosticInterface(this, "marker_detection_status")); + new autoware::universe_utils::DiagnosticsInterface(this, "marker_detection_status")); } void LidarMarkerLocalizer::initialize_diagnostics() diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp index 1b5672cff9d04..22a0c3f642563 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -134,7 +134,7 @@ class LidarMarkerLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; - std::shared_ptr diagnostics_interface_; + std::shared_ptr diagnostics_interface_; Param param_; bool is_activated_; diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp index 5ebcd105d57ba..1f26f6b80aa17 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp @@ -59,7 +59,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); diagnostics_error_monitor_ = - std::make_unique(this, "ellipse_error_status"); + std::make_unique(this, "ellipse_error_status"); } void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp index b38958c420b19..b7d2454eb9f75 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_error_monitor_; + std::unique_ptr diagnostics_error_monitor_; double scale_; double error_ellipse_size_; diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index bf8fce2b302c0..12990259f88cd 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -42,7 +42,7 @@ namespace autoware::ndt_scan_matcher { -using DiagnosticInterface = autoware::universe_utils::DiagnosticInterface; +using DiagnosticsInterface = autoware::universe_utils::DiagnosticsInterface; class MapUpdateModule { @@ -63,19 +63,19 @@ class MapUpdateModule void callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); [[nodiscard]] bool should_update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); // Update the specified NDT bool update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 25b7417ffbe3c..e083e6751c3db 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -211,12 +211,12 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::unique_ptr diagnostics_scan_points_; - std::unique_ptr diagnostics_initial_pose_; - std::unique_ptr diagnostics_regularization_pose_; - std::unique_ptr diagnostics_map_update_; - std::unique_ptr diagnostics_ndt_align_; - std::unique_ptr diagnostics_trigger_node_; + std::unique_ptr diagnostics_scan_points_; + std::unique_ptr diagnostics_initial_pose_; + std::unique_ptr diagnostics_regularization_pose_; + std::unique_ptr diagnostics_map_update_; + std::unique_ptr diagnostics_ndt_align_; + std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; diff --git a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index eea0b8f3d06c4..299d596401b19 100644 --- a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -56,7 +56,7 @@ MapUpdateModule::MapUpdateModule( void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { // check is_activated diagnostics_ptr->add_key_value("is_activated", is_activated); @@ -87,7 +87,7 @@ void MapUpdateModule::callback_timer( bool MapUpdateModule::should_update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { last_update_position_mtx_.lock(); @@ -143,7 +143,7 @@ bool MapUpdateModule::out_of_map_range(const geometry_msgs::msg::Point & positio void MapUpdateModule::update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_); @@ -231,7 +231,7 @@ void MapUpdateModule::update_map( bool MapUpdateModule::update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size()); diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index cef8724423bed..d3e1fa5982867 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -51,7 +51,7 @@ using autoware::localization_util::pose_to_matrix4f; using autoware::localization_util::SmartPoseBuffer; using autoware::localization_util::TreeStructuredParzenEstimator; -using autoware::universe_utils::DiagnosticInterface; +using autoware::universe_utils::DiagnosticsInterface; tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) @@ -141,7 +141,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); diagnostics_regularization_pose_ = - std::make_unique(this, "regularization_pose_subscriber_status"); + std::make_unique(this, "regularization_pose_subscriber_status"); } sensor_aligned_pose_pub_ = @@ -209,13 +209,13 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); - diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); + diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); diagnostics_initial_pose_ = - std::make_unique(this, "initial_pose_subscriber_status"); - diagnostics_map_update_ = std::make_unique(this, "map_update_status"); - diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); + std::make_unique(this, "initial_pose_subscriber_status"); + diagnostics_map_update_ = std::make_unique(this, "map_update_status"); + diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); diagnostics_trigger_node_ = - std::make_unique(this, "trigger_node_service_status"); + std::make_unique(this, "trigger_node_service_status"); logger_configure_ = std::make_unique(this); } diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 67d5c447c09b6..5bde25a564f1d 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -40,7 +40,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); - diagnostics_pose_reliable_ = std::make_unique( + diagnostics_pose_reliable_ = std::make_unique( this, "pose_initializer_status"); if (declare_parameter("ekf_enabled")) { diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index 5305bcc3ad347..a0c1ed3a86576 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -60,7 +60,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false);