feat(control_evaluator): add lanelet info to the metrics (#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>

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 <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
-#include <array>
 #include <deque>
-#include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -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> odometry_sub_{
     this, "~/input/odometry"};
+  autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
+    this, "/localization/acceleration"};
   autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
     this, "~/input/trajectory"};
+  autoware::universe_utils::InterProcessPollingSubscriber<LaneletRoute> route_subscriber_{
+    this, "~/input/route", rclcpp::QoS{1}.transient_local()};
+  autoware::universe_utils::InterProcessPollingSubscriber<LaneletMapBin> vector_map_subscriber_{
+    this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
 
   rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
 
+  // update Route Handler
+  void getRouteData();
+
   // Calculator
   // Metrics
   std::deque<rclcpp::Time> stamps_;
@@ -87,7 +104,9 @@ class controlEvaluatorNode : public rclcpp::Node
   std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
   const std::vector<std::string> target_functions_ = {"autonomous_emergency_braking"};
 
+  autoware::route_handler::RouteHandler route_handler_;
   rclcpp::TimerBase::SharedPtr timer_;
+  std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
 };
 }  // namespace control_diagnostics
 

evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml

@@ -1,15 +1,21 @@
 <launch>
   <arg name="input/diagnostics" default="/diagnostics"/>
   <arg name="input/odometry" default="/localization/kinematic_state"/>
+  <arg name="input/acceleration" default="/localization/acceleration"/>
   <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
+  <arg name="map_topic_name" default="/map/vector_map"/>
+  <arg name="route_topic_name" default="/planning/mission_planning/route"/>
   <!-- control evaluator -->
   <group>
     <node name="control_evaluator" exec="control_evaluator" pkg="autoware_control_evaluator">
       <param from="$(find-pkg-share autoware_control_evaluator)/param/control_evaluator.defaults.yaml"/>
       <remap from="~/input/diagnostics" to="$(var input/diagnostics)"/>
       <remap from="~/input/odometry" to="$(var input/odometry)"/>
+      <remap from="~/input/acceleration" to="$(var input/acceleration)"/>
       <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
       <remap from="~/metrics" to="/diagnostic/control_evaluator/metrics"/>
+      <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
+      <remap from="~/input/route" to="$(var route_topic_name)"/>
     </node>
   </group>
 </launch>

evaluator/autoware_control_evaluator/package.xml

@@ -16,6 +16,7 @@
 
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
+  <depend>autoware_route_handler</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>pluginlib</depend>

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

@@ -14,10 +14,11 @@
 
 #include "autoware/control_evaluator/control_evaluator_node.hpp"
 
-#include <fstream>
-#include <iostream>
-#include <map>
-#include <memory>
+#include <autoware_lanelet2_extension/utility/query.hpp>
+#include <autoware_lanelet2_extension/utility/utilities.hpp>
+
+#include <algorithm>
+#include <limits>
 #include <string>
 #include <utility>
 #include <vector>
@@ -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, &current_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<double>(accel_stamped.header.stamp.sec) +
+                   static_cast<double>(accel_stamped.header.stamp.nanosec) * 1e-9;
+    const auto prev_t = static_cast<double>(prev_acc_stamped_.value().header.stamp.sec) +
+                        static_cast<double>(prev_acc_stamped_.value().header.stamp.nanosec) * 1e-9;
+    const auto dt = t - prev_t;
+    if (dt < std::numeric_limits<double>::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);
 }

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"),
         ],
     )