From 01a80587736606125a728129709163f0ff437290 Mon Sep 17 00:00:00 2001
From: danielsanchezaran <daniel.sanchez@tier4.jp>
Date: Tue, 2 Jul 2024 15:58:24 +0900
Subject: [PATCH] feat(planning_evaluator): add lanelet info to the planning
 evaluator (#7781)

add lanelet info to the planning evaluator

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
 .../planning_evaluator_node.hpp               |  29 +++++
 .../launch/planning_evaluator.launch.xml      |   6 +
 .../autoware_planning_evaluator/package.xml   |   1 +
 .../src/planning_evaluator_node.cpp           | 115 ++++++++++++++++++
 4 files changed, 151 insertions(+)

diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp
index 0121425c15d1d..74523398c3a77 100644
--- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp
+++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp
@@ -21,12 +21,17 @@
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 
+#include <autoware/route_handler/route_handler.hpp>
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
+
 #include "autoware_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 #include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
 
 #include <array>
 #include <deque>
@@ -43,6 +48,9 @@ using autoware_planning_msgs::msg::TrajectoryPoint;
 using diagnostic_msgs::msg::DiagnosticArray;
 using diagnostic_msgs::msg::DiagnosticStatus;
 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 planning evaluation
  */
@@ -88,9 +96,22 @@ class PlanningEvaluatorNode : public rclcpp::Node
   DiagnosticStatus generateDiagnosticStatus(
     const Metric & metric, const Stat<double> & metric_stat) const;
 
+  /**
+   * @brief publish current ego lane info
+   */
+  DiagnosticStatus generateLaneletDiagnosticStatus();
+
+  /**
+   * @brief publish current ego kinematic state
+   */
+  DiagnosticStatus generateKinematicStateDiagnosticStatus(
+    const AccelWithCovarianceStamped & accel_stamped);
+
 private:
   static bool isFinite(const TrajectoryPoint & p);
   void publishModifiedGoalDeviationMetrics();
+  // update Route Handler
+  void getRouteData();
 
   // ROS
   rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
@@ -98,10 +119,17 @@ class PlanningEvaluatorNode : public rclcpp::Node
   rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
   rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr modified_goal_sub_;
   rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
+  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()};
+  autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
+    this, "~/input/acceleration"};
 
   rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
   std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  autoware::route_handler::RouteHandler route_handler_;
 
   // Parameters
   std::string output_file_str_;
@@ -116,6 +144,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
 
   Odometry::ConstSharedPtr ego_state_ptr_;
   PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_;
+  std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
 };
 }  // namespace planning_diagnostics
 
diff --git a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml
index 2e358f6272379..70301ebe6275f 100644
--- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml
+++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml
@@ -1,19 +1,25 @@
 <launch>
   <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="input/reference_trajectory" default="/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/trajectory"/>
   <arg name="input/objects" default="/perception/object_recognition/objects"/>
   <arg name="input/modified_goal" default="/planning/scenario_planning/modified_goal"/>
+  <arg name="input/map_topic" default="/map/vector_map"/>
+  <arg name="input/route_topic" default="/planning/mission_planning/route"/>
 
   <!-- planning evaluator -->
   <group>
     <node name="planning_evaluator" exec="planning_evaluator" pkg="autoware_planning_evaluator">
       <param from="$(find-pkg-share autoware_planning_evaluator)/param/planning_evaluator.defaults.yaml"/>
       <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="~/input/reference_trajectory" to="$(var input/reference_trajectory)"/>
       <remap from="~/input/objects" to="$(var input/objects)"/>
       <remap from="~/input/modified_goal" to="$(var input/modified_goal)"/>
+      <remap from="~/input/route" to="$(var input/route_topic)"/>
+      <remap from="~/input/vector_map" to="$(var input/map_topic)"/>
       <remap from="~/metrics" to="/diagnostic/planning_evaluator/metrics"/>
     </node>
   </group>
diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml
index 5bd500f200eac..3c3d4bd8bb595 100644
--- a/evaluator/autoware_planning_evaluator/package.xml
+++ b/evaluator/autoware_planning_evaluator/package.xml
@@ -16,6 +16,7 @@
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>
+  <depend>autoware_route_handler</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp
index 103e14c73f26d..5e959aed123af 100644
--- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp
+++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp
@@ -14,6 +14,11 @@
 
 #include "autoware/planning_evaluator/planning_evaluator_node.hpp"
 
+#include <autoware_lanelet2_extension/utility/query.hpp>
+#include <autoware_lanelet2_extension/utility/utilities.hpp>
+
+#include <diagnostic_msgs/msg/detail/diagnostic_status__struct.hpp>
+
 #include "boost/lexical_cast.hpp"
 
 #include <fstream>
@@ -105,6 +110,97 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode()
   }
 }
 
+void PlanningEvaluatorNode::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);
+    }
+  }
+}
+
+DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus()
+{
+  const auto & ego_pose = ego_state_ptr_->pose.pose;
+  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 PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus(
+  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(ego_state_ptr_->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;
+}
+
 DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus(
   const Metric & metric, const Stat<double> & metric_stat) const
 {
@@ -172,6 +268,25 @@ void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_m
 {
   ego_state_ptr_ = odometry_msg;
   metrics_calculator_.setEgoPose(odometry_msg->pose.pose);
+  {
+    DiagnosticArray metrics_msg;
+    metrics_msg.header.stamp = now();
+
+    getRouteData();
+    if (route_handler_.isHandlerReady() && ego_state_ptr_) {
+      metrics_msg.status.push_back(generateLaneletDiagnosticStatus());
+    }
+
+    const auto acc = accel_sub_.takeData();
+
+    if (acc && ego_state_ptr_) {
+      metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*acc));
+    }
+
+    if (!metrics_msg.status.empty()) {
+      metrics_pub_->publish(metrics_msg);
+    }
+  }
 
   if (modified_goal_ptr_) {
     publishModifiedGoalDeviationMetrics();