2525
2626namespace control_diagnostics
2727{
28- controlEvaluatorNode::controlEvaluatorNode (const rclcpp::NodeOptions & node_options)
28+ ControlEvaluatorNode::ControlEvaluatorNode (const rclcpp::NodeOptions & node_options)
2929: Node(" control_evaluator"
3030{
3131 using std::placeholders::_1;
3232 control_diag_sub_ = create_subscription<DiagnosticArray>(
33- " ~/input/diagnostics" 1 , std::bind (&controlEvaluatorNode ::onDiagnostics, this , _1));
33+ " ~/input/diagnostics" 1 , std::bind (&ControlEvaluatorNode ::onDiagnostics, this , _1));
3434
3535 // Publisher
3636 metrics_pub_ = create_publisher<DiagnosticArray>(" ~/metrics" 1 );
3737
3838 // Timer callback to publish evaluator diagnostics
3939 using namespace std ::literals::chrono_literals;
4040 timer_ =
41- rclcpp::create_timer (this , get_clock (), 100ms, std::bind (&controlEvaluatorNode ::onTimer, this ));
41+ rclcpp::create_timer (this , get_clock (), 100ms, std::bind (&ControlEvaluatorNode ::onTimer, this ));
4242}
4343
44- void controlEvaluatorNode ::getRouteData
44+ void ControlEvaluatorNode ::getRouteData
4545{
4646 // route
4747 {
@@ -64,7 +64,7 @@ void controlEvaluatorNode::getRouteData()
6464 }
6565}
6666
67- void controlEvaluatorNode ::removeOldDiagnosticsconst rclcpp::Time & stamp)
67+ void ControlEvaluatorNode ::removeOldDiagnosticsconst rclcpp::Time & stamp)
6868{
6969 constexpr double KEEP_TIME = 1.0 ;
7070 diag_queue_.erase (
@@ -76,7 +76,7 @@ void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp)
7676 diag_queue_.end ());
7777}
7878
79- void controlEvaluatorNode ::removeDiagnosticsByNameconst std::string & name)
79+ void ControlEvaluatorNode ::removeDiagnosticsByNameconst std::string & name)
8080{
8181 diag_queue_.erase (
8282 std::remove_if (
@@ -87,13 +87,13 @@ void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name)
8787 diag_queue_.end ());
8888}
8989
90- void controlEvaluatorNode ::addDiagnostic
90+ void ControlEvaluatorNode ::addDiagnostic
9191 const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp)
9292{
9393 diag_queue_.push_back (std::make_pair (diag, stamp));
9494}
9595
96- void controlEvaluatorNode ::updateDiagnosticQueue
96+ void ControlEvaluatorNode ::updateDiagnosticQueue
9797 const DiagnosticArray & input_diagnostics, const std::string & function,
9898 const rclcpp::Time & stamp)
9999{
@@ -110,15 +110,15 @@ void controlEvaluatorNode::updateDiagnosticQueue(
110110 removeOldDiagnostics (stamp);
111111}
112112
113- void controlEvaluatorNode ::onDiagnosticsconst DiagnosticArray::ConstSharedPtr diag_msg)
113+ void ControlEvaluatorNode ::onDiagnosticsconst DiagnosticArray::ConstSharedPtr diag_msg)
114114{
115115 // add target diagnostics to the queue and remove old ones
116116 for (const auto & function : target_functions_) {
117117 updateDiagnosticQueue (*diag_msg, function, now ());
118118 }
119119}
120120
121- DiagnosticStatus controlEvaluatorNode ::generateAEBDiagnosticStatusconst DiagnosticStatus & diag)
121+ DiagnosticStatus ControlEvaluatorNode ::generateAEBDiagnosticStatusconst DiagnosticStatus & diag)
122122{
123123 DiagnosticStatus status;
124124 status.level = status.OK ;
@@ -131,7 +131,7 @@ DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const Diagnos
131131 return status;
132132}
133133
134- DiagnosticStatus controlEvaluatorNode ::generateLaneletDiagnosticStatusconst Pose & ego_pose) const
134+ DiagnosticStatus ControlEvaluatorNode ::generateLaneletDiagnosticStatusconst Pose & ego_pose) const
135135{
136136 const auto current_lanelets = [&]() {
137137 lanelet::ConstLanelet closest_route_lanelet;
@@ -162,7 +162,7 @@ DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pos
162162 return status;
163163}
164164
165- DiagnosticStatus controlEvaluatorNode ::generateKinematicStateDiagnosticStatus
165+ DiagnosticStatus ControlEvaluatorNode ::generateKinematicStateDiagnosticStatus
166166 const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped)
167167{
168168 DiagnosticStatus status;
@@ -198,7 +198,7 @@ DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus(
198198 return status;
199199}
200200
201- DiagnosticStatus controlEvaluatorNode ::generateLateralDeviationDiagnosticStatus
201+ DiagnosticStatus ControlEvaluatorNode ::generateLateralDeviationDiagnosticStatus
202202 const Trajectory & traj, const Point & ego_point)
203203{
204204 const double lateral_deviation = metrics::calcLateralDeviation (traj, ego_point);
@@ -214,7 +214,7 @@ DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus(
214214 return status;
215215}
216216
217- DiagnosticStatus controlEvaluatorNode ::generateYawDeviationDiagnosticStatus
217+ DiagnosticStatus ControlEvaluatorNode ::generateYawDeviationDiagnosticStatus
218218 const Trajectory & traj, const Pose & ego_pose)
219219{
220220 const double yaw_deviation = metrics::calcYawDeviation (traj, ego_pose);
@@ -230,7 +230,7 @@ DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus(
230230 return status;
231231}
232232
233- void controlEvaluatorNode ::onTimer
233+ void ControlEvaluatorNode ::onTimer
234234{
235235 DiagnosticArray metrics_msg;
236236 const auto traj = traj_sub_.takeData ();
@@ -278,4 +278,4 @@ void controlEvaluatorNode::onTimer()
278278} // namespace control_diagnostics
279279
280280#include " rclcpp_components/register_node_macro.hpp"
281- RCLCPP_COMPONENTS_REGISTER_NODE (control_diagnostics::controlEvaluatorNode )
281+ RCLCPP_COMPONENTS_REGISTER_NODE (control_diagnostics::ControlEvaluatorNode )
0 commit comments