fixup! feat: use mapplotlibcpp

planning/autoware_planning_data_analyzer/launch/behavior_analyzer.launch.xml

@@ -29,7 +29,7 @@
     </include>
   </group>
 
-  <node pkg="autoware_planning_data_analyzer" exec="minimal" name="behavior_analyzer">
+  <node pkg="autoware_planning_data_analyzer" exec="minimal" name="behavior_analyzer" output="screen">
     <param name="bag_path" value="$(var bag_path)"/>
     <param from="$(find-pkg-share autoware_planning_data_analyzer)/config/behavior_analyzer.param.yaml"/>
     <remap from="input/lanelet2_map" to="/map/vector_map"/>

planning/autoware_planning_data_analyzer/src/data_structs.cpp

@@ -157,20 +157,23 @@ void CommonData::calculate()
     scores.at(static_cast<size_t>(SCORE::EFFICIENCY)) = 0.0;
     scores.at(static_cast<size_t>(SCORE::SAFETY)) = 0.0;
     scores.at(static_cast<size_t>(SCORE::ACHIEVABILITY)) = 0.0;
+    scores.at(static_cast<size_t>(SCORE::CONSISTENCY)) = 0.0;
   }
 
   std::vector<double> lateral_accel_values;
   std::vector<double> minimum_ttc_values;
   std::vector<double> longitudinal_jerk_values;
   std::vector<double> travel_distance_values;
   std::vector<double> lateral_deviation_values;
+  std::vector<double> trajectory_deviation_values;
 
   for (size_t i = 0; i < parameters->resample_num - 1; i++) {
     lateral_accel_values.push_back(lateral_accel(i));
     longitudinal_jerk_values.push_back(longitudinal_jerk(i));
     minimum_ttc_values.push_back(minimum_ttc(i));
     travel_distance_values.push_back(travel_distance(i));
     lateral_deviation_values.push_back(lateral_deviation(i));
+    trajectory_deviation_values.push_back(trajectory_deviation(i));
   }
 
   {
@@ -179,20 +182,23 @@ void CommonData::calculate()
     minimum_ttc_values.push_back(minimum_ttc(parameters->resample_num - 1));
     travel_distance_values.push_back(travel_distance(parameters->resample_num - 1));
     lateral_deviation_values.push_back(lateral_deviation(parameters->resample_num - 1));
+    trajectory_deviation_values.push_back(trajectory_deviation(parameters->resample_num - 1));
   }
 
   values.at(static_cast<size_t>(METRIC::LATERAL_ACCEL)) = lateral_accel_values;
   values.at(static_cast<size_t>(METRIC::LONGITUDINAL_JERK)) = longitudinal_jerk_values;
   values.at(static_cast<size_t>(METRIC::MINIMUM_TTC)) = minimum_ttc_values;
   values.at(static_cast<size_t>(METRIC::TRAVEL_DISTANCE)) = travel_distance_values;
   values.at(static_cast<size_t>(METRIC::LATERAL_DEVIATION)) = lateral_deviation_values;
+  values.at(static_cast<size_t>(METRIC::TRAJECTORY_DEVIATION)) = trajectory_deviation_values;
 
   scores.at(static_cast<size_t>(SCORE::LATERAL_COMFORTABILITY)) = lateral_comfortability();
   scores.at(static_cast<size_t>(SCORE::LONGITUDINAL_COMFORTABILITY)) =
     longitudinal_comfortability();
   scores.at(static_cast<size_t>(SCORE::EFFICIENCY)) = efficiency();
   scores.at(static_cast<size_t>(SCORE::SAFETY)) = safety();
   scores.at(static_cast<size_t>(SCORE::ACHIEVABILITY)) = achievability();
+  scores.at(static_cast<size_t>(SCORE::CONSISTENCY)) = consistency();
 }
 
 void CommonData::normalize(
@@ -271,6 +277,20 @@ double CommonData::achievability() const
   return score;
 }
 
