fix

Author: PanConChicharron

control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp

@@ -54,11 +54,11 @@ bool QPSolverOSQP::solve(
   /* execute optimization */
   auto result = osqpsolver_.optimize(h_mat, osqpA, f, lower_bound, upper_bound);
 
-  std::vector<double> U_osqp = std::get<0>(result);
+  std::vector<double> U_osqp = result.primal_solution;
   u = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
     &U_osqp[0], static_cast<Eigen::Index>(U_osqp.size()), 1);
 
-  const int status_val = std::get<3>(result);
+  const int status_val = result.solution_status;
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str());
     return false;
@@ -71,7 +71,7 @@ bool QPSolverOSQP::solve(
   }
 
   // polish status: successful (1), unperformed (0), (-1) unsuccessful
-  int status_polish = std::get<2>(result);
+  int status_polish = result.polish_status;
   if (status_polish == -1 || status_polish == 0) {
     const auto s = (status_polish == 0) ? "Polish process is not performed in osqp."
                                         : "Polish process failed in osqp.";

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

@@ -1,4 +1,4 @@
-// Copyright 2025 Tier IV, Inc.
+// Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -16,54 +16,41 @@
 #define AUTOWARE__PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_
 
 #include "autoware/planning_evaluator/metrics_calculator.hpp"
-#include "autoware_utils/math/accumulator.hpp"
+#include "autoware/planning_evaluator/stat.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 
-#include <autoware/route_handler/route_handler.hpp>
-#include <autoware_utils/ros/polling_subscriber.hpp>
-#include <autoware_utils/system/stop_watch.hpp>
-#include <autoware_vehicle_info_utils/vehicle_info_utils.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 "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
 #include "nav_msgs/msg/odometry.hpp"
-#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
-#include <autoware_planning_msgs/msg/lanelet_route.hpp>
-#include <tier4_metric_msgs/msg/metric.hpp>
-#include <tier4_metric_msgs/msg/metric_array.hpp>
 
 #include <array>
 #include <deque>
 #include <memory>
 #include <string>
 #include <vector>
+
 namespace planning_diagnostics
 {
-using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
-using autoware_utils::Accumulator;
-using MetricMsg = tier4_metric_msgs::msg::Metric;
-using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
+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
  */
 class PlanningEvaluatorNode : public rclcpp::Node
 {
 public:
   explicit PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options);
-  ~PlanningEvaluatorNode() override;
+  ~PlanningEvaluatorNode();
 
   /**
    * @brief callback on receiving an odometry
@@ -75,8 +62,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
    * @brief callback on receiving a trajectory
    * @param [in] traj_msg received trajectory message
    */
-  void onTrajectory(
-    const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr);
+  void onTrajectory(const Trajectory::ConstSharedPtr traj_msg);
 
   /**
    * @brief callback on receiving a reference trajectory
@@ -94,81 +80,42 @@ class PlanningEvaluatorNode : public rclcpp::Node
    * @brief callback on receiving a modified goal
    * @param [in] modified_goal_msg received modified goal message
    */
-  void onModifiedGoal(
-    const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg,
-    const Odometry::ConstSharedPtr ego_state_ptr);
-
-  /**
-   * @brief add the given metric statistic
-   */
-  void AddMetricMsg(const Metric & metric, const Accumulator<double> & metric_stat);
-
-  /**
-   * @brief add current ego lane info
-   */
-  void AddLaneletMetricMsg(const Odometry::ConstSharedPtr ego_state_ptr);
+  void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg);
 
   /**
-   * @brief add current ego kinematic state
+   * @brief publish the given metric statistic
    */
-  void AddKinematicStateMetricMsg(
-    const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr);
+  DiagnosticStatus generateDiagnosticStatus(
+    const Metric & metric, const Stat<double> & metric_stat) const;
 
 private:
   static bool isFinite(const TrajectoryPoint & p);
-
-  /**
-   * @brief update route handler data
-   */
-  void getRouteData();
-
-  /**
-   * @brief fetch data and publish diagnostics
-   */
-  void onTimer();
+  void publishModifiedGoalDeviationMetrics();
 
   // ROS
-  autoware_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{this, "~/input/trajectory"};
-  autoware_utils::InterProcessPollingSubscriber<Trajectory> ref_sub_{
-    this, "~/input/reference_trajectory"};
-  autoware_utils::InterProcessPollingSubscriber<PredictedObjects> objects_sub_{
-    this, "~/input/objects"};
-  autoware_utils::InterProcessPollingSubscriber<PoseWithUuidStamped> modified_goal_sub_{
-    this, "~/input/modified_goal"};
-  autoware_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{this, "~/input/odometry"};
-  autoware_utils::InterProcessPollingSubscriber<
-    LaneletRoute, autoware_utils::polling_policy::Newest>
-    route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()};
-  autoware_utils::InterProcessPollingSubscriber<
-    LaneletMapBin, autoware_utils::polling_policy::Newest>
-    vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
-  autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
-    this, "~/input/acceleration"};
-
-  rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
-    processing_time_pub_;
-  rclcpp::Publisher<MetricArrayMsg>::SharedPtr metrics_pub_;
+  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_;
-  autoware::route_handler::RouteHandler route_handler_;
-
-  // Message to publish
-  MetricArrayMsg metrics_msg_;
 
   // Parameters
