fix planning evaluator

Signed-off-by: Daniel Sanchez <[email protected]>

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

@@ -16,8 +16,8 @@
 
 #include "autoware/evaluator_utils/evaluator_utils.hpp"
 
-#include <autoware_lanelet2_extension/utility/query.hpp>
-#include <autoware_lanelet2_extension/utility/utilities.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
 
 #include <algorithm>
 #include <limits>

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp

@@ -21,25 +21,19 @@
 #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 <diagnostic_msgs/msg/detail/diagnostic_status__struct.hpp>
 
 #include <array>
 #include <deque>
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
+
 namespace planning_diagnostics
 {
 using autoware_perception_msgs::msg::PredictedObjects;
@@ -49,9 +43,6 @@ 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
  */
@@ -91,72 +82,26 @@ class PlanningEvaluatorNode : public rclcpp::Node
    */
   void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg);
 
-  /**
-   * @brief obtain diagnostics information
-   */
-  void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
-
   /**
    * @brief publish the given metric statistic
    */
   DiagnosticStatus generateDiagnosticStatus(
     const Metric & metric, const Stat<double> & metric_stat) const;
 
-  /**
-   * @brief publish current ego lane info
-   */
-  DiagnosticStatus generateDiagnosticEvaluationStatus(const DiagnosticStatus & diag);
-
-  /**
-   * @brief publish current ego lane info
-   */
-  DiagnosticStatus generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr);
-
-  /**
-   * @brief publish current ego kinematic state
-   */
-  DiagnosticStatus generateKinematicStateDiagnosticStatus(
-    const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr);
-
 private:
   static bool isFinite(const TrajectoryPoint & p);
-
-  /**
-   * @brief update route handler data
-   */
-  void getRouteData();
-
-  /**
-   * @brief fetch data and publish diagnostics
-   */
-  void onTimer();
-
-  /**
-   * @brief fetch topic data
-   */
-  void fetchData();
-  // The diagnostics cycle is faster than timer, and each node publishes diagnostic separately.
-  // takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with
-  // onDiagnostics().
-  rclcpp::Subscription<DiagnosticArray>::SharedPtr planning_diag_sub_;
+  void publishModifiedGoalDeviationMetrics();
 
   // ROS
   rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
   rclcpp::Subscription<Trajectory>::SharedPtr ref_sub_;
   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_;
@@ -169,11 +114,8 @@ class PlanningEvaluatorNode : public rclcpp::Node
   std::deque<rclcpp::Time> stamps_;
   std::array<std::deque<Stat<double>>, static_cast<size_t>(Metric::SIZE)> metric_stats_;
 
-  rclcpp::TimerBase::SharedPtr timer_;
-  // queue for diagnostics and time stamp
-  std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
-  const std::vector<std::string> target_functions_ = {"obstacle_cruise_planner"};
-  std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
+  Odometry::ConstSharedPtr ego_state_ptr_;
+  PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_;
 };
 }  // namespace planning_diagnostics
 

evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml

@@ -1,27 +1,19 @@
 <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"/>
-  <arg name="input/diagnostics" default="/diagnostics"/>
 
   <!-- 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/diagnostics" to="$(var input/diagnostics)"/>
       <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="/planning/planning_evaluator/metrics"/>
     </node>
   </group>

evaluator/autoware_planning_evaluator/package.xml

@@ -13,11 +13,9 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>autoware_evaluator_utils</depend>
   <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>

evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp

@@ -30,8 +30,9 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons
   const TrajectoryPoint & curr_p = traj.points.at(curr_id);
 
   size_t target_id = curr_id;
+  double current_distance = 0.0;
   for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) {
-    double current_distance = calcDistance2d(traj.points.at(traj_id), curr_p);
+    current_distance = calcDistance2d(traj.points.at(traj_id), curr_p);
     if (current_distance >= distance) {
       target_id = traj_id;
       break;