From 646e1ebb4cec1ac7d63eb8fb55fd6f95681dafa8 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Fri, 7 Jun 2024 16:03:22 +0900 Subject: [PATCH] refactor based on linter Signed-off-by: a-maumau --- .../diagnostics.hpp | 6 +-- .../localization_error_monitor.hpp | 7 ++- .../src/diagnostics.cpp | 6 +-- .../src/localization_error_monitor.cpp | 18 +++---- .../test/test_diagnostics.cpp | 49 ++++++++++--------- 5 files changed, 45 insertions(+), 41 deletions(-) diff --git a/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp index 4fe4e5a5aac14..b1da87128bee5 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp @@ -20,12 +20,12 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direction( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array); #endif // LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ diff --git a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index 6016677d88136..53f243520742f 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -52,13 +52,12 @@ class LocalizationErrorMonitor : public rclcpp::Node double warn_ellipse_size_lateral_direction_; Ellipse ellipse_; - void onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); - visualization_msgs::msg::Marker createEllipseMarker( + void on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); + visualization_msgs::msg::Marker create_ellipse_marker( const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom); - double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); + static double measure_size_ellipse_along_body_frame(const Eigen::Matrix2d & Pinv, double theta); public: explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options); - ~LocalizationErrorMonitor() = default; }; #endif // LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp index 375fccfa06787..e6b9da8fc4a97 100644 --- a/localization/localization_error_monitor/src/diagnostics.cpp +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -17,7 +17,7 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -41,7 +41,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direction( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -66,7 +66,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection } // The highest level within the stat_array will be reflected in the merged_stat. -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; diff --git a/localization/localization_error_monitor/src/localization_error_monitor.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp index 7a1cd0b213c39..04bb37a85e41b 100644 --- a/localization/localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -45,7 +45,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o this->declare_parameter("warn_ellipse_size_lateral_direction"); odom_sub_ = this->create_subscription( - "input/odom", 1, std::bind(&LocalizationErrorMonitor::onOdom, this, std::placeholders::_1)); + "input/odom", 1, std::bind(&LocalizationErrorMonitor::on_odom, this, std::placeholders::_1)); // QoS setup rclcpp::QoS durable_qos(1); @@ -58,7 +58,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); } -visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( +visualization_msgs::msg::Marker LocalizationErrorMonitor::create_ellipse_marker( const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom) { tf2::Quaternion quat; @@ -85,7 +85,7 @@ visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( return marker; } -void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) +void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) { // create xy covariance (2x2 matrix) // input geometry_msgs::PoseWithCovariance contain 6x6 matrix @@ -110,21 +110,21 @@ void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr in ellipse_.P = xy_covariance; const double yaw_vehicle = tf2::getYaw(input_msg->pose.pose.orientation); ellipse_.size_lateral_direction = - scale_ * measureSizeEllipseAlongBodyFrame(ellipse_.P.inverse(), yaw_vehicle); + scale_ * measure_size_ellipse_along_body_frame(ellipse_.P.inverse(), yaw_vehicle); - const auto ellipse_marker = createEllipseMarker(ellipse_, input_msg); + const auto ellipse_marker = create_ellipse_marker(ellipse_, input_msg); ellipse_marker_pub_->publish(ellipse_marker); // diagnostics std::vector diag_status_array; diag_status_array.push_back( - checkLocalizationAccuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_)); - diag_status_array.push_back(checkLocalizationAccuracyLateralDirection( + check_localization_accuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_)); + diag_status_array.push_back(check_localization_accuracy_lateral_direction( ellipse_.size_lateral_direction, warn_ellipse_size_lateral_direction_, error_ellipse_size_lateral_direction_)); diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; - diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status = merge_diagnostic_status(diag_status_array); diag_merged_status.name = "localization: " + std::string(this->get_name()); diag_merged_status.hardware_id = this->get_name(); @@ -134,7 +134,7 @@ void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr in diag_pub_->publish(diag_msg); } -double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( +double LocalizationErrorMonitor::measure_size_ellipse_along_body_frame( const Eigen::Matrix2d & Pinv, const double theta) { Eigen::MatrixXd e(2, 1); diff --git a/localization/localization_error_monitor/test/test_diagnostics.cpp b/localization/localization_error_monitor/test/test_diagnostics.cpp index 7b7e8aaab4ed9..3b96758f000d4 100644 --- a/localization/localization_error_monitor/test/test_diagnostics.cpp +++ b/localization/localization_error_monitor/test/test_diagnostics.cpp @@ -24,23 +24,23 @@ TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracy) const double error_ellipse_size = 1.0; double ellipse_size = 0.0; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); ellipse_size = 0.7; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); ellipse_size = 0.8; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); ellipse_size = 0.9; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); ellipse_size = 1.0; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } @@ -52,28 +52,33 @@ TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracyLateralDi const double error_ellipse_size = 0.3; double ellipse_size = 0.0; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size + ); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); ellipse_size = 0.24; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size + ); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); ellipse_size = 0.25; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size + ); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); ellipse_size = 0.29; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size + ); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); ellipse_size = 0.3; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size + ); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } @@ -86,7 +91,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); EXPECT_EQ(merged_stat.message, "OK"); @@ -94,7 +99,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0"); @@ -102,7 +107,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN1"); @@ -110,7 +115,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); @@ -118,7 +123,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR1"); @@ -126,7 +131,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); @@ -134,7 +139,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "ERROR0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); }