Skip to content

Commit f6db96f

Browse files
feat(control_evaluator): add lanelet info to the metrics (autowarefoundation#7765)
* add route handler Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add lanelet info to diagnostic Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add const Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add kinematic state info Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * clean Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove unusde subscriptions Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * clean Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add shoulder lanelets Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * fix includes Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent b1549cc commit f6db96f

File tree

5 files changed

+137
-7
lines changed

5 files changed

+137
-7
lines changed

evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp

+21-2
Original file line numberDiff line numberDiff line change
@@ -17,15 +17,17 @@
1717

1818
#include "autoware/control_evaluator/metrics/deviation_metrics.hpp"
1919

20+
#include <autoware/route_handler/route_handler.hpp>
2021
#include <autoware/universe_utils/ros/polling_subscriber.hpp>
2122
#include <rclcpp/rclcpp.hpp>
2223

24+
#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
25+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
2326
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
2427
#include <nav_msgs/msg/odometry.hpp>
2528

26-
#include <array>
2729
#include <deque>
28-
#include <memory>
30+
#include <optional>
2931
#include <string>
3032
#include <utility>
3133
#include <vector>
@@ -39,6 +41,9 @@ using diagnostic_msgs::msg::DiagnosticStatus;
3941
using geometry_msgs::msg::Point;
4042
using geometry_msgs::msg::Pose;
4143
using nav_msgs::msg::Odometry;
44+
using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;
45+
using autoware_planning_msgs::msg::LaneletRoute;
46+
using geometry_msgs::msg::AccelWithCovarianceStamped;
4247

4348
/**
4449
* @brief Node for control evaluation
@@ -62,6 +67,9 @@ class controlEvaluatorNode : public rclcpp::Node
6267
const DiagnosticArray & diag, const std::string & function_name);
6368

6469
DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag);
70+
DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const;
71+
DiagnosticStatus generateKinematicStateDiagnosticStatus(
72+
const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped);
6573

6674
void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
6775
void onTimer();
@@ -74,11 +82,20 @@ class controlEvaluatorNode : public rclcpp::Node
7482

7583
autoware::universe_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
7684
this, "~/input/odometry"};
85+
autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
86+
this, "/localization/acceleration"};
7787
autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
7888
this, "~/input/trajectory"};
89+
autoware::universe_utils::InterProcessPollingSubscriber<LaneletRoute> route_subscriber_{
90+
this, "~/input/route", rclcpp::QoS{1}.transient_local()};
91+
autoware::universe_utils::InterProcessPollingSubscriber<LaneletMapBin> vector_map_subscriber_{
92+
this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
7993

8094
rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
8195

96+
// update Route Handler
97+
void getRouteData();
98+
8299
// Calculator
83100
// Metrics
84101
std::deque<rclcpp::Time> stamps_;
@@ -87,7 +104,9 @@ class controlEvaluatorNode : public rclcpp::Node
87104
std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
88105
const std::vector<std::string> target_functions_ = {"autonomous_emergency_braking"};
89106

107+
autoware::route_handler::RouteHandler route_handler_;
90108
rclcpp::TimerBase::SharedPtr timer_;
109+
std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
91110
};
92111
} // namespace control_diagnostics
93112

Original file line numberDiff line numberDiff line change
@@ -1,15 +1,21 @@
11
<launch>
22
<arg name="input/diagnostics" default="/diagnostics"/>
33
<arg name="input/odometry" default="/localization/kinematic_state"/>
4+
<arg name="input/acceleration" default="/localization/acceleration"/>
45
<arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
6+
<arg name="map_topic_name" default="/map/vector_map"/>
7+
<arg name="route_topic_name" default="/planning/mission_planning/route"/>
58
<!-- control evaluator -->
69
<group>
710
<node name="control_evaluator" exec="control_evaluator" pkg="autoware_control_evaluator">
811
<param from="$(find-pkg-share autoware_control_evaluator)/param/control_evaluator.defaults.yaml"/>
912
<remap from="~/input/diagnostics" to="$(var input/diagnostics)"/>
1013
<remap from="~/input/odometry" to="$(var input/odometry)"/>
14+
<remap from="~/input/acceleration" to="$(var input/acceleration)"/>
1115
<remap from="~/input/trajectory" to="$(var input/trajectory)"/>
1216
<remap from="~/metrics" to="/diagnostic/control_evaluator/metrics"/>
17+
<remap from="~/input/vector_map" to="$(var map_topic_name)"/>
18+
<remap from="~/input/route" to="$(var route_topic_name)"/>
1319
</node>
1420
</group>
1521
</launch>

evaluator/autoware_control_evaluator/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616

1717
<depend>autoware_motion_utils</depend>
1818
<depend>autoware_planning_msgs</depend>
19+
<depend>autoware_route_handler</depend>
1920
<depend>autoware_universe_utils</depend>
2021
<depend>diagnostic_msgs</depend>
2122
<depend>pluginlib</depend>

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

+106-5
Original file line numberDiff line numberDiff line change
@@ -14,10 +14,11 @@
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_lanelet2_extension/utility/query.hpp>
18+
#include <autoware_lanelet2_extension/utility/utilities.hpp>
19+
20+
#include <algorithm>
21+
#include <limits>
2122
#include <string>
2223
#include <utility>
2324
#include <vector>
@@ -40,6 +41,29 @@ controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_opti
4041
rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this));
4142
}
4243

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+
4367
void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp)
4468
{
4569
constexpr double KEEP_TIME = 1.0;
@@ -102,9 +126,75 @@ DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const Diagnos
102126
diagnostic_msgs::msg::KeyValue key_value;
103127
key_value.key = "decision";
104128
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, &current_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());
106155
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;
107191

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);
108198
return status;
109199
}
110200

@@ -145,6 +235,7 @@ void controlEvaluatorNode::onTimer()
145235
DiagnosticArray metrics_msg;
146236
const auto traj = traj_sub_.takeData();
147237
const auto odom = odometry_sub_.takeData();
238+
const auto acc = accel_sub_.takeData();
148239

149240
// generate decision diagnostics from input diagnostics
150241
for (const auto & function : target_functions_) {
@@ -171,6 +262,16 @@ void controlEvaluatorNode::onTimer()
171262
metrics_msg.status.push_back(generateYawDeviationDiagnosticStatus(*traj, ego_pose));
172263
}
173264

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+
174275
metrics_msg.header.stamp = now();
175276
metrics_pub_->publish(metrics_msg);
176277
}

launch/tier4_control_launch/launch/control.launch.py

+3
Original file line numberDiff line numberDiff line change
@@ -381,8 +381,11 @@ def launch_setup(context, *args, **kwargs):
381381
remappings=[
382382
("~/input/diagnostics", "/diagnostics"),
383383
("~/input/odometry", "/localization/kinematic_state"),
384+
("~/input/acceleration", "/localization/acceleration"),
384385
("~/input/trajectory", "/planning/scenario_planning/trajectory"),
385386
("~/metrics", "/diagnostic/control_evaluator/metrics"),
387+
("~/input/vector_map", "/map/vector_map"),
388+
("~/input/route", "/planning/mission_planning/route"),
386389
],
387390
)
388391

0 commit comments

Comments
 (0)