+double CommonData::consistency() const
+{
+  constexpr double TIME_FACTOR = 1.0;
+
+  double score = 0.0;
+
+  for (size_t i = 0; i < parameters->resample_num; i++) {
+    score += std::pow(TIME_FACTOR, i) *
+             std::abs(values.at(static_cast<size_t>(METRIC::TRAJECTORY_DEVIATION)).at(i));
+  }
+
+  return score;
+}
+
 double CommonData::get(const SCORE & score_type) const
 {
   return scores.at(static_cast<size_t>(score_type));
@@ -283,7 +303,8 @@ double CommonData::total(
          w1 * scores.at(static_cast<size_t>(SCORE::LONGITUDINAL_COMFORTABILITY)) +
          w2 * scores.at(static_cast<size_t>(SCORE::EFFICIENCY)) +
          w3 * scores.at(static_cast<size_t>(SCORE::SAFETY)) +
-         w4 * scores.at(static_cast<size_t>(SCORE::ACHIEVABILITY));
+         w4 * scores.at(static_cast<size_t>(SCORE::ACHIEVABILITY)) +
+         1.0 * scores.at(static_cast<size_t>(SCORE::CONSISTENCY));
 }
 
 ManualDrivingData::ManualDrivingData(
@@ -378,6 +399,11 @@ double ManualDrivingData::lateral_deviation(const size_t idx) const
   return arc_coordinates.distance;
 }
 
+double ManualDrivingData::trajectory_deviation([[maybe_unused]] const size_t idx) const
+{
+  return 1.0;
+}
+
 bool ManualDrivingData::ready() const
 {
   if (objects_history.size() < parameters->resample_num) {
@@ -402,8 +428,8 @@ bool ManualDrivingData::ready() const
 TrajectoryData::TrajectoryData(
   const std::shared_ptr<BagData> & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info,
   const std::shared_ptr<Parameters> & parameters, const std::string & tag,
-  const std::vector<TrajectoryPoint> & points)
-: CommonData(bag_data, vehicle_info, parameters, tag), points{points}
+  const std::vector<TrajectoryPoint> & points, const std::optional<TrajectoryPoints> & t_best)
+: CommonData(bag_data, vehicle_info, parameters, tag), points{points}, t_best{t_best}
 {
   calculate();
 }
@@ -445,6 +471,15 @@ double TrajectoryData::lateral_deviation(const size_t idx) const
   return arc_coordinates.distance;
 }
 
+double TrajectoryData::trajectory_deviation(const size_t idx) const
+{
+  if (!t_best.has_value()) return 0.0;
+
+  const auto & p1 = autoware::universe_utils::getPose(points.at(idx));
+  const auto & p2 = t_best.value().at(idx);
+  return autoware::universe_utils::calcSquaredDistance2d(p1, p2);
+}
+
 bool TrajectoryData::feasible() const
 {
   const auto condition = [](const auto & p) { return p.longitudinal_velocity_mps >= 0.0; };
@@ -466,7 +501,7 @@ bool TrajectoryData::ready() const
 
 SamplingTrajectoryData::SamplingTrajectoryData(
   const std::shared_ptr<BagData> & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info,
-  const std::shared_ptr<Parameters> & parameters)
+  const std::shared_ptr<Parameters> & parameters, const std::optional<TrajectoryPoints> & t_best)
 {
   const auto opt_odometry = std::dynamic_pointer_cast<Buffer<Odometry>>(
                               bag_data->buffers.at("/localization/kinematic_state"))
@@ -497,7 +532,7 @@ SamplingTrajectoryData::SamplingTrajectoryData(
   for (const auto & sample : utils::sampling(
          *opt_trajectory, opt_odometry->pose.pose, opt_odometry->twist.twist.linear.x,
          opt_accel->accel.accel.linear.x, vehicle_info, parameters)) {
-    data.emplace_back(bag_data, vehicle_info, parameters, "frenet", sample);
+    data.emplace_back(bag_data, vehicle_info, parameters, "frenet", sample, t_best);
   }
 
   // std::vector<TrajectoryPoint> stop_points(parameters->resample_num);

planning/autoware_planning_data_analyzer/src/data_structs.hpp

@@ -38,6 +38,7 @@ enum class METRIC {
   TRAVEL_DISTANCE = 3,
   MINIMUM_TTC = 4,
   LATERAL_DEVIATION = 5,
+  TRAJECTORY_DEVIATION = 6,
   SIZE
 };
 
@@ -47,7 +48,8 @@ enum class SCORE {
   EFFICIENCY = 2,
   SAFETY = 3,
   ACHIEVABILITY = 4,
-  TOTAL = 5,
+  CONSISTENCY = 5,
+  TOTAL = 6,
   SIZE
 };
 
@@ -242,6 +244,8 @@ struct CommonData
 
   double achievability() const;
 
+  double consistency() const;
+
   double total(
     const double w0, const double w1, const double w2, const double w3, const double w4) const;
 
@@ -259,6 +263,8 @@ struct CommonData
 
   virtual double lateral_deviation(const size_t idx) const = 0;
 
+  virtual double trajectory_deviation(const size_t idx) const = 0;
+
   virtual bool feasible() const = 0;
 
   virtual bool ready() const = 0;
@@ -294,6 +300,8 @@ struct ManualDrivingData : CommonData
 
   double lateral_deviation(const size_t idx) const override;
 
+  double trajectory_deviation(const size_t idx) const override;
+
   bool feasible() const override { return true; }
 
   bool ready() const override;
@@ -308,7 +316,7 @@ struct TrajectoryData : CommonData
   TrajectoryData(
     const std::shared_ptr<BagData> & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info,
     const std::shared_ptr<Parameters> & parameters, const std::string & tag,
-    const std::vector<TrajectoryPoint> & points);
+    const std::vector<TrajectoryPoint> & points, const std::optional<TrajectoryPoints> & t_best);
 
   double lateral_accel(const size_t idx) const override;
 
@@ -320,18 +328,22 @@ struct TrajectoryData : CommonData
 
   double lateral_deviation(const size_t idx) const override;
 
+  double trajectory_deviation(const size_t idx) const override;
+
   bool feasible() const override;
 
   bool ready() const override;
 
   std::vector<TrajectoryPoint> points;
+
+  std::optional<TrajectoryPoints> t_best;
 };
 
 struct SamplingTrajectoryData
 {
   SamplingTrajectoryData(
     const std::shared_ptr<BagData> & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info,
-    const std::shared_ptr<Parameters> & parameters);
+    const std::shared_ptr<Parameters> & parameters, const std::optional<TrajectoryPoints> & t_best);
 
   auto best(const double w0, const double w1, const double w2, const double w3, const double w4)
     const -> std::optional<TrajectoryData>
@@ -370,9 +382,9 @@ struct DataSet
 {
   DataSet(
     const std::shared_ptr<BagData> & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info,
-    const std::shared_ptr<Parameters> & parameters)
+    const std::shared_ptr<Parameters> & parameters, const std::optional<TrajectoryPoints> & t_best)
   : manual{ManualDrivingData(bag_data, vehicle_info, parameters)},
-    sampling{SamplingTrajectoryData(bag_data, vehicle_info, parameters)},
+    sampling{SamplingTrajectoryData(bag_data, vehicle_info, parameters, t_best)},
     route_handler{bag_data->route_handler},
     parameters{parameters}
   {
@@ -397,13 +409,15 @@ struct DataSet
     const auto [s2_min, s2_max] = range(static_cast<size_t>(SCORE::EFFICIENCY));
     const auto [s3_min, s3_max] = range(static_cast<size_t>(SCORE::SAFETY));
     const auto [s4_min, s4_max] = range(static_cast<size_t>(SCORE::ACHIEVABILITY));
+    const auto [s5_min, s5_max] = range(static_cast<size_t>(SCORE::CONSISTENCY));
 
     for (auto & data : sampling.data) {
       data.normalize(s0_min, s0_max, static_cast<size_t>(SCORE::LATERAL_COMFORTABILITY), true);
       data.normalize(s1_min, s1_max, static_cast<size_t>(SCORE::LONGITUDINAL_COMFORTABILITY), true);
       data.normalize(s2_min, s2_max, static_cast<size_t>(SCORE::EFFICIENCY));
       data.normalize(s3_min, s3_max, static_cast<size_t>(SCORE::SAFETY));
       data.normalize(s4_min, s4_max, static_cast<size_t>(SCORE::ACHIEVABILITY), true);
+      data.normalize(s5_min, s5_max, static_cast<size_t>(SCORE::CONSISTENCY), true);
       data.scores.at(static_cast<size_t>(SCORE::TOTAL)) =
         data.total(parameters->w0, parameters->w1, parameters->w2, parameters->w3, parameters->w4);
     }
@@ -458,6 +472,8 @@ struct DataSet
     double s3_dev = 0.0;
     double s4_ave = 0.0;
     double s4_dev = 0.0;
+    double s5_ave = 0.0;
+    double s5_dev = 0.0;
 
     const auto update = [](const double ave, const double dev, const double value, const size_t i) {
       const auto new_ave = (i * ave + value) / (i + 1);
@@ -474,6 +490,7 @@ struct DataSet
       std::tie(s2_ave, s2_dev) = update(s2_ave, s2_dev, data.get(SCORE::EFFICIENCY), i);
       std::tie(s3_ave, s3_dev) = update(s3_ave, s3_dev, data.get(SCORE::SAFETY), i);
       std::tie(s4_ave, s4_dev) = update(s4_ave, s4_dev, data.get(SCORE::ACHIEVABILITY), i);
+      std::tie(s5_ave, s5_dev) = update(s5_ave, s5_dev, data.get(SCORE::CONSISTENCY), i);
     }
 
     std::stringstream ss;
@@ -485,6 +502,7 @@ struct DataSet
     ss << " efficiency        :" << best.value().get(SCORE::EFFICIENCY)                   << " mean:" << s2_ave << " std:" << std::sqrt(s2_dev) << "\n"; // NOLINT
     ss << " safety            :" << best.value().get(SCORE::SAFETY)                       << " mean:" << s3_ave << " std:" << std::sqrt(s3_dev) << "\n"; // NOLINT
     ss << " achievability     :" << best.value().get(SCORE::ACHIEVABILITY)                << " mean:" << s4_ave << " std:" << std::sqrt(s4_dev) << "\n"; // NOLINT
+    ss << " consistency       :" << best.value().get(SCORE::CONSISTENCY)                  << " mean:" << s5_ave << " std:" << std::sqrt(s5_dev) << "\n"; // NOLINT
     ss << " total             :" << best.value().get(SCORE::TOTAL);
     // clang-format on
     RCLCPP_INFO_STREAM(rclcpp::get_logger(__func__), ss.str());

planning/autoware_planning_data_analyzer/src/node.cpp

@@ -31,6 +31,7 @@ using autoware::universe_utils::Polygon2d;
 BehaviorAnalyzerNode::BehaviorAnalyzerNode(const rclcpp::NodeOptions & node_options)
 : Node("path_selector_node", node_options),
   route_handler_{std::make_shared<RouteHandler>()},
+  best_{std::nullopt},
   buffer_{static_cast<size_t>(SCORE::SIZE)}
 {
   using namespace std::literals::chrono_literals;
@@ -317,12 +318,14 @@ void BehaviorAnalyzerNode::weight(
     RCLCPP_INFO_STREAM(get_logger(), ss.str());
   };
 
+  std::optional<TrajectoryPoints> best{std::nullopt};
+
   while (reader_.has_next() && rclcpp::ok()) {
     update(bag_data, p->grid_seach.dt);
 
     if (!bag_data->ready()) break;
 
-    const auto data_set = std::make_shared<DataSet>(bag_data, vehicle_info_, p);
+    const auto data_set = std::make_shared<DataSet>(bag_data, vehicle_info_, p, best);
 
     std::mutex grid_mutex;
 
@@ -382,7 +385,7 @@ void BehaviorAnalyzerNode::analyze(const std::shared_ptr<BagData> & bag_data) co
 {
   if (!bag_data->ready()) return;
 
-  const auto data_set = std::make_shared<DataSet>(bag_data, vehicle_info_, parameters_);
+  const auto data_set = std::make_shared<DataSet>(bag_data, vehicle_info_, parameters_, best_);
 
   const auto opt_tf = std::dynamic_pointer_cast<Buffer<TFMessage>>(bag_data->buffers.at(TOPIC::TF))
                         ->get(bag_data->timestamp);
@@ -523,6 +526,9 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr<DataSet> & data_set)
       marker.points.push_back(point.pose.position);
     }
     msg.markers.push_back(marker);
+    best_ = best.value().points;
+  } else {
+    best_ = std::nullopt;
   }
 
   for (size_t i = 0; i < data_set->sampling.data.size(); ++i) {
@@ -532,6 +538,7 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr<DataSet> & data_set)
     msg.markers.push_back(utils::to_marker(data, SCORE::EFFICIENCY, i));
     msg.markers.push_back(utils::to_marker(data, SCORE::SAFETY, i));
     msg.markers.push_back(utils::to_marker(data, SCORE::ACHIEVABILITY, i));
+    msg.markers.push_back(utils::to_marker(data, SCORE::CONSISTENCY, i));
     msg.markers.push_back(utils::to_marker(data, SCORE::TOTAL, i));
   }
 
@@ -552,6 +559,7 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr<DataSet> & data_set)
     clear_buffer(SCORE::EFFICIENCY);
     clear_buffer(SCORE::SAFETY);
     clear_buffer(SCORE::ACHIEVABILITY);
