feat(planning_evaluator): add modified goal deviation (#3053)

* feat(planning_evaluator): add modified goal deviation

Signed-off-by: kosuke55 <[email protected]>

* add abs

Signed-off-by: kosuke55 <[email protected]>

* remove setModifiedGoal

Signed-off-by: kosuke55 <[email protected]>

* use Point and rename func

Signed-off-by: kosuke55 <[email protected]>

* fix func docs

Signed-off-by: kosuke55 <[email protected]>

* fix and add tests

Signed-off-by: kosuke55 <[email protected]>

---------

Signed-off-by: kosuke55 <[email protected]>

evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp

@@ -26,6 +26,8 @@ namespace metrics
 {
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
 
 /**
  * @brief calculate lateral deviation of the given trajectory from the reference trajectory
@@ -51,6 +53,30 @@ Stat<double> calcYawDeviation(const Trajectory & ref, const Trajectory & traj);
  */
 Stat<double> calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj);
 
+/**
+ * @brief calculate longitudinal deviation of the given ego pose from the modified goal pose
+ * @param [in] base_pose base pose
+ * @param [in] target_point target point
+ * @return calculated statistics
+ */
+Stat<double> calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point);
+
+/**
+ * @brief calculate lateral deviation of the given ego pose from the modified goal pose
+ * @param [in] base_pose base pose
+ * @param [in] target_point target point
+ * @return calculated statistics
+ */
+Stat<double> calcLateralDeviation(const Pose & base_pose, const Point & target_point);
+
+/**
+ * @brief calculate yaw deviation of the given ego pose from the modified goal pose
+ * @param [in] base_pose base pose
+ * @param [in] target_pose target pose
+ * @return calculated statistics
+ */
+Stat<double> calcYawDeviation(const Pose & base_pose, const Pose & target_pose);
+
 }  // namespace metrics
 }  // namespace planning_diagnostics
 

evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp

@@ -41,6 +41,9 @@ enum class Metric {
   stability_frechet,
   obstacle_distance,
   obstacle_ttc,
+  modified_goal_longitudinal_deviation,
+  modified_goal_lateral_deviation,
+  modified_goal_yaw_deviation,
   SIZE,
 };
 
@@ -63,7 +66,10 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"stability_frechet", Metric::stability_frechet},
   {"obstacle_distance", Metric::obstacle_distance},
   {"obstacle_ttc", Metric::obstacle_ttc},
-};
+  {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation},
+  {"modified_goal_lateral_deviation", Metric::modified_goal_lateral_deviation},
+  {"modified_goal_yaw_deviation", Metric::modified_goal_yaw_deviation}};
+
 static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::curvature, "curvature"},
   {Metric::point_interval, "point_interval"},
@@ -79,7 +85,10 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::stability, "stability"},
   {Metric::stability_frechet, "stability_frechet"},
   {Metric::obstacle_distance, "obstacle_distance"},
-  {Metric::obstacle_ttc, "obstacle_ttc"}};
+  {Metric::obstacle_ttc, "obstacle_ttc"},
+  {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"},
+  {Metric::modified_goal_lateral_deviation, "modified_goal_lateral_deviation"},
+  {Metric::modified_goal_yaw_deviation, "modified_goal_yaw_deviation"}};
 
 // Metrics descriptions
 static const std::unordered_map<Metric, std::string> metric_descriptions = {
@@ -97,7 +106,10 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::stability, "Stability[m]"},
   {Metric::stability_frechet, "StabilityFrechet[m]"},
   {Metric::obstacle_distance, "Obstacle_distance[m]"},
