From f6db96f40250209387ac1d8846a96b8369c44b6a Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 2 Jul 2024 11:03:21 +0900 Subject: [PATCH] feat(control_evaluator): add lanelet info to the metrics (#7765) * add route handler Signed-off-by: Daniel Sanchez * add lanelet info to diagnostic Signed-off-by: Daniel Sanchez * add const Signed-off-by: Daniel Sanchez * add kinematic state info Signed-off-by: Daniel Sanchez * clean Signed-off-by: Daniel Sanchez * remove unusde subscriptions Signed-off-by: Daniel Sanchez * clean Signed-off-by: Daniel Sanchez * add shoulder lanelets Signed-off-by: Daniel Sanchez * fix includes Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../control_evaluator_node.hpp | 23 +++- .../launch/control_evaluator.launch.xml | 6 + .../autoware_control_evaluator/package.xml | 1 + .../src/control_evaluator_node.cpp | 111 +++++++++++++++++- .../launch/control.launch.py | 3 + 5 files changed, 137 insertions(+), 7 deletions(-) diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 279bada80e1b9..749ed2296313a 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -17,15 +17,17 @@ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" +#include #include #include +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include #include #include -#include #include -#include +#include #include #include #include @@ -39,6 +41,9 @@ using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; +using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletRoute; +using geometry_msgs::msg::AccelWithCovarianceStamped; /** * @brief Node for control evaluation @@ -62,6 +67,9 @@ class controlEvaluatorNode : public rclcpp::Node const DiagnosticArray & diag, const std::string & function_name); DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag); + DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const; + DiagnosticStatus generateKinematicStateDiagnosticStatus( + const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped); void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); void onTimer(); @@ -74,11 +82,20 @@ class controlEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ this, "~/input/odometry"}; + autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ + this, "/localization/acceleration"}; autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ + this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ + this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; rclcpp::Publisher::SharedPtr metrics_pub_; + // update Route Handler + void getRouteData(); + // Calculator // Metrics std::deque stamps_; @@ -87,7 +104,9 @@ class controlEvaluatorNode : public rclcpp::Node std::deque> diag_queue_; const std::vector target_functions_ = {"autonomous_emergency_braking"}; + autoware::route_handler::RouteHandler route_handler_; rclcpp::TimerBase::SharedPtr timer_; + std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace control_diagnostics diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index e119f1787d21b..84e11208770b8 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -1,15 +1,21 @@ + + + + + + diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 49631c566eaf5..21f48b1c64485 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -16,6 +16,7 @@ autoware_motion_utils autoware_planning_msgs + autoware_route_handler autoware_universe_utils diagnostic_msgs pluginlib diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index ce749e6e8d5eb..1343137e54bb0 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -14,10 +14,11 @@ #include "autoware/control_evaluator/control_evaluator_node.hpp" -#include -#include -#include -#include +#include +#include + +#include +#include #include #include #include @@ -40,6 +41,29 @@ controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_opti rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this)); } +void controlEvaluatorNode::getRouteData() +{ + // route + { + const auto msg = route_subscriber_.takeNewData(); + if (msg) { + if (msg->segments.empty()) { + RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); + } else { + route_handler_.setRoute(*msg); + } + } + } + + // map + { + const auto msg = vector_map_subscriber_.takeNewData(); + if (msg) { + route_handler_.setMap(*msg); + } + } +} + void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) { constexpr double KEEP_TIME = 1.0; @@ -102,9 +126,75 @@ DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const Diagnos diagnostic_msgs::msg::KeyValue key_value; key_value.key = "decision"; const bool is_emergency_brake = (diag.level == DiagnosticStatus::ERROR); - key_value.value = (is_emergency_brake) ? "stop" : "none"; + key_value.value = (is_emergency_brake) ? "deceleration" : "none"; + status.values.push_back(key_value); + return status; +} + +DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pose & ego_pose) const +{ + const auto current_lanelets = [&]() { + lanelet::ConstLanelet closest_route_lanelet; + route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet); + const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(ego_pose); + lanelet::ConstLanelets closest_lanelets{closest_route_lanelet}; + closest_lanelets.insert( + closest_lanelets.end(), shoulder_lanelets.begin(), shoulder_lanelets.end()); + return closest_lanelets; + }(); + const auto arc_coordinates = lanelet::utils::getArcCoordinates(current_lanelets, ego_pose); + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, ¤t_lane); + + DiagnosticStatus status; + status.name = "ego_lane_info"; + status.level = status.OK; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "lane_id"; + key_value.value = std::to_string(current_lane.id()); status.values.push_back(key_value); + key_value.key = "s"; + key_value.value = std::to_string(arc_coordinates.length); + status.values.push_back(key_value); + key_value.key = "t"; + key_value.value = std::to_string(arc_coordinates.distance); + status.values.push_back(key_value); + return status; +} + +DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus( + const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped) +{ + DiagnosticStatus status; + status.name = "kinematic_state"; + status.level = status.OK; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "vel"; + key_value.value = std::to_string(odom.twist.twist.linear.x); + status.values.push_back(key_value); + key_value.key = "acc"; + const auto & acc = accel_stamped.accel.accel.linear.x; + key_value.value = std::to_string(acc); + status.values.push_back(key_value); + key_value.key = "jerk"; + const auto jerk = [&]() { + if (!prev_acc_stamped_.has_value()) { + prev_acc_stamped_ = accel_stamped; + return 0.0; + } + const auto t = static_cast(accel_stamped.header.stamp.sec) + + static_cast(accel_stamped.header.stamp.nanosec) * 1e-9; + const auto prev_t = static_cast(prev_acc_stamped_.value().header.stamp.sec) + + static_cast(prev_acc_stamped_.value().header.stamp.nanosec) * 1e-9; + const auto dt = t - prev_t; + if (dt < std::numeric_limits::epsilon()) return 0.0; + const auto prev_acc = prev_acc_stamped_.value().accel.accel.linear.x; + prev_acc_stamped_ = accel_stamped; + return (acc - prev_acc) / dt; + }(); + key_value.value = std::to_string(jerk); + status.values.push_back(key_value); return status; } @@ -145,6 +235,7 @@ void controlEvaluatorNode::onTimer() DiagnosticArray metrics_msg; const auto traj = traj_sub_.takeData(); const auto odom = odometry_sub_.takeData(); + const auto acc = accel_sub_.takeData(); // generate decision diagnostics from input diagnostics for (const auto & function : target_functions_) { @@ -171,6 +262,16 @@ void controlEvaluatorNode::onTimer() metrics_msg.status.push_back(generateYawDeviationDiagnosticStatus(*traj, ego_pose)); } + getRouteData(); + if (odom && route_handler_.isHandlerReady()) { + const Pose ego_pose = odom->pose.pose; + metrics_msg.status.push_back(generateLaneletDiagnosticStatus(ego_pose)); + } + + if (odom && acc) { + metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*odom, *acc)); + } + metrics_msg.header.stamp = now(); metrics_pub_->publish(metrics_msg); } diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 15cab8df52ede..b54714fecc2a5 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -381,8 +381,11 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("~/input/diagnostics", "/diagnostics"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/acceleration", "/localization/acceleration"), ("~/input/trajectory", "/planning/scenario_planning/trajectory"), ("~/metrics", "/diagnostic/control_evaluator/metrics"), + ("~/input/vector_map", "/map/vector_map"), + ("~/input/route", "/planning/mission_planning/route"), ], )