diff --git a/common/osqp_interface/design/osqp_interface-design.md b/common/osqp_interface/design/osqp_interface-design.md index e9ff4a2a3bc3a..90c6eb7e5068b 100644 --- a/common/osqp_interface/design/osqp_interface-design.md +++ b/common/osqp_interface/design/osqp_interface-design.md @@ -54,7 +54,7 @@ The interface can be used in several ways: ```cpp std::tuple, std::vector> result = osqp_interface.optimize(); - std::vector param = std::get<0>(result); + std::vector param = result.primal_solution; double x_0 = param[0]; double x_1 = param[1]; ``` diff --git a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp index a919bc1f1c038..a9036b1b57e3c 100644 --- a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp +++ b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp @@ -37,6 +37,44 @@ struct OSQP_INTERFACE_PUBLIC CSC_Matrix std::vector m_row_idxs; /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') std::vector m_col_idxs; + + friend std::ostream & operator<<(std::ostream & os, const CSC_Matrix & matrix) + { + os << "CSC_Matrix: {\n"; + os << "\tm_vals: ["; + + // Iterator-based loop for m_vals + for (auto it = std::begin(matrix.m_vals); it != std::end(matrix.m_vals); ++it) { + os << *it; // Print the current element (dereference iterator) + if (std::next(it) != std::end(matrix.m_vals)) { // Check if not the last element + os << ", "; + } + } + os << "],\n"; + + os << "\tm_row_idxs: ["; + // Iterator-based loop for m_row_idxs + for (auto it = std::begin(matrix.m_row_idxs); it != std::end(matrix.m_row_idxs); ++it) { + os << *it; + if (std::next(it) != std::end(matrix.m_row_idxs)) { + os << ", "; + } + } + os << "],\n"; + + os << "\tm_col_idxs: ["; + // Iterator-based loop for m_col_idxs + for (auto it = std::begin(matrix.m_col_idxs); it != std::end(matrix.m_col_idxs); ++it) { + os << *it; + if (std::next(it) != std::end(matrix.m_col_idxs)) { + os << ", "; + } + } + os << "]\n"; + + os << "}\n"; + return os; + } }; /// \brief Calculate CSC matrix from Eigen matrix diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp index f126ba9329d3e..d9c2c0f2c091b 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp @@ -36,6 +36,16 @@ namespace osqp { constexpr c_float INF = 1e30; +struct OSQPResult +{ + std::vector primal_solution; + std::vector lagrange_multipliers; + int polish_status; + int solution_status; + int iteration_status; + int exit_flag; +}; + /** * Implementation of a native C++ interface for the OSQP solver. * @@ -56,7 +66,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface int64_t m_exitflag; // Runs the solver on the stored problem. - std::tuple, std::vector, int64_t, int64_t, int64_t> solve(); + OSQPResult solve(); static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; @@ -97,10 +107,10 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface /// \details std::tuple, std::vector> result; /// \details result = osqp_interface.optimize(); /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); + /// \details std::vector param = result.primal_solution; /// \details double x_0 = param[0]; /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize(); + OSQPResult optimize(); /// \brief Solves convex quadratic programs (QPs) using the OSQP solver. /// \return The function returns a tuple containing the solution as two float vectors. @@ -115,10 +125,10 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface /// \details std::tuple, std::vector> result; /// \details result = osqp_interface.optimize(P, A, q, l, u, 1e-6); /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); + /// \details std::vector param = result.primal_solution; /// \details double x_0 = param[0]; /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize( + OSQPResult optimize( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u); diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp index f0dbc3ab22e4a..dc35f8bbce885 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/osqp_interface/src/osqp_interface.cpp @@ -363,11 +363,10 @@ int64_t OSQPInterface::initializeProblem( return m_exitflag; } -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::solve() +OSQPResult OSQPInterface::solve() { // Solve Problem - osqp_solve(m_work.get()); + int32_t exit_flag = osqp_solve(m_work.get()); /******************** * EXTRACT SOLUTION @@ -382,25 +381,28 @@ OSQPInterface::solve() int64_t status_iteration = m_work->info->iter; // Result tuple - std::tuple, std::vector, int64_t, int64_t, int64_t> result = - std::make_tuple( - sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration); + OSQPResult result; + + result.primal_solution = sol_primal; + result.lagrange_multipliers = sol_lagrange_multiplier; + result.polish_status = status_polish; + result.solution_status = status_solution; + result.iteration_status = status_iteration; + result.exit_flag = exit_flag; m_latest_work_info = *(m_work->info); return result; } -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize() +OSQPResult OSQPInterface::optimize() { // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); + OSQPResult result = solve(); return result; } -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize( +OSQPResult OSQPInterface::optimize( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u) { @@ -408,7 +410,7 @@ OSQPInterface::optimize( initializeProblem(P, A, q, l, u); // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); + OSQPResult result = solve(); m_work.reset(); m_work_initialized = false; diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp index caa89c79d08ea..38083c76ef94d 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/osqp_interface/test/test_osqp_interface.cpp @@ -44,25 +44,24 @@ TEST(TestOsqpInterface, BasicQp) using autoware::common::osqp::calCSCMatrixTrapezoidal; using autoware::common::osqp::CSC_Matrix; - auto check_result = - [](const std::tuple, std::vector, int, int, int> & result) { - EXPECT_EQ(std::get<2>(result), 1); // polish succeeded - EXPECT_EQ(std::get<3>(result), 1); // solution succeeded - - static const auto ep = 1.0e-8; - - const auto prime_val = std::get<0>(result); - ASSERT_EQ(prime_val.size(), size_t(2)); - EXPECT_NEAR(prime_val[0], 0.3, ep); - EXPECT_NEAR(prime_val[1], 0.7, ep); - - const auto dual_val = std::get<1>(result); - ASSERT_EQ(dual_val.size(), size_t(4)); - EXPECT_NEAR(dual_val[0], -2.9, ep); - EXPECT_NEAR(dual_val[1], 0.0, ep); - EXPECT_NEAR(dual_val[2], 0.2, ep); - EXPECT_NEAR(dual_val[3], 0.0, ep); - }; + auto check_result = [](const autoware::common::osqp::OSQPResult & result) { + EXPECT_EQ(result.polish_status, 1); // polish succeeded + EXPECT_EQ(result.solution_status, 1); // solution succeeded + + static const auto ep = 1.0e-8; + + const auto prime_val = result.primal_solution; + ASSERT_EQ(prime_val.size(), size_t(2)); + EXPECT_NEAR(prime_val[0], 0.3, ep); + EXPECT_NEAR(prime_val[1], 0.7, ep); + + const auto dual_val = result.lagrange_multipliers; + ASSERT_EQ(dual_val.size(), size_t(4)); + EXPECT_NEAR(dual_val[0], -2.9, ep); + EXPECT_NEAR(dual_val[1], 0.0, ep); + EXPECT_NEAR(dual_val[2], 0.2, ep); + EXPECT_NEAR(dual_val[3], 0.0, ep); + }; const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); @@ -73,20 +72,19 @@ TEST(TestOsqpInterface, BasicQp) { // Define problem during optimization autoware::common::osqp::OSQPInterface osqp; - std::tuple, std::vector, int, int, int> result = - osqp.optimize(P, A, q, l, u); + autoware::common::osqp::OSQPResult result = osqp.optimize(P, A, q, l, u); check_result(result); } { // Define problem during initialization autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = osqp.optimize(); + autoware::common::osqp::OSQPResult result = osqp.optimize(); check_result(result); } { - std::tuple, std::vector, int, int, int> result; + autoware::common::osqp::OSQPResult result; // Dummy initial problem Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); @@ -107,12 +105,12 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = osqp.optimize(); + autoware::common::osqp::OSQPResult result = osqp.optimize(); check_result(result); } { - std::tuple, std::vector, int, int, int> result; + autoware::common::osqp::OSQPResult result; // Dummy initial problem with csc matrix CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); @@ -132,7 +130,7 @@ TEST(TestOsqpInterface, BasicQp) // add warm startup { - std::tuple, std::vector, int, int, int> result; + autoware::common::osqp::OSQPResult result; // Dummy initial problem with csc matrix CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); @@ -150,8 +148,8 @@ TEST(TestOsqpInterface, BasicQp) check_result(result); osqp.updateCheckTermination(1); - const auto primal_val = std::get<0>(result); - const auto dual_val = std::get<1>(result); + const auto primal_val = result.primal_solution; + const auto dual_val = result.lagrange_multipliers; for (size_t i = 0; i < primal_val.size(); ++i) { std::cerr << primal_val.at(i) << std::endl; } diff --git a/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index 66a7dcacbeae1..ee1d48d8522d7 100644 --- a/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/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 U_osqp = std::get<0>(result); + std::vector U_osqp = result.primal_solution; u = Eigen::Map>( &U_osqp[0], static_cast(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."; diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp index 8e31411bfb4af..ae38cf98f6d65 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp @@ -244,10 +244,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 optval = std::get<0>(result); + const autoware::common::osqp::OSQPResult result = + qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + const std::vector 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; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 3443ab663b642..8b5fe1fddae17 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -18,6 +18,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/universe_utils/geometry/geometry.hpp" #include "autoware/universe_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,60 @@ 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) { + if (b) { + os << *b << ", "; + } else { + os << "nullopt, "; + } + } + 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; // Assuming KinematicState is printable + } else { + os << "nullopt"; + } + os << ",\n"; + os << "\toptimized_kinematic_state: " << ref_point.optimized_kinematic_state + << ",\n"; // Assuming KinematicState is printable + 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::universe_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); @@ -110,7 +183,7 @@ class MPTOptimizer const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, const std::shared_ptr time_keeper_); - std::vector optimizeTrajectory(const PlannerData & planner_data); + std::optional> optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param); @@ -126,12 +199,30 @@ class MPTOptimizer { Eigen::SparseMatrix Q; Eigen::SparseMatrix 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 @@ -139,6 +230,16 @@ class MPTOptimizer 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 diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index 6f75c649e02b2..4fc8c9cc339cf 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -20,12 +20,14 @@ #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/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include +#include #include #include @@ -67,6 +69,7 @@ class PathOptimizer : public rclcpp::Node autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; mutable std::shared_ptr time_keeper_{nullptr}; + mutable std::shared_ptr conditional_timer_{nullptr}; // flags for some functions bool enable_pub_debug_marker_; @@ -75,6 +78,7 @@ class PathOptimizer : public rclcpp::Node bool enable_outside_drivable_area_stop_; bool enable_skip_optimization_; bool enable_reset_prev_optimization_; + bool is_optimization_failed_{false}; bool use_footprint_polygon_for_outside_drivable_area_check_; // core algorithms @@ -123,6 +127,7 @@ class PathOptimizer : public rclcpp::Node const std::vector & traj_points, const std::vector & optimized_points) const; void publishDebugData(const Header & header) const; + void onCheckPathOptimizationValid(diagnostic_updater::DiagnosticStatusWrapper & stat); // functions in generateOptimizedTrajectory std::vector optimizeTrajectory(const PlannerData & planner_data); @@ -143,6 +148,9 @@ class PathOptimizer : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; + + // diag + diagnostic_updater::Updater updater_{this}; }; } // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp index 1d23d7788dca1..c02f193f5120a 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp @@ -35,6 +35,16 @@ class StateEquationGenerator Eigen::MatrixXd A; Eigen::MatrixXd B; 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; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/conditional_timer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/conditional_timer.hpp new file mode 100644 index 0000000000000..48e8e8c3273d8 --- /dev/null +++ b/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 +#include + +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 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(0.0); + ; + } + } + +private: + std::optional> start_time_{ + std::nullopt}; +}; diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 7697760fb960c..05443db8f825d 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -23,6 +23,8 @@ #include "interpolation/spline_interpolation_points_2d.hpp" #include "tf2/utils.h" +#include + #include #include #include @@ -122,6 +124,18 @@ std::vector toStdVector(const Eigen::VectorXd & eigen_vec) return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()}; } +std::string vectorToString(const std::vector & vec) +{ + std::ostringstream oss; + if (!vec.empty()) { + oss << vec.front(); // Add the first element + for (size_t i = 1; i < vec.size(); ++i) { + oss << "," << vec[i]; // Add the rest with commas + } + } + return oss.str(); +} + bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos) { const double base_theta = tf2::getYaw(pose.orientation); @@ -468,31 +482,20 @@ void MPTOptimizer::onParam(const std::vector & parameters) debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num; } -std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) +std::optional> MPTOptimizer::optimizeTrajectory( + const PlannerData & planner_data) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; const auto & traj_points = p.traj_points; - const auto get_prev_optimized_traj_points = [&]() { - if (prev_optimized_traj_points_ptr_) { - RCLCPP_WARN(logger_, "return the previous optimized_trajectory as exceptional behavior."); - return *prev_optimized_traj_points_ptr_; - } - RCLCPP_WARN( - logger_, - "Try to return the previous optimized_trajectory as exceptional behavior, " - "but this failure also. Then return path_smoother output."); - return traj_points; - }; - // 1. calculate reference points auto ref_points = calcReferencePoints(planner_data, traj_points); if (ref_points.size() < 2) { RCLCPP_INFO_EXPRESSION( logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2."); - return get_prev_optimized_traj_points(); + return std::nullopt; } // 2. calculate B and W matrices where x = B u + W @@ -511,14 +514,26 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData const auto optimized_variables = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat); if (!optimized_variables) { RCLCPP_WARN(logger_, "return std::nullopt since could not solve qp"); - return get_prev_optimized_traj_points(); + RCLCPP_INFO(logger_, "Inputs causing failure:"); + RCLCPP_INFO_STREAM(logger_, "ref_points: "); + + for (const auto & ref_point : ref_points) { + std::cout << "\t" << ref_point << "\n"; + } + + RCLCPP_INFO_STREAM(logger_, "mpt_mat: " << mpt_mat); + RCLCPP_INFO_STREAM(logger_, "val_mat: " << val_mat); + RCLCPP_INFO_STREAM(logger_, "obj_mat: " << obj_mat); + RCLCPP_INFO_STREAM(logger_, "const_mat: " << const_mat); + + return std::nullopt; } // 7. convert to points with validation - const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_variables, mpt_mat); + auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_variables, mpt_mat); if (!mpt_traj_points) { RCLCPP_WARN(logger_, "return std::nullopt since lateral or yaw error is too large."); - return get_prev_optimized_traj_points(); + return std::nullopt; } // 8. publish trajectories for debug @@ -529,7 +544,7 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData prev_optimized_traj_points_ptr_ = std::make_shared>(*mpt_traj_points); - return *mpt_traj_points; + return mpt_traj_points; } std::optional> MPTOptimizer::getPrevOptimizedTrajectoryPoints() const @@ -1497,6 +1512,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const Eigen::MatrixXd & H = updated_obj_mat.hessian; const Eigen::MatrixXd & A = updated_const_mat.linear; const auto f = toStdVector(updated_obj_mat.gradient); + const auto upper_bound = toStdVector(updated_const_mat.upper_bound); const auto lower_bound = toStdVector(updated_const_mat.lower_bound); @@ -1525,29 +1541,66 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( // solve qp time_keeper_->start_track("solveOsqp"); - const auto result = osqp_solver_ptr_->optimize(); + const autoware::common::osqp::OSQPResult osqp_result = osqp_solver_ptr_->optimize(); time_keeper_->end_track("solveOsqp"); // check solution status - const int solution_status = std::get<3>(result); + const int solution_status = osqp_result.solution_status; prev_solution_status_ = solution_status; if (solution_status != 1) { + const std::vector lagrange_multiplier = osqp_result.lagrange_multipliers; + const int status_polish = osqp_result.polish_status; + const int exit_flag = osqp_result.exit_flag; + + RCLCPP_INFO(logger_, "optimization failed: result contains NaN values"); + RCLCPP_INFO(logger_, "Problem details:"); + RCLCPP_INFO_STREAM(logger_, "P_csc: " << P_csc); + RCLCPP_INFO_STREAM(logger_, "f: " << vectorToString(f)); + RCLCPP_INFO_STREAM(logger_, "A_csc: " << A_csc); + RCLCPP_INFO_STREAM(logger_, "lower_bound: " << vectorToString(lower_bound)); + RCLCPP_INFO_STREAM(logger_, "upper_bound: " << vectorToString(upper_bound)); + RCLCPP_INFO(logger_, "Solver details:"); + RCLCPP_INFO_STREAM(logger_, "lagrange_multiplier: " << vectorToString(lagrange_multiplier)); + RCLCPP_INFO_STREAM(logger_, "status_polish: " << status_polish); + RCLCPP_INFO_STREAM(logger_, "solution_status: " << solution_status); + RCLCPP_INFO_STREAM(logger_, "exit_flag: " << exit_flag); + osqp_solver_ptr_->logUnsolvedStatus("[MPT]"); return std::nullopt; } // print iteration - const int iteration_status = std::get<4>(result); + const int iteration_status = osqp_result.iteration_status; RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "iteration: %d", iteration_status); // get optimization result auto optimization_result = - std::get<0>(result); // NOTE: const cannot be added due to the next operation. + osqp_result.primal_solution; // NOTE: const cannot be added due to the next operation. + const auto has_nan = std::any_of( optimization_result.begin(), optimization_result.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { - RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + const std::vector lagrange_multiplier = osqp_result.lagrange_multipliers; + const int status_polish = osqp_result.polish_status; + const int exit_flag = osqp_result.exit_flag; + + RCLCPP_INFO(logger_, "optimization failed: result contains NaN values"); + RCLCPP_INFO(logger_, "Problem details:"); + RCLCPP_INFO_STREAM(logger_, "P_csc: " << P_csc); + RCLCPP_INFO_STREAM(logger_, "f: " << vectorToString(f)); + RCLCPP_INFO_STREAM(logger_, "A_csc: " << A_csc); + RCLCPP_INFO_STREAM(logger_, "lower_bound: " << vectorToString(lower_bound)); + RCLCPP_INFO_STREAM(logger_, "upper_bound: " << vectorToString(upper_bound)); + RCLCPP_INFO(logger_, "Solver details:"); + RCLCPP_INFO_STREAM(logger_, "optimization_result: " << vectorToString(optimization_result)); + RCLCPP_INFO_STREAM(logger_, "lagrange_multiplier: " << vectorToString(lagrange_multiplier)); + RCLCPP_INFO_STREAM(logger_, "status_polish: " << status_polish); + RCLCPP_INFO_STREAM(logger_, "solution_status: " << solution_status); + RCLCPP_INFO_STREAM(logger_, "iteration_status: " << iteration_status); + RCLCPP_INFO_STREAM(logger_, "exit_flag: " << exit_flag); + return std::nullopt; } const Eigen::VectorXd optimized_variables = diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 7c73d1ad1fee3..98140dc7e9c2a 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -87,7 +87,8 @@ std::vector calcSegmentLengthVector(const std::vector & PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), - debug_data_ptr_(std::make_shared()) + debug_data_ptr_(std::make_shared()), + conditional_timer_(std::make_shared()) { // interface publisher traj_pub_ = create_publisher("~/output/path", 1); @@ -135,6 +136,13 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) traj_param_ = TrajectoryParam(this); } + // Diagnostics + { + updater_.setHardwareID("path_optimizer"); + updater_.add( + "path_optimizer_emergency_stop", this, &PathOptimizer::onCheckPathOptimizationValid); + } + time_keeper_ = std::make_shared(debug_processing_time_detail_pub_); @@ -267,6 +275,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) // 5. publish debug data publishDebugData(planner_data.header); + updater_.force_update(); // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time @@ -301,6 +310,21 @@ bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const return true; } +void PathOptimizer::onCheckPathOptimizationValid(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + const std::string error_msg = "[Path Optimizer]: MPT Optimizer has failed"; + if (is_optimization_failed_) { + const std::string error_msg = + "[Path Optimizer]: Emergency Brake due to prolonged Path Optimization failure"; + const auto diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.summary(diag_level, error_msg); + } else { + const std::string error_msg = "[Path Optimizer]: MPT Optimizer successful"; + const auto diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.summary(diag_level, error_msg); + } +} + PlannerData PathOptimizer::createPlannerData( const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const { @@ -322,22 +346,15 @@ std::vector PathOptimizer::generateOptimizedTrajectory( { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto & input_traj_points = planner_data.traj_points; - // 1. calculate trajectory with MPT // NOTE: This function may return previously optimized trajectory points. // Also, velocity on some points will not be updated for a logic purpose. auto optimized_traj_points = optimizeTrajectory(planner_data); - // 2. update velocity - // NOTE: When optimization failed or is skipped, velocity in trajectory points must - // be updated since velocity in input trajectory (path) may change. - applyInputVelocity(optimized_traj_points, input_traj_points, planner_data.ego_pose); - - // 3. insert zero velocity when trajectory is over drivable area + // 2. insert zero velocity when trajectory is over drivable area insertZeroVelocityOutsideDrivableArea(planner_data, optimized_traj_points); - // 4. publish debug marker + // 3. publish debug marker publishDebugMarkerOfOptimization(optimized_traj_points); return optimized_traj_points; @@ -346,6 +363,7 @@ std::vector PathOptimizer::generateOptimizedTrajectory( std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + is_optimization_failed_ = false; const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -372,7 +390,23 @@ std::vector PathOptimizer::optimizeTrajectory(const PlannerData // with model predictive trajectory const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data); - return mpt_traj; + const bool optimized_traj_failed = !static_cast(mpt_traj); + + conditional_timer_->update(optimized_traj_failed); + + const double elapsed_time = conditional_timer_->getElapsedTime().count(); + const bool elapsed_time_over_three_seconds = (elapsed_time > 3.0); + + auto optimized_traj_points = + optimized_traj_failed && elapsed_time_over_three_seconds ? p.traj_points : std::move(*mpt_traj); + is_optimization_failed_ = optimized_traj_failed && elapsed_time_over_three_seconds; + + // 3. update velocity + // NOTE: When optimization failed or is skipped, velocity in trajectory points must + // be updated since velocity in input trajectory (path) may change. + applyInputVelocity(optimized_traj_points, p.traj_points, planner_data.ego_pose); + + return optimized_traj_points; } std::vector PathOptimizer::getPrevOptimizedTrajectory( diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index 4222e0fe98438..93bd96558dd97 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -34,9 +34,9 @@ Eigen::SparseMatrix makePMatrix(const int num_points) { std::vector> triplet_vec; const auto assign_value_to_triplet_vec = - [&](const double row, const double colum, const double value) { - triplet_vec.push_back(Eigen::Triplet(row, colum, value)); - triplet_vec.push_back(Eigen::Triplet(row + num_points, colum + num_points, value)); + [&](const double row, const double column, const double value) { + triplet_vec.push_back(Eigen::Triplet(row, column, value)); + triplet_vec.push_back(Eigen::Triplet(row + num_points, column + num_points, value)); }; for (int r = 0; r < num_points; ++r) { @@ -402,9 +402,9 @@ std::optional> EBPathSmoother::calcSmoothedTrajectory() // solve QP const auto result = osqp_solver_ptr_->optimize(); - const auto optimized_points = std::get<0>(result); + const auto optimized_points = result.primal_solution; - const auto status = std::get<3>(result); + const auto status = result.solution_status; // check status if (status != 1) { diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index 678c8e01bf67e..c46be38cc4606 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -192,8 +192,8 @@ bool L2PseudoJerkSmoother::apply( // [b0, b1, ..., bN, | a0, a1, ..., aN, | // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] - const std::vector optval = std::get<0>(result); - const int status_val = std::get<3>(result); + const std::vector optval = result.primal_solution; + const int status_val = result.solution_status; if (status_val != 1) { RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); return false; diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 60345ff0fa6f4..c3c92263b5e8d 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -207,8 +207,8 @@ bool LinfPseudoJerkSmoother::apply( // [b0, b1, ..., bN, | a0, a1, ..., aN, | // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] - const std::vector optval = std::get<0>(result); - const int status_val = std::get<3>(result); + const std::vector optval = result.primal_solution; + const int status_val = result.solution_status; if (status_val != 1) { RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); return false;