fix(obstacle_avoidance_planner): use prev eb traj when failed (autowa…

…refoundation#1192) * fix(obstacle_avoidance_planner): use prev eb traj when failed Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * feat(obstacle_avoidance_planner): modify debug message Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix conflict Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
tzhong518 · Oct 14, 2022 · 44a20e7 · 44a20e7
1 parent dfc95d8
commit 44a20e7
Show file tree

Hide file tree

Showing 5 changed files with 47 additions and 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
@@ -30,6 +30,7 @@
 #include "boost/optional.hpp"
 
 #include <memory>
+#include <utility>
 #include <vector>
 
 class EBPathOptimizer
@@ -164,9 +165,10 @@ class EBPathOptimizer
   CandidatePoints getDefaultCandidatePoints(
     const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points);
 
-  std::vector<double> solveQP();
+  std::pair<std::vector<double>, int64_t> solveQP();
 
-  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> calculateTrajectory(
+  boost::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>
+  calculateTrajectory(
     const std::vector<geometry_msgs::msg::Point> & padded_interpolated_points,
     const std::vector<ConstrainRectangle> & constrain_rectangles, const int farthest_idx,
     std::shared_ptr<DebugData> 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
@@ -29,6 +29,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <string>
 #include <vector>
 
 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
@@ -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<autoware_auto_planning_msgs::msg::TrajectoryPoint> EBPathOptimizer::calculateTrajectory(
+boost::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>
+EBPathOptimizer::calculateTrajectory(
   const std::vector<geometry_msgs::msg::Point> & padded_interpolated_points,
   const std::vector<ConstrainRectangle> & constrain_rectangles, const int farthest_idx,
   std::shared_ptr<DebugData> debug_data_ptr)
@@ -188,7 +192,13 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> EBPathOptimizer::
   updateConstrain(padded_interpolated_points, constrain_rectangles);
 
   // solve QP and get optimized trajectory
-  std::vector<double> 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<autoware_auto_planning_msgs::msg::TrajectoryPoint> EBPathOptimizer::
   return traj_points;
 }
 
-std::vector<double> EBPathOptimizer::solveQP()
+std::pair<std::vector<double>, 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<geometry_msgs::msg::Pose> EBPathOptimizer::getFixedPoints(

diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
@@ -801,7 +801,7 @@ boost::optional<Eigen::VectorXd> 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
@@ -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