From ce099071716d57abb19d7eb10c6e0b4e65bbe0db Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 2 Jul 2024 18:30:09 +0900 Subject: [PATCH] refactor(control_evaluator): use class naming standard and use remapped param name (#7782) use class naming standard and use remapped param name Signed-off-by: Daniel Sanchez --- .../autoware_control_evaluator/CMakeLists.txt | 2 +- .../control_evaluator_node.hpp | 6 ++-- .../src/control_evaluator_node.cpp | 32 +++++++++---------- .../launch/control.launch.py | 2 +- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/evaluator/autoware_control_evaluator/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt index 189745349a592..2e6f9851cafce 100644 --- a/evaluator/autoware_control_evaluator/CMakeLists.txt +++ b/evaluator/autoware_control_evaluator/CMakeLists.txt @@ -12,7 +12,7 @@ ament_auto_add_library(control_evaluator_node SHARED ) rclcpp_components_register_node(control_evaluator_node - PLUGIN "control_diagnostics::controlEvaluatorNode" + PLUGIN "control_diagnostics::ControlEvaluatorNode" EXECUTABLE control_evaluator ) diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 749ed2296313a..da60c820c45b1 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -48,10 +48,10 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; /** * @brief Node for control evaluation */ -class controlEvaluatorNode : public rclcpp::Node +class ControlEvaluatorNode : public rclcpp::Node { public: - explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options); + explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); void removeOldDiagnostics(const rclcpp::Time & stamp); void removeDiagnosticsByName(const std::string & name); void addDiagnostic(const DiagnosticStatus & diag, const rclcpp::Time & stamp); @@ -83,7 +83,7 @@ class controlEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ this, "~/input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ - this, "/localization/acceleration"}; + this, "~/input/acceleration"}; autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 1343137e54bb0..5862522d582f7 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -25,12 +25,12 @@ namespace control_diagnostics { -controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_options) +ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_options) : Node("control_evaluator", node_options) { using std::placeholders::_1; control_diag_sub_ = create_subscription( - "~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1)); + "~/input/diagnostics", 1, std::bind(&ControlEvaluatorNode::onDiagnostics, this, _1)); // Publisher metrics_pub_ = create_publisher("~/metrics", 1); @@ -38,10 +38,10 @@ controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_opti // Timer callback to publish evaluator diagnostics using namespace std::literals::chrono_literals; timer_ = - rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this)); + rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ControlEvaluatorNode::onTimer, this)); } -void controlEvaluatorNode::getRouteData() +void ControlEvaluatorNode::getRouteData() { // route { @@ -64,7 +64,7 @@ void controlEvaluatorNode::getRouteData() } } -void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) +void ControlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) { constexpr double KEEP_TIME = 1.0; diag_queue_.erase( @@ -76,7 +76,7 @@ void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) diag_queue_.end()); } -void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name) +void ControlEvaluatorNode::removeDiagnosticsByName(const std::string & name) { diag_queue_.erase( std::remove_if( @@ -87,13 +87,13 @@ void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name) diag_queue_.end()); } -void controlEvaluatorNode::addDiagnostic( +void ControlEvaluatorNode::addDiagnostic( const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp) { diag_queue_.push_back(std::make_pair(diag, stamp)); } -void controlEvaluatorNode::updateDiagnosticQueue( +void ControlEvaluatorNode::updateDiagnosticQueue( const DiagnosticArray & input_diagnostics, const std::string & function, const rclcpp::Time & stamp) { @@ -110,7 +110,7 @@ void controlEvaluatorNode::updateDiagnosticQueue( removeOldDiagnostics(stamp); } -void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) +void ControlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) { // add target diagnostics to the queue and remove old ones for (const auto & function : target_functions_) { @@ -118,7 +118,7 @@ void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr d } } -DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag) +DiagnosticStatus ControlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag) { DiagnosticStatus status; status.level = status.OK; @@ -131,7 +131,7 @@ DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const Diagnos return status; } -DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pose & ego_pose) const +DiagnosticStatus ControlEvaluatorNode::generateLaneletDiagnosticStatus(const Pose & ego_pose) const { const auto current_lanelets = [&]() { lanelet::ConstLanelet closest_route_lanelet; @@ -162,7 +162,7 @@ DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pos return status; } -DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus( +DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus( const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped) { DiagnosticStatus status; @@ -198,7 +198,7 @@ DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus( return status; } -DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus( +DiagnosticStatus ControlEvaluatorNode::generateLateralDeviationDiagnosticStatus( const Trajectory & traj, const Point & ego_point) { const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point); @@ -214,7 +214,7 @@ DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus( return status; } -DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus( +DiagnosticStatus ControlEvaluatorNode::generateYawDeviationDiagnosticStatus( const Trajectory & traj, const Pose & ego_pose) { const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose); @@ -230,7 +230,7 @@ DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus( return status; } -void controlEvaluatorNode::onTimer() +void ControlEvaluatorNode::onTimer() { DiagnosticArray metrics_msg; const auto traj = traj_sub_.takeData(); @@ -278,4 +278,4 @@ void controlEvaluatorNode::onTimer() } // namespace control_diagnostics #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::controlEvaluatorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::ControlEvaluatorNode) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index b54714fecc2a5..5843ec98d7068 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -376,7 +376,7 @@ def launch_setup(context, *args, **kwargs): # control evaluator control_evaluator_component = ComposableNode( package="autoware_control_evaluator", - plugin="control_diagnostics::controlEvaluatorNode", + plugin="control_diagnostics::ControlEvaluatorNode", name="control_evaluator", remappings=[ ("~/input/diagnostics", "/diagnostics"),