-  {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}};
+  {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"},
+  {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"},
+  {Metric::modified_goal_lateral_deviation, "Modified_goal_lateral_deviation[m]"},
+  {Metric::modified_goal_yaw_deviation, "Modified_goal_yaw_deviation[rad]"}};
 
 namespace details
 {

evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp

@@ -14,21 +14,26 @@
 
 #ifndef PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_
 #define PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_
-
 #include "planning_evaluator/metrics/metric.hpp"
 #include "planning_evaluator/parameters.hpp"
 #include "planning_evaluator/stat.hpp"
 
 #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 
+#include <optional>
+
 namespace planning_diagnostics
 {
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using autoware_planning_msgs::msg::PoseWithUuidStamped;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
 
 class MetricsCalculator
 {
@@ -42,7 +47,9 @@ class MetricsCalculator
    * @param [in] metric Metric enum value
    * @return string describing the requested metric
    */
-  Stat<double> calculate(const Metric metric, const Trajectory & traj) const;
+  std::optional<Stat<double>> calculate(const Metric metric, const Trajectory & traj) const;
+  std::optional<Stat<double>> calculate(
+    const Metric metric, const Pose & base_pose, const Pose & target_pose) const;
 
   /**
    * @brief set the reference trajectory used to calculate the deviation metrics
@@ -68,6 +75,12 @@ class MetricsCalculator
    */
   void setEgoPose(const geometry_msgs::msg::Pose & pose);
 
+  /**
+   * @brief get the ego pose
+   * @return ego pose
+   */
+  Pose getEgoPose();
+
 private:
   /**
    * @brief trim a trajectory from the current ego pose to some fixed time or distance
@@ -86,6 +99,7 @@ class MetricsCalculator
   Trajectory previous_trajectory_lookahead_;
   PredictedObjects dynamic_objects_;
   geometry_msgs::msg::Pose ego_pose_;
+  PoseWithUuidStamped modified_goal_;
 };  // class MetricsCalculator
 
 }  // namespace planning_diagnostics

evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp

@@ -24,7 +24,9 @@
 #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp"
 #include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 
 #include <array>
 #include <deque>
@@ -37,8 +39,10 @@ namespace planning_diagnostics
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using diagnostic_msgs::msg::DiagnosticArray;
 using diagnostic_msgs::msg::DiagnosticStatus;
+using nav_msgs::msg::Odometry;
 
 /**
  * @brief Node for planning evaluation
@@ -49,6 +53,12 @@ class PlanningEvaluatorNode : public rclcpp::Node
   explicit PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options);
   ~PlanningEvaluatorNode();
 
+  /**
+   * @brief callback on receiving an odometry
+   * @param [in] odometry_msg received odometry message
+   */
+  void onOdometry(const Odometry::ConstSharedPtr odometry_msg);
+
   /**
    * @brief callback on receiving a trajectory
    * @param [in] traj_msg received trajectory message
@@ -68,9 +78,10 @@ class PlanningEvaluatorNode : public rclcpp::Node
   void onObjects(const PredictedObjects::ConstSharedPtr objects_msg);
 
   /**
-   * @brief update the ego pose stored in the MetricsCalculator
+   * @brief callback on receiving a modified goal
+   * @param [in] modified_goal_msg received modified goal message
    */
-  void updateCalculatorEgoPose(const std::string & target_frame);
+  void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg);
 
   /**
    * @brief publish the given metric statistic
@@ -80,11 +91,15 @@ class PlanningEvaluatorNode : public rclcpp::Node
 
 private:
   static bool isFinite(const TrajectoryPoint & p);
+  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_;
+
   rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
   std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -99,6 +114,9 @@ class PlanningEvaluatorNode : public rclcpp::Node
   std::vector<Metric> metrics_;
   std::deque<rclcpp::Time> stamps_;
   std::array<std::deque<Stat<double>>, static_cast<size_t>(Metric::SIZE)> metric_stats_;
+
+  Odometry::ConstSharedPtr ego_state_ptr_;
+  PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_;
 };
 }  // namespace planning_diagnostics
 

evaluator/planning_evaluator/launch/planning_evaluator.launch.xml

@@ -1,15 +1,19 @@
 <launch>
+  <arg name="input/odometry" default="/localization/kinematic_state"/>
   <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
   <arg name="input/reference_trajectory" default="/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory"/>
   <arg name="input/objects" default="/perception/object_recognition/objects"/>
+  <arg name="input/modified_goal" default="/planning/scenario_planning/modified_goal"/>
 
   <!-- planning evaluator -->
   <group>
     <node name="planning_evaluator" exec="planning_evaluator" pkg="planning_evaluator">
       <param from="$(find-pkg-share planning_evaluator)/param/planning_evaluator.defaults.yaml"/>
+      <remap from="~/input/odometry" to="$(var input/odometry)"/>
       <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="~/metrics" to="/diagnostic/planning_evaluator/metrics"/>
     </node>
   </group>

evaluator/planning_evaluator/package.xml

@@ -16,6 +16,7 @@
 
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>

evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml

@@ -19,6 +19,9 @@
       - stability_frechet
       - obstacle_distance
       - obstacle_ttc
+      - modified_goal_longitudinal_deviation
+      - modified_goal_lateral_deviation
+      - modified_goal_yaw_deviation
 
     trajectory:
       min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation

evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp

@@ -77,5 +77,25 @@ Stat<double> calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr
   return stat;
 }
 
+Stat<double> calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point)
+{
+  Stat<double> stat;
+  stat.add(std::abs(tier4_autoware_utils::calcLongitudinalDeviation(base_pose, target_point)));
+  return stat;
+}
+
+Stat<double> calcLateralDeviation(const Pose & base_pose, const Point & target_point)
+{
+  Stat<double> stat;
+  stat.add(std::abs(tier4_autoware_utils::calcLateralDeviation(base_pose, target_point)));
+  return stat;
+}
+
+Stat<double> calcYawDeviation(const Pose & base_pose, const Pose & target_pose)
+{
+  Stat<double> stat;
+  stat.add(std::abs(tier4_autoware_utils::calcYawDeviation(base_pose, target_pose)));
+  return stat;
+}
 }  // namespace metrics
 }  // namespace planning_diagnostics

evaluator/planning_evaluator/src/metrics_calculator.cpp

@@ -23,9 +23,10 @@
 
 namespace planning_diagnostics
 {
-Stat<double> MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const
+std::optional<Stat<double>> MetricsCalculator::calculate(
+  const Metric metric, const Trajectory & traj) const
 {
-  // Functions to calculate metrics
+  // Functions to calculate trajectory metrics
   switch (metric) {
     case Metric::curvature:
       return metrics::calcTrajectoryCurvature(traj);
@@ -70,9 +71,23 @@ Stat<double> MetricsCalculator::calculate(const Metric metric, const Trajectory
     case Metric::obstacle_ttc:
       return metrics::calcTimeToCollision(dynamic_objects_, traj, parameters.obstacle.dist_thr_m);
     default:
-      throw std::runtime_error(
-        "[MetricsCalculator][calculate()] unknown Metric " +
-        std::to_string(static_cast<int>(metric)));
+      return {};
+  }
+}
+
+std::optional<Stat<double>> MetricsCalculator::calculate(
+  const Metric metric, const Pose & base_pose, const Pose & target_pose) const
+{
+  // Functions to calculate pose metrics
+  switch (metric) {
+    case Metric::modified_goal_longitudinal_deviation:
+      return metrics::calcLongitudinalDeviation(base_pose, target_pose.position);
+    case Metric::modified_goal_lateral_deviation:
+      return metrics::calcLateralDeviation(base_pose, target_pose.position);
+    case Metric::modified_goal_yaw_deviation:
+      return metrics::calcYawDeviation(base_pose, target_pose);
+    default:
+      return {};
   }
 }
 
@@ -93,6 +108,8 @@ void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects)
 
 void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) { ego_pose_ = pose; }
 
+Pose MetricsCalculator::getEgoPose() { return ego_pose_; }
+
 Trajectory MetricsCalculator::getLookaheadTrajectory(
   const Trajectory & traj, const double max_dist_m, const double max_time_s) const
 {

evaluator/planning_evaluator/src/motion_evaluator_node.cpp

@@ -55,7 +55,9 @@ MotionEvaluatorNode::~MotionEvaluatorNode()
   f << std::endl;
   for (Metric metric : metrics_) {
     const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_);
-    f /* << std::setw(3 * column_width) */ << stat << " ";
+    if (stat) {
+      f /* << std::setw(3 * column_width) */ << *stat << " ";
+    }
   }
   f.close();
 }