-  bool output_metrics_;
+  std::string output_file_str_;
   std::string ego_frame_str_;
 
   // Calculator
   MetricsCalculator metrics_calculator_;
   // Metrics
   std::vector<Metric> metrics_;
-  std::array<std::array<Accumulator<double>, 3>, static_cast<size_t>(Metric::SIZE)>
-    metric_accumulators_;  // 3(min, max, mean) * metric_size
+  std::deque<rclcpp::Time> stamps_;
+  std::array<std::deque<Stat<double>>, static_cast<size_t>(Metric::SIZE)> metric_stats_;
 
-  rclcpp::TimerBase::SharedPtr timer_;
-  VehicleInfo vehicle_info_;
-  std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
+  Odometry::ConstSharedPtr ego_state_ptr_;
+  PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_;
 };
 }  // namespace planning_diagnostics
 

planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp

@@ -246,10 +246,11 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
   }
 
   // execute optimization
-  const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
-  const std::vector<double> optval = std::get<0>(result);
+  const autoware::osqp_interface::OSQPResult result =
+    qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
+  const std::vector<double> optval = result.primal_solution;
 
-  const int status_val = std::get<3>(result);
+  const int status_val = result.solution_status;
   if (status_val != 1)
     std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl;
 

planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp

@@ -21,6 +21,7 @@
 #include "autoware/path_optimizer/common_structs.hpp"
 #include "autoware/path_optimizer/state_equation_generator.hpp"
 #include "autoware/path_optimizer/type_alias.hpp"
+#include "autoware/path_optimizer/utils/conditional_timer.hpp"
 #include "autoware_utils/geometry/geometry.hpp"
 #include "autoware_utils/system/time_keeper.hpp"
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
@@ -57,6 +58,15 @@ struct Bounds
     lower_bound -= offset;
     upper_bound -= offset;
   }
+
+  friend std::ostream & operator<<(std::ostream & os, const Bounds & bounds)
+  {
+    os << "Bounds: {\n";
+    os << "\tlower_bound: " << bounds.lower_bound << ",\n";
+    os << "\tupper_bound: " << bounds.upper_bound << "\n";
+    os << "}\n";
+    return os;
+  }
 };
 
 struct KinematicState
@@ -65,6 +75,15 @@ struct KinematicState
   double yaw{0.0};
 
   Eigen::Vector2d toEigenVector() const { return Eigen::Vector2d{lat, yaw}; }
+
+  friend std::ostream & operator<<(std::ostream & os, const KinematicState & state)
+  {
+    os << "KinematicState: {\n";
+    os << "\tlat: " << state.lat << ",\n";
+    os << "\tyaw: " << state.yaw << "\n";
+    os << "}\n";
+    return os;
+  }
 };
 
 struct ReferencePoint
@@ -92,6 +111,57 @@ struct ReferencePoint
 
   double getYaw() const { return tf2::getYaw(pose.orientation); }
 
+
+  friend std::ostream & operator<<(std::ostream & os, const ReferencePoint & ref_point)
+  {
+    os << "ReferencePoint: {\n";
+    os << "\tpose: " << ref_point.pose.position.x << ", " << ref_point.pose.position.y << ",\n";
+    os << "\tlongitudinal_velocity_mps: " << ref_point.longitudinal_velocity_mps << ",\n";
+    os << "\tcurvature: " << ref_point.curvature << ",\n";
+    os << "\tdelta_arc_length: " << ref_point.delta_arc_length << ",\n";
+    os << "\talpha: " << ref_point.alpha << ",\n";
+    os << "\tbounds: " << ref_point.bounds << ",\n";
+    os << "\tbeta: [";
+    for (const auto & b : ref_point.beta) {
+      os << b << ", ";
+    }
+    os << "],\n";
+    os << "\tnormalized_avoidance_cost: " << ref_point.normalized_avoidance_cost << ",\n";
+    os << "\tbounds_on_constraints: [";
+    for (const auto & b : ref_point.bounds_on_constraints) {
+      os << b << ", ";
+    }
+    os << "],\n";
+    os << "\tpose_on_constraints: [";
+    for (const auto & p : ref_point.pose_on_constraints) {
+      os << "(" << p.position.x << ", " << p.position.y << ") , ";
+    }
+    os << "],\n";
+    os << "\tfixed_kinematic_state: ";
+    if (ref_point.fixed_kinematic_state) {
+      os << *ref_point.fixed_kinematic_state;
+    } else {
+      os << "nullopt";
+    }
+    os << ",\n";
+    os << "\toptimized_kinematic_state: " << ref_point.optimized_kinematic_state
+       << ",\n";
+    os << "\toptimized_input: " << ref_point.optimized_input << ",\n";
+    os << "\tslack_variables: ";
+    if (ref_point.slack_variables) {
+      os << "[";
+      for (const auto & s : *ref_point.slack_variables) {
+        os << s << ", ";
+      }
+      os << "]";
+    } else {
+      os << "nullopt";
+    }
+    os << "\n";
+    os << "}\n";
+    return os;
+  }
+
   geometry_msgs::msg::Pose offsetDeviation(const double lat_dev, const double yaw_dev) const
   {
     auto pose_with_deviation = autoware_utils::calc_offset_pose(pose, 0.0, lat_dev, 0.0);
@@ -110,7 +180,7 @@ class MPTOptimizer
     const TrajectoryParam & traj_param, const std::shared_ptr<DebugData> debug_data_ptr,
     const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_);
 
-  std::vector<TrajectoryPoint> optimizeTrajectory(const PlannerData & planner_data);
+  std::pair<std::vector<TrajectoryPoint>, int> optimizeTrajectory(const PlannerData & planner_data);
   std::optional<std::vector<TrajectoryPoint>> getPrevOptimizedTrajectoryPoints() const;
 
   void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param);
@@ -126,19 +196,47 @@ class MPTOptimizer
   {
     Eigen::SparseMatrix<double> Q;
     Eigen::SparseMatrix<double> R;
+
+    friend std::ostream & operator<<(std::ostream & os, const ValueMatrix & matrix)
+    {
+      os << "ValueMatrix: {\n";
+      os << "\tQ: (Sparse Matrix):" << matrix.Q;
+      os << "\tR: (Sparse Matrix):" << matrix.R;
+      os << "}\n";
+      return os;
+    }
   };
 
   struct ObjectiveMatrix
   {
     Eigen::MatrixXd hessian;
     Eigen::VectorXd gradient;
+
+    friend std::ostream & operator<<(std::ostream & os, const ObjectiveMatrix & matrix)
+    {
+      os << "ObjectiveMatrix: {\n";
+      os << "\thessian:\n" << matrix.hessian << "\n";
+      os << "\tgradient:\n" << matrix.gradient << "\n";
+      os << "}\n";
+      return os;
+    }
   };
 
   struct ConstraintMatrix
   {
     Eigen::MatrixXd linear;
     Eigen::VectorXd lower_bound;
     Eigen::VectorXd upper_bound;
+    
+    friend std::ostream & operator<<(std::ostream & os, const ConstraintMatrix & matrix)
+    {
+      os << "ConstraintMatrix: {\n";
+      os << "\tlinear:\n" << matrix.linear << "\n";
+      os << "\tlower_bound:\n" << matrix.lower_bound << "\n";
+      os << "\tupper_bound:\n" << matrix.upper_bound << "\n";
+      os << "}\n";
+      return os;
+    }
   };
 
   struct MPTParam