14
14
15
15
#include " autoware/control_evaluator/control_evaluator_node.hpp"
16
16
17
- #include < fstream>
18
- #include < iostream>
19
- #include < map>
20
- #include < memory>
17
+ #include < autoware_lanelet2_extension/utility/query.hpp>
18
+ #include < autoware_lanelet2_extension/utility/utilities.hpp>
19
+
20
+ #include < algorithm>
21
+ #include < limits>
21
22
#include < string>
22
23
#include < utility>
23
24
#include < vector>
@@ -40,6 +41,29 @@ controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_opti
40
41
rclcpp::create_timer (this , get_clock (), 100ms, std::bind (&controlEvaluatorNode::onTimer, this ));
41
42
}
42
43
44
+ void controlEvaluatorNode::getRouteData ()
45
+ {
46
+ // route
47
+ {
48
+ const auto msg = route_subscriber_.takeNewData ();
49
+ if (msg) {
50
+ if (msg->segments .empty ()) {
51
+ RCLCPP_ERROR (get_logger (), " input route is empty. ignored" );
52
+ } else {
53
+ route_handler_.setRoute (*msg);
54
+ }
55
+ }
56
+ }
57
+
58
+ // map
59
+ {
60
+ const auto msg = vector_map_subscriber_.takeNewData ();
61
+ if (msg) {
62
+ route_handler_.setMap (*msg);
63
+ }
64
+ }
65
+ }
66
+
43
67
void controlEvaluatorNode::removeOldDiagnostics (const rclcpp::Time & stamp)
44
68
{
45
69
constexpr double KEEP_TIME = 1.0 ;
@@ -102,9 +126,75 @@ DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const Diagnos
102
126
diagnostic_msgs::msg::KeyValue key_value;
103
127
key_value.key = " decision" ;
104
128
const bool is_emergency_brake = (diag.level == DiagnosticStatus::ERROR);
105
- key_value.value = (is_emergency_brake) ? " stop" : " none" ;
129
+ key_value.value = (is_emergency_brake) ? " deceleration" : " none" ;
130
+ status.values .push_back (key_value);
131
+ return status;
132
+ }
133
+
134
+ DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus (const Pose & ego_pose) const
135
+ {
136
+ const auto current_lanelets = [&]() {
137
+ lanelet::ConstLanelet closest_route_lanelet;
138
+ route_handler_.getClosestLaneletWithinRoute (ego_pose, &closest_route_lanelet);
139
+ const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose (ego_pose);
140
+ lanelet::ConstLanelets closest_lanelets{closest_route_lanelet};
141
+ closest_lanelets.insert (
142
+ closest_lanelets.end (), shoulder_lanelets.begin (), shoulder_lanelets.end ());
143
+ return closest_lanelets;
144
+ }();
145
+ const auto arc_coordinates = lanelet::utils::getArcCoordinates (current_lanelets, ego_pose);
146
+ lanelet::ConstLanelet current_lane;
147
+ lanelet::utils::query::getClosestLanelet (current_lanelets, ego_pose, ¤t_lane);
148
+
149
+ DiagnosticStatus status;
150
+ status.name = " ego_lane_info" ;
151
+ status.level = status.OK ;
152
+ diagnostic_msgs::msg::KeyValue key_value;
153
+ key_value.key = " lane_id" ;
154
+ key_value.value = std::to_string (current_lane.id ());
106
155
status.values .push_back (key_value);
156
+ key_value.key = " s" ;
157
+ key_value.value = std::to_string (arc_coordinates.length );
158
+ status.values .push_back (key_value);
159
+ key_value.key = " t" ;
160
+ key_value.value = std::to_string (arc_coordinates.distance );
161
+ status.values .push_back (key_value);
162
+ return status;
163
+ }
164
+
165
+ DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus (
166
+ const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped)
167
+ {
168
+ DiagnosticStatus status;
169
+ status.name = " kinematic_state" ;
170
+ status.level = status.OK ;
171
+ diagnostic_msgs::msg::KeyValue key_value;
172
+ key_value.key = " vel" ;
173
+ key_value.value = std::to_string (odom.twist .twist .linear .x );
174
+ status.values .push_back (key_value);
175
+ key_value.key = " acc" ;
176
+ const auto & acc = accel_stamped.accel .accel .linear .x ;
177
+ key_value.value = std::to_string (acc);
178
+ status.values .push_back (key_value);
179
+ key_value.key = " jerk" ;
180
+ const auto jerk = [&]() {
181
+ if (!prev_acc_stamped_.has_value ()) {
182
+ prev_acc_stamped_ = accel_stamped;
183
+ return 0.0 ;
184
+ }
185
+ const auto t = static_cast <double >(accel_stamped.header .stamp .sec ) +
186
+ static_cast <double >(accel_stamped.header .stamp .nanosec ) * 1e-9 ;
187
+ const auto prev_t = static_cast <double >(prev_acc_stamped_.value ().header .stamp .sec ) +
188
+ static_cast <double >(prev_acc_stamped_.value ().header .stamp .nanosec ) * 1e-9 ;
189
+ const auto dt = t - prev_t ;
190
+ if (dt < std::numeric_limits<double >::epsilon ()) return 0.0 ;
107
191
192
+ const auto prev_acc = prev_acc_stamped_.value ().accel .accel .linear .x ;
193
+ prev_acc_stamped_ = accel_stamped;
194
+ return (acc - prev_acc) / dt;
195
+ }();
196
+ key_value.value = std::to_string (jerk);
197
+ status.values .push_back (key_value);
108
198
return status;
109
199
}
110
200
@@ -145,6 +235,7 @@ void controlEvaluatorNode::onTimer()
145
235
DiagnosticArray metrics_msg;
146
236
const auto traj = traj_sub_.takeData ();
147
237
const auto odom = odometry_sub_.takeData ();
238
+ const auto acc = accel_sub_.takeData ();
148
239
149
240
// generate decision diagnostics from input diagnostics
150
241
for (const auto & function : target_functions_) {
@@ -171,6 +262,16 @@ void controlEvaluatorNode::onTimer()
171
262
metrics_msg.status .push_back (generateYawDeviationDiagnosticStatus (*traj, ego_pose));
172
263
}
173
264
265
+ getRouteData ();
266
+ if (odom && route_handler_.isHandlerReady ()) {
267
+ const Pose ego_pose = odom->pose .pose ;
268
+ metrics_msg.status .push_back (generateLaneletDiagnosticStatus (ego_pose));
269
+ }
270
+
271
+ if (odom && acc) {
272
+ metrics_msg.status .push_back (generateKinematicStateDiagnosticStatus (*odom, *acc));
273
+ }
274
+
174
275
metrics_msg.header .stamp = now ();
175
276
metrics_pub_->publish (metrics_msg);
176
277
}
0 commit comments