+    clear_buffer(SCORE::CONSISTENCY);
     clear_buffer(SCORE::TOTAL);
     count_ = 0;
   } else {
@@ -560,6 +568,7 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr<DataSet> & data_set)
     set_buffer(SCORE::EFFICIENCY);
     set_buffer(SCORE::SAFETY);
     set_buffer(SCORE::ACHIEVABILITY);
+    set_buffer(SCORE::CONSISTENCY);
     set_buffer(SCORE::TOTAL);
     count_++;
   }
@@ -615,8 +624,9 @@ void BehaviorAnalyzerNode::plot(const std::shared_ptr<DataSet> & data_set) const
   subplot(SCORE::EFFICIENCY, 3, 3, 3);
   subplot(SCORE::SAFETY, 3, 3, 4);
   subplot(SCORE::ACHIEVABILITY, 3, 3, 5);
-  subplot(SCORE::TOTAL, 3, 3, 6);
-  plot_best(data_set->sampling.best(p->w0, p->w1, p->w2, p->w3, p->w4), 3, 3, 7);
+  subplot(SCORE::CONSISTENCY, 3, 3, 6);
+  subplot(SCORE::TOTAL, 3, 3, 7);
+  plot_best(data_set->sampling.best(p->w0, p->w1, p->w2, p->w3, p->w4), 3, 3, 8);
   matplotlibcpp::pause(1e-9);
 }
 }  // namespace autoware::behavior_analyzer

planning/autoware_planning_data_analyzer/src/node.hpp

@@ -93,6 +93,8 @@ class BehaviorAnalyzerNode : public rclcpp::Node
 
   std::shared_ptr<Parameters> parameters_;
 
+  mutable std::optional<TrajectoryPoints> best_;
+
   mutable std::mutex mutex_;
 
   mutable std::vector<std::vector<double>> buffer_;

planning/autoware_planning_data_analyzer/src/type_alias.hpp

@@ -50,6 +50,8 @@ using autoware_planning_msgs::msg::TrajectoryPoint;
 using autoware_vehicle_msgs::msg::SteeringReport;
 using route_handler::RouteHandler;
 
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
 // ros2
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::Point;