refactor(osqp_interface, motion_velocity_smoother): unsolved status log

Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Oct 15, 2022 · b2614d5 · b2614d5
1 parent 0f16c2b
commit b2614d5
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 12 deletions.
diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp
@@ -21,6 +21,8 @@
 #include "osqp_interface/csc_matrix_conv.hpp"
 #include "osqp_interface/visibility_control.hpp"
 
+#include <rclcpp/rclcpp.hpp>
+
 #include <limits>
 #include <memory>
 #include <string>
@@ -179,6 +181,8 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
   inline float64_t getObjVal() const { return m_latest_work_info.obj_val; }
   /// \brief Returns flag asserting interface condition (Healthy condition: 0).
   inline int64_t getExitFlag() const { return m_exitflag; }
+
+  void logUnsolvedStatus(const std::string & prefix_message = "") const;
 };
 
 }  // namespace osqp

diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp
@@ -335,6 +335,26 @@ OSQPInterface::optimize(
   return result;
 }
 
+void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const
+{
+  const int status = getStatus();
+  if (status == 1) {
+    // No need to log since optimziatoin was solved.
+    return;
+  }
+
+  // create message
+  std::string output_message = "";
+  if (prefix_message != "") {
+    output_message = prefix_message + " ";
+  }
+
+  const auto status_message = getStatusMessage();
+  output_message += "Optimization failed due to " + status_message;
+
+  // log with warning
+  RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str());
+}
 }  // namespace osqp
 }  // namespace common
 }  // namespace autoware
diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
@@ -304,10 +304,7 @@ bool JerkFilteredSmoother::apply(
     output.at(i).acceleration_mps2 = a_stop_decel;
   }
 
-  const int status_val = std::get<3>(result);
-  if (status_val != 1) {
-    RCLCPP_ERROR(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
-  }
+  qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]");
 
   if (TMP_SHOW_DEBUG_INFO) {
     // jerk calculation

diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
@@ -208,10 +208,7 @@ bool L2PseudoJerkSmoother::apply(
   //     v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N));
   // }
 
-  const int status_val = std::get<3>(result);
-  if (status_val != 1) {
-    RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
-  }
+  qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]");
 
   const auto tf2 = std::chrono::system_clock::now();
   const double dt_ms2 =

diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
@@ -221,10 +221,7 @@ bool LinfPseudoJerkSmoother::apply(
   //     v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N));
   // }
 
-  const int status_val = std::get<3>(result);
-  if (status_val != 1) {
-    RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
-  }
+  qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]");
 
   const auto tf2 = std::chrono::system_clock::now();
   const double dt_ms2 =