1414
1515#include " autoware/control_evaluator/control_evaluator_node.hpp"
1616
17- #include < fstream>
18- #include < iostream>
19- #include < map>
20- #include < memory>
17+ #include " autoware/evaluator_utils/evaluator_utils.hpp"
18+
19+ #include < lanelet2_extension/utility/query.hpp>
20+ #include < lanelet2_extension/utility/utilities.hpp>
21+
22+ #include < algorithm>
23+ #include < limits>
2124#include < string>
2225#include < utility>
2326#include < vector>
2427
2528namespace control_diagnostics
2629{
27- controlEvaluatorNode::controlEvaluatorNode (const rclcpp::NodeOptions & node_options)
30+ ControlEvaluatorNode::ControlEvaluatorNode (const rclcpp::NodeOptions & node_options)
2831: Node(" control_evaluator"
2932{
3033 using std::placeholders::_1;
3134 control_diag_sub_ = create_subscription<DiagnosticArray>(
32- " ~/input/diagnostics" 1 , std::bind (&controlEvaluatorNode ::onDiagnostics, this , _1));
35+ " ~/input/diagnostics" 1 , std::bind (&ControlEvaluatorNode ::onDiagnostics, this , _1));
3336
3437 // Publisher
3538 metrics_pub_ = create_publisher<DiagnosticArray>(" ~/metrics" 1 );
3639
3740 // Timer callback to publish evaluator diagnostics
3841 using namespace std ::literals::chrono_literals;
3942 timer_ =
40- rclcpp::create_timer (this , get_clock (), 100ms, std::bind (&controlEvaluatorNode::onTimer, this ));
41- }
42-
43- void controlEvaluatorNode::removeOldDiagnostics (const rclcpp::Time & stamp)
44- {
45- constexpr double KEEP_TIME = 1.0 ;
46- diag_queue_.erase (
47- std::remove_if (
48- diag_queue_.begin (), diag_queue_.end (),
49- [stamp](const std::pair<diagnostic_msgs::msg::DiagnosticStatus, rclcpp::Time> & p) {
50- return (stamp - p.second ).seconds () > KEEP_TIME;
51- }),
52- diag_queue_.end ());
53- }
54-
55- void controlEvaluatorNode::removeDiagnosticsByName (const std::string & name)
56- {
57- diag_queue_.erase (
58- std::remove_if (
59- diag_queue_.begin (), diag_queue_.end (),
60- [&name](const std::pair<diagnostic_msgs::msg::DiagnosticStatus, rclcpp::Time> & p) {
61- return p.first .name .find (name) != std::string::npos;
62- }),
63- diag_queue_.end ());
43+ rclcpp::create_timer (this , get_clock (), 100ms, std::bind (&ControlEvaluatorNode::onTimer, this ));
6444}
6545
66- void controlEvaluatorNode::addDiagnostic (
67- const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp)
46+ void ControlEvaluatorNode::getRouteData ()
6847{
69- diag_queue_.push_back (std::make_pair (diag, stamp));
70- }
71-
72- void controlEvaluatorNode::updateDiagnosticQueue (
73- const DiagnosticArray & input_diagnostics, const std::string & function,
74- const rclcpp::Time & stamp)
75- {
76- const auto it = std::find_if (
77- input_diagnostics.status .begin (), input_diagnostics.status .end (),
78- [&function](const diagnostic_msgs::msg::DiagnosticStatus & diag) {
79- return diag.name .find (function) != std::string::npos;
80- });
81- if (it != input_diagnostics.status .end ()) {
82- removeDiagnosticsByName (it->name );
83- addDiagnostic (*it, input_diagnostics.header .stamp );
48+ // route
49+ {
50+ const auto msg = route_subscriber_.takeNewData ();
51+ if (msg) {
52+ if (msg->segments .empty ()) {
53+ RCLCPP_ERROR (get_logger (), " input route is empty. ignored"
54+ } else {
55+ route_handler_.setRoute (*msg);
56+ }
57+ }
8458 }
8559
86- removeOldDiagnostics (stamp);
60+ // map
61+ {
62+ const auto msg = vector_map_subscriber_.takeNewData ();
63+ if (msg) {
64+ route_handler_.setMap (*msg);
65+ }
66+ }
8767}
8868
89- void controlEvaluatorNode ::onDiagnosticsconst DiagnosticArray::ConstSharedPtr diag_msg)
69+ void ControlEvaluatorNode ::onDiagnosticsconst DiagnosticArray::ConstSharedPtr diag_msg)
9070{
9171 // add target diagnostics to the queue and remove old ones
9272 for (const auto & function : target_functions_) {
93- updateDiagnosticQueue (*diag_msg, function, now ());
73+ autoware::evaluator_utils:: updateDiagnosticQueuenow (), diag_queue_ );
9474 }
9575}
9676
97- DiagnosticStatus controlEvaluatorNode ::generateAEBDiagnosticStatusconst DiagnosticStatus & diag)
77+ DiagnosticStatus ControlEvaluatorNode ::generateAEBDiagnosticStatusconst DiagnosticStatus & diag)
9878{
9979 DiagnosticStatus status;
10080 status.level = status.OK ;
10181 status.name = diag.name ;
10282 diagnostic_msgs::msg::KeyValue key_value;
10383 key_value.key = " decision"
10484 const bool is_emergency_brake = (diag.level == DiagnosticStatus::ERROR);
105- key_value.value = (is_emergency_brake) ? " stop " " none"
85+ key_value.value = (is_emergency_brake) ? " deceleration " " none"
10686 status.values .push_back (key_value);
87+ return status;
88+ }
10789
90+ DiagnosticStatus ControlEvaluatorNode::generateLaneletDiagnosticStatus (const Pose & ego_pose) const
91+ {
92+ const auto current_lanelets = [&]() {
93+ lanelet::ConstLanelet closest_route_lanelet;
94+ route_handler_.getClosestLaneletWithinRoute (ego_pose, &closest_route_lanelet);
95+ const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose (ego_pose);
96+ lanelet::ConstLanelets closest_lanelets{closest_route_lanelet};
97+ closest_lanelets.insert (
98+ closest_lanelets.end (), shoulder_lanelets.begin (), shoulder_lanelets.end ());
99+ return closest_lanelets;
100+ }();
101+ const auto arc_coordinates = lanelet::utils::getArcCoordinates (current_lanelets, ego_pose);
102+ lanelet::ConstLanelet current_lane;
103+ lanelet::utils::query::getClosestLanelet (current_lanelets, ego_pose, ¤t_lane);
104+
105+ DiagnosticStatus status;
106+ status.name = " ego_lane_info"
107+ status.level = status.OK ;
108+ diagnostic_msgs::msg::KeyValue key_value;
109+ key_value.key = " lane_id"
110+ key_value.value = std::to_string (current_lane.id ());
111+ status.values .push_back (key_value);
112+ key_value.key = " s"
113+ key_value.value = std::to_string (arc_coordinates.length );
114+ status.values .push_back (key_value);
115+ key_value.key = " t"
116+ key_value.value = std::to_string (arc_coordinates.distance );
117+ status.values .push_back (key_value);
108118 return status;
109119}
110120
111- DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus (
121+ DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus (
122+ const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped)
123+ {
124+ DiagnosticStatus status;
125+ status.name = " kinematic_state"
126+ status.level = status.OK ;
127+ diagnostic_msgs::msg::KeyValue key_value;
128+ key_value.key = " vel"
129+ key_value.value = std::to_string (odom.twist .twist .linear .x );
130+ status.values .push_back (key_value);
131+ key_value.key = " acc"
132+ const auto & acc = accel_stamped.accel .accel .linear .x ;
133+ key_value.value = std::to_string (acc);
134+ status.values .push_back (key_value);
135+ key_value.key = " jerk"
136+ const auto jerk = [&]() {
137+ if (!prev_acc_stamped_.has_value ()) {
138+ prev_acc_stamped_ = accel_stamped;
139+ return 0.0 ;
140+ }
141+ const auto t = static_cast <double >(accel_stamped.header .stamp .sec ) +
142+ static_cast <double >(accel_stamped.header .stamp .nanosec ) * 1e-9 ;
143+ const auto prev_t = static_cast <double >(prev_acc_stamped_.value ().header .stamp .sec ) +
144+ static_cast <double >(prev_acc_stamped_.value ().header .stamp .nanosec ) * 1e-9 ;
145+ const auto dt = t - prev_t ;
146+ if (dt < std::numeric_limits<double >::epsilon ()) return 0.0 ;
147+
148+ const auto prev_acc = prev_acc_stamped_.value ().accel .accel .linear .x ;
149+ prev_acc_stamped_ = accel_stamped;
150+ return (acc - prev_acc) / dt;
151+ }();
152+ key_value.value = std::to_string (jerk);
153+ status.values .push_back (key_value);
154+ return status;
155+ }
156+
157+ DiagnosticStatus ControlEvaluatorNode::generateLateralDeviationDiagnosticStatus (
112158 const Trajectory & traj, const Point & ego_point)
113159{
114160 const double lateral_deviation = metrics::calcLateralDeviation (traj, ego_point);
@@ -124,7 +170,7 @@ DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus(
124170 return status;
125171}
126172
127- DiagnosticStatus controlEvaluatorNode ::generateYawDeviationDiagnosticStatus
173+ DiagnosticStatus ControlEvaluatorNode ::generateYawDeviationDiagnosticStatus
128174 const Trajectory & traj, const Pose & ego_pose)
129175{
130176 const double yaw_deviation = metrics::calcYawDeviation (traj, ego_pose);
@@ -140,11 +186,59 @@ DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus(
140186 return status;
141187}
142188
143- void controlEvaluatorNode::onTimer ()
189+ DiagnosticStatus ControlEvaluatorNode::generateGoalLongitudinalDeviationDiagnosticStatus (
190+ const Pose & ego_pose)
191+ {
192+ DiagnosticStatus status;
193+ const double longitudinal_deviation =
194+ metrics::calcLongitudinalDeviation (route_handler_.getGoalPose (), ego_pose.position );
195+
196+ status.level = status.OK ;
197+ status.name = " goal_longitudinal_deviation"
198+ diagnostic_msgs::msg::KeyValue key_value;
199+ key_value.key = " metrics_value"
200+ key_value.value = std::to_string (longitudinal_deviation);
201+ status.values .push_back (key_value);
202+ return status;
203+ }
204+
205+ DiagnosticStatus ControlEvaluatorNode::generateGoalLateralDeviationDiagnosticStatus (
206+ const Pose & ego_pose)
207+ {
208+ DiagnosticStatus status;
209+ const double lateral_deviation =
210+ metrics::calcLateralDeviation (route_handler_.getGoalPose (), ego_pose.position );
211+
212+ status.level = status.OK ;
213+ status.name = " goal_lateral_deviation"
214+ diagnostic_msgs::msg::KeyValue key_value;
215+ key_value.key = " metrics_value"
216+ key_value.value = std::to_string (lateral_deviation);
217+ status.values .push_back (key_value);
218+ return status;
219+ }
220+
221+ DiagnosticStatus ControlEvaluatorNode::generateGoalYawDeviationDiagnosticStatus (
222+ const Pose & ego_pose)
223+ {
224+ DiagnosticStatus status;
225+ const double yaw_deviation = metrics::calcYawDeviation (route_handler_.getGoalPose (), ego_pose);
226+
227+ status.level = status.OK ;
228+ status.name = " goal_yaw_deviation"
229+ diagnostic_msgs::msg::KeyValue key_value;
230+ key_value.key = " metrics_value"
231+ key_value.value = std::to_string (yaw_deviation);
232+ status.values .push_back (key_value);
233+ return status;
234+ }
235+
236+ void ControlEvaluatorNode::onTimer ()
144237{
145238 DiagnosticArray metrics_msg;
146239 const auto traj = traj_sub_.takeData ();
147240 const auto odom = odometry_sub_.takeData ();
241+ const auto acc = accel_sub_.takeData ();
148242
149243 // generate decision diagnostics from input diagnostics
150244 for (const auto & function : target_functions_) {
@@ -171,10 +265,24 @@ void controlEvaluatorNode::onTimer()
171265 metrics_msg.status .push_back (generateYawDeviationDiagnosticStatus (*traj, ego_pose));
172266 }
173267
268+ getRouteData ();
269+ if (odom && route_handler_.isHandlerReady ()) {
270+ const Pose ego_pose = odom->pose .pose ;
271+ metrics_msg.status .push_back (generateLaneletDiagnosticStatus (ego_pose));
272+
273+ metrics_msg.status .push_back (generateGoalLongitudinalDeviationDiagnosticStatus (ego_pose));
274+ metrics_msg.status .push_back (generateGoalLateralDeviationDiagnosticStatus (ego_pose));
275+ metrics_msg.status .push_back (generateGoalYawDeviationDiagnosticStatus (ego_pose));
276+ }
277+
278+ if (odom && acc) {
279+ metrics_msg.status .push_back (generateKinematicStateDiagnosticStatus (*odom, *acc));
280+ }
281+
174282 metrics_msg.header .stamp = now ();
175283 metrics_pub_->publish (metrics_msg);
176284}
177285} // namespace control_diagnostics
178286
179287#include " rclcpp_components/register_node_macro.hpp"
180- RCLCPP_COMPONENTS_REGISTER_NODE (control_diagnostics::controlEvaluatorNode )
288+ RCLCPP_COMPONENTS_REGISTER_NODE (control_diagnostics::ControlEvaluatorNode )
0 commit comments