MRM when MPT fails

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

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.";

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

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

@@ -20,13 +20,15 @@
 #include "autoware/path_optimizer/mpt_optimizer.hpp"
 #include "autoware/path_optimizer/replan_checker.hpp"
 #include "autoware/path_optimizer/type_alias.hpp"
+#include "autoware/path_optimizer/utils/conditional_timer.hpp"
 #include "autoware_utils/ros/logger_level_configure.hpp"
 #include "autoware_utils/ros/polling_subscriber.hpp"
 #include "autoware_utils/system/stop_watch.hpp"
 #include "autoware_utils/system/time_keeper.hpp"
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
 #include <autoware_utils/ros/published_time_publisher.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/publisher.hpp>
 
 #include <algorithm>
@@ -68,6 +70,7 @@ class PathOptimizer : public rclcpp::Node
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_{};
   mutable std::shared_ptr<DebugData> debug_data_ptr_{nullptr};
   mutable std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_{nullptr};
+  mutable std::shared_ptr<ConditionalTimer> conditional_timer_{nullptr};
 
   // flags for some functions
   bool enable_pub_debug_marker_;
@@ -76,6 +79,7 @@ class PathOptimizer : public rclcpp::Node
   bool enable_outside_drivable_area_stop_;
   bool enable_skip_optimization_;
   bool enable_reset_prev_optimization_;
+  bool mrm_required_{false};
   bool use_footprint_polygon_for_outside_drivable_area_check_;
 
   // core algorithms
@@ -123,6 +127,7 @@ class PathOptimizer : public rclcpp::Node
     const std::vector<TrajectoryPoint> & traj_points,
     const std::vector<TrajectoryPoint> & optimized_points) const;
   void publishDebugData(const Header & header) const;
+  void onCheckPathOptimizationValid(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
   // functions in generateOptimizedTrajectory
   std::vector<TrajectoryPoint> optimizeTrajectory(const PlannerData & planner_data);
@@ -145,6 +150,9 @@ class PathOptimizer : public rclcpp::Node
   std::unique_ptr<autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
   autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
+  
+  // diag
+  diagnostic_updater::Updater updater_{this};
 };
 }  // namespace autoware::path_optimizer
 

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

@@ -37,6 +37,16 @@ class StateEquationGenerator
     Eigen::VectorXd W;
   };
 
+  friend std::ostream & operator<<(std::ostream & os, const Matrix & matrix)
+  {
+    os << "Matrix: {\n";
+    os << "\tA:\n" << matrix.A << "\n";
+    os << "\tB:\n" << matrix.B << "\n";
+    os << "\tW:\n" << matrix.W << "\n";
+    os << "}\n";
+    return os;
+  }
+  
   StateEquationGenerator() = default;
   StateEquationGenerator(
     const double wheel_base, const double max_steer_rad,

planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/conditional_timer.hpp

@@ -0,0 +1,48 @@
+// Copyright 2023 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <chrono>
+#include <optional>
+
+class ConditionalTimer
+{
+public:
+  void update(bool condition)
+  {
+    if (condition && !start_time_.has_value()) {
+      // Condition met, start the timer
+      start_time_ = std::chrono::high_resolution_clock::now();
+    } else if (!condition && start_time_.has_value()) {
+      // Condition no longer met, stop the timer
+      start_time_ = std::nullopt;
+    }
+  }
+
+  std::chrono::duration<double> getElapsedTime() const
+  {
+    if (start_time_.has_value()) {
+      auto current_time = std::chrono::high_resolution_clock::now();
+      return current_time - *start_time_;
+    } else {
+      return std::chrono::duration<double>(0.0);
+      ;
+    }
+  }
+
+private:
+  std::optional<std::chrono::time_point<std::chrono::high_resolution_clock>> start_time_{
+    std::nullopt};
+};