From f33514044d77c1c7b3fc743bb30841cdcb4747fe Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 7 Jul 2022 09:03:22 +0900 Subject: [PATCH] fix(obstacle_avoidance_planner): use prev eb traj when failed (#1192) * fix(obstacle_avoidance_planner): use prev eb traj when failed Signed-off-by: Takayuki Murooka * feat(obstacle_avoidance_planner): modify debug message Signed-off-by: Takayuki Murooka * fix conflict Signed-off-by: Takayuki Murooka --- .../eb_path_optimizer.hpp | 6 ++-- .../obstacle_avoidance_planner/utils.hpp | 3 +- .../src/eb_path_optimizer.cpp | 23 ++++++++---- .../src/mpt_optimizer.cpp | 2 +- .../obstacle_avoidance_planner/src/utils.cpp | 36 ++++++++++++------- 5 files changed, 47 insertions(+), 23 deletions(-) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp index a2f70cefc9e48..41f296eb053df 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp @@ -30,6 +30,7 @@ #include "boost/optional.hpp" #include +#include #include class EBPathOptimizer @@ -164,9 +165,10 @@ class EBPathOptimizer CandidatePoints getDefaultCandidatePoints( const std::vector & path_points); - std::vector solveQP(); + std::pair, int64_t> solveQP(); - std::vector calculateTrajectory( + boost::optional> + calculateTrajectory( const std::vector & padded_interpolated_points, const std::vector & constrain_rectangles, const int farthest_idx, std::shared_ptr debug_data_ptr); diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp index e9f9b1be23fb0..3072a418b47e9 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include struct ReferencePoint; @@ -336,7 +337,7 @@ bool isNearLastPathPoint( namespace utils { -void logOSQPSolutionStatus(const int solution_status); +void logOSQPSolutionStatus(const int solution_status, const std::string & msg); } // namespace utils #endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp index 0633877a0e4d4..732ea4dbc43cc 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp @@ -171,13 +171,17 @@ EBPathOptimizer::getOptimizedTrajectory( const auto traj_points = calculateTrajectory(padded_interpolated_points, rectangles.get(), farthest_idx, debug_data_ptr); + if (!traj_points) { + return boost::none; + } debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return traj_points; + return *traj_points; } -std::vector EBPathOptimizer::calculateTrajectory( +boost::optional> +EBPathOptimizer::calculateTrajectory( const std::vector & padded_interpolated_points, const std::vector & constrain_rectangles, const int farthest_idx, std::shared_ptr debug_data_ptr) @@ -188,7 +192,13 @@ std::vector EBPathOptimizer:: updateConstrain(padded_interpolated_points, constrain_rectangles); // solve QP and get optimized trajectory - std::vector optimized_points = solveQP(); + const auto result = solveQP(); + const auto optimized_points = result.first; + const auto status = result.second; + if (status != 1) { + utils::logOSQPSolutionStatus(status, "EB: "); + return boost::none; + } const auto traj_points = convertOptimizedPointsToTrajectory(optimized_points, constrain_rectangles, farthest_idx); @@ -200,17 +210,18 @@ std::vector EBPathOptimizer:: return traj_points; } -std::vector EBPathOptimizer::solveQP() +std::pair, int64_t> EBPathOptimizer::solveQP() { osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs); const auto result = osqp_solver_ptr_->optimize(); const auto optimized_points = std::get<0>(result); + const auto status = std::get<3>(result); - utils::logOSQPSolutionStatus(std::get<3>(result)); + utils::logOSQPSolutionStatus(std::get<3>(result), "EB: "); - return optimized_points; + return std::make_pair(optimized_points, status); } std::vector EBPathOptimizer::getFixedPoints( diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index a828373bcacf8..925bef756fbe6 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -801,7 +801,7 @@ boost::optional MPTOptimizer::executeOptimization( // check solution status const int solution_status = std::get<3>(result); if (solution_status != 1) { - utils::logOSQPSolutionStatus(solution_status); + utils::logOSQPSolutionStatus(solution_status, "MPT: "); return boost::none; } diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp index 3ae8832531da5..90e9daccb0a89 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -493,7 +493,7 @@ int getNearestIdx( namespace utils { -void logOSQPSolutionStatus(const int solution_status) +void logOSQPSolutionStatus(const int solution_status, const std::string & msg) { /****************** * Solver Status * @@ -514,38 +514,48 @@ void logOSQPSolutionStatus(const int solution_status) } else if (solution_status == LOCAL_OSQP_DUAL_INFEASIBLE_INACCURATE) { RCLCPP_WARN( rclcpp::get_logger("util"), - "[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE_INACCURATE"); + "[Avoidance] %s OSQP solution status: OSQP_DUAL_INFEASIBLE_INACCURATE", msg.c_str()); } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE_INACCURATE) { RCLCPP_WARN( rclcpp::get_logger("util"), - "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE_INACCURATE"); + "[Avoidance] %s OSQP solution status: OSQP_PRIMAL_INFEASIBLE_INACCURATE", msg.c_str()); } else if (solution_status == LOCAL_OSQP_SOLVED_INACCURATE) { RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SOLVED_INACCURATE"); + rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_SOLVED_INACCURATE", + msg.c_str()); } else if (solution_status == LOCAL_OSQP_MAX_ITER_REACHED) { - RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_ITER_REACHED"); + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_ITER_REACHED", + msg.c_str()); } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE) { RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE"); + rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_PRIMAL_INFEASIBLE", + msg.c_str()); } else if (solution_status == LOCAL_OSQP_DUAL_INFEASIBLE) { RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE"); + rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_DUAL_INFEASIBLE", + msg.c_str()); } else if (solution_status == LOCAL_OSQP_SIGINT) { - RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SIGINT"); + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_SIGINT", msg.c_str()); RCLCPP_WARN( rclcpp::get_logger("util"), "[Avoidance] Interrupted by user, process will be finished."); std::exit(0); } else if (solution_status == LOCAL_OSQP_TIME_LIMIT_REACHED) { RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_TIME_LIMIT_REACHED"); + rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_TIME_LIMIT_REACHED", + msg.c_str()); } else if (solution_status == LOCAL_OSQP_UNSOLVED) { - RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_UNSOLVED"); + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_UNSOLVED", + msg.c_str()); } else if (solution_status == LOCAL_OSQP_NON_CVX) { - RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_NON_CVX"); + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: OSQP_NON_CVX", msg.c_str()); } else { RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: Not defined %d", - solution_status); + rclcpp::get_logger("util"), "[Avoidance] %s OSQP solution status: Not defined %d", + msg.c_str(), solution_status); } } } // namespace utils