25
25
26
26
namespace control_diagnostics
27
27
{
28
- controlEvaluatorNode::controlEvaluatorNode (const rclcpp::NodeOptions & node_options)
28
+ ControlEvaluatorNode::ControlEvaluatorNode (const rclcpp::NodeOptions & node_options)
29
29
: Node(" control_evaluator" , node_options)
30
30
{
31
31
using std::placeholders::_1;
32
32
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));
34
34
35
35
// Publisher
36
36
metrics_pub_ = create_publisher<DiagnosticArray>(" ~/metrics" , 1 );
37
37
38
38
// Timer callback to publish evaluator diagnostics
39
39
using namespace std ::literals::chrono_literals;
40
40
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 ));
42
42
}
43
43
44
- void controlEvaluatorNode ::getRouteData ()
44
+ void ControlEvaluatorNode ::getRouteData ()
45
45
{
46
46
// route
47
47
{
@@ -64,7 +64,7 @@ void controlEvaluatorNode::getRouteData()
64
64
}
65
65
}
66
66
67
- void controlEvaluatorNode ::removeOldDiagnostics (const rclcpp::Time & stamp)
67
+ void ControlEvaluatorNode ::removeOldDiagnostics (const rclcpp::Time & stamp)
68
68
{
69
69
constexpr double KEEP_TIME = 1.0 ;
70
70
diag_queue_.erase (
@@ -76,7 +76,7 @@ void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp)
76
76
diag_queue_.end ());
77
77
}
78
78
79
- void controlEvaluatorNode ::removeDiagnosticsByName (const std::string & name)
79
+ void ControlEvaluatorNode ::removeDiagnosticsByName (const std::string & name)
80
80
{
81
81
diag_queue_.erase (
82
82
std::remove_if (
@@ -87,13 +87,13 @@ void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name)
87
87
diag_queue_.end ());
88
88
}
89
89
90
- void controlEvaluatorNode ::addDiagnostic (
90
+ void ControlEvaluatorNode ::addDiagnostic (
91
91
const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp)
92
92
{
93
93
diag_queue_.push_back (std::make_pair (diag, stamp));
94
94
}
95
95
96
- void controlEvaluatorNode ::updateDiagnosticQueue (
96
+ void ControlEvaluatorNode ::updateDiagnosticQueue (
97
97
const DiagnosticArray & input_diagnostics, const std::string & function,
98
98
const rclcpp::Time & stamp)
99
99
{
@@ -110,15 +110,15 @@ void controlEvaluatorNode::updateDiagnosticQueue(
110
110
removeOldDiagnostics (stamp);
111
111
}
112
112
113
- void controlEvaluatorNode ::onDiagnostics (const DiagnosticArray::ConstSharedPtr diag_msg)
113
+ void ControlEvaluatorNode ::onDiagnostics (const DiagnosticArray::ConstSharedPtr diag_msg)
114
114
{
115
115
// add target diagnostics to the queue and remove old ones
116
116
for (const auto & function : target_functions_) {
117
117
updateDiagnosticQueue (*diag_msg, function, now ());
118
118
}
119
119
}
120
120
121
- DiagnosticStatus controlEvaluatorNode ::generateAEBDiagnosticStatus (const DiagnosticStatus & diag)
121
+ DiagnosticStatus ControlEvaluatorNode ::generateAEBDiagnosticStatus (const DiagnosticStatus & diag)
122
122
{
123
123
DiagnosticStatus status;
124
124
status.level = status.OK ;
@@ -131,7 +131,7 @@ DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const Diagnos
131
131
return status;
132
132
}
133
133
134
- DiagnosticStatus controlEvaluatorNode ::generateLaneletDiagnosticStatus (const Pose & ego_pose) const
134
+ DiagnosticStatus ControlEvaluatorNode ::generateLaneletDiagnosticStatus (const Pose & ego_pose) const
135
135
{
136
136
const auto current_lanelets = [&]() {
137
137
lanelet::ConstLanelet closest_route_lanelet;
@@ -162,7 +162,7 @@ DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pos
162
162
return status;
163
163
}
164
164
165
- DiagnosticStatus controlEvaluatorNode ::generateKinematicStateDiagnosticStatus (
165
+ DiagnosticStatus ControlEvaluatorNode ::generateKinematicStateDiagnosticStatus (
166
166
const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped)
167
167
{
168
168
DiagnosticStatus status;
@@ -198,7 +198,7 @@ DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus(
198
198
return status;
199
199
}
200
200
201
- DiagnosticStatus controlEvaluatorNode ::generateLateralDeviationDiagnosticStatus (
201
+ DiagnosticStatus ControlEvaluatorNode ::generateLateralDeviationDiagnosticStatus (
202
202
const Trajectory & traj, const Point & ego_point)
203
203
{
204
204
const double lateral_deviation = metrics::calcLateralDeviation (traj, ego_point);
@@ -214,7 +214,7 @@ DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus(
214
214
return status;
215
215
}
216
216
217
- DiagnosticStatus controlEvaluatorNode ::generateYawDeviationDiagnosticStatus (
217
+ DiagnosticStatus ControlEvaluatorNode ::generateYawDeviationDiagnosticStatus (
218
218
const Trajectory & traj, const Pose & ego_pose)
219
219
{
220
220
const double yaw_deviation = metrics::calcYawDeviation (traj, ego_pose);
@@ -230,7 +230,7 @@ DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus(
230
230
return status;
231
231
}
232
232
233
- void controlEvaluatorNode ::onTimer ()
233
+ void ControlEvaluatorNode ::onTimer ()
234
234
{
235
235
DiagnosticArray metrics_msg;
236
236
const auto traj = traj_sub_.takeData ();
@@ -278,4 +278,4 @@ void controlEvaluatorNode::onTimer()
278
278
} // namespace control_diagnostics
279
279
280
280
#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