From 22bebafdbbfe29000f273a029dab4726b5d95a49 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@eurekarobotics.com>
Date: Wed, 13 Sep 2023 10:46:11 +0200
Subject: [PATCH] feat: warn when solver fails.

---
 HISTORY.md                                       |  1 +
 cpp/src/toppra/algorithm/toppra.cpp              |  2 +-
 .../geometric_path/piecewise_poly_path.cpp       | 16 ++++++++++------
 cpp/src/toppra/solver/seidel.cpp                 |  8 +++++++-
 4 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/HISTORY.md b/HISTORY.md
index 5ec17802..43ad6218 100644
--- a/HISTORY.md
+++ b/HISTORY.md
@@ -5,6 +5,7 @@
 ### Changed
 
 - [cpp]: fix numerical issue in Seidel solver for 1D problems. (#232)
+- [cpp]: warn when solver fails. (#241)
 
 ## 0.6.0 (Mar 08 2023)
 
diff --git a/cpp/src/toppra/algorithm/toppra.cpp b/cpp/src/toppra/algorithm/toppra.cpp
index efd7b0af..56de465b 100644
--- a/cpp/src/toppra/algorithm/toppra.cpp
+++ b/cpp/src/toppra/algorithm/toppra.cpp
@@ -29,7 +29,7 @@ ReturnCode TOPPRA::computeForwardPass(value_type vel_start) {
     solver_ret = m_solver->solveStagewiseOptim(i, H, g_upper, x, x_next, solution);
     if (!solver_ret) {
       ret = ReturnCode::ERR_FAIL_FORWARD_PASS;
-      TOPPRA_LOG_DEBUG("Fail: forward pass, idx: " << i);
+      TOPPRA_LOG_WARN("Fail: forward pass, idx: " << i);
       break;
     }
     /// \todo This can be optimized further by solving a 1D problem instead of 2D
diff --git a/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp b/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp
index abda1077..36ea59c5 100644
--- a/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp
+++ b/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp
@@ -1,12 +1,15 @@
+#include <toppra/geometric_path/piecewise_poly_path.hpp>
+
 #include <iostream>
+#include <iomanip>
 #include <ostream>
 #include <stdexcept>
 #include <string>
-#include <toppra/geometric_path/piecewise_poly_path.hpp>
-#include <toppra/toppra.hpp>
-#include <Eigen/Dense>
 #include <array>
 
+#include <Eigen/Dense>
+#include <toppra/toppra.hpp>
+
 #ifdef TOPPRA_OPT_MSGPACK
 #include <msgpack.hpp>
 #endif
@@ -238,10 +241,11 @@ Vectors PiecewisePolyPath::eval(const Vector &positions, int order) const {
 }
 
 size_t PiecewisePolyPath::findSegmentIndex(value_type pos) const {
-  if (pos > m_breakpoints[m_breakpoints.size()-1] || pos < m_breakpoints[0]) {
+  if (pos > m_breakpoints.back() || pos < m_breakpoints[0]) {
+    constexpr auto max_precision {std::numeric_limits<value_type>::digits10 + 1};
     std::ostringstream oss;
-    oss << "Position " << pos << " is outside of range [ " << m_breakpoints[0]
-      << ", " << m_breakpoints[m_breakpoints.size()-1] << ']';
+    oss << std::setprecision(max_precision) << "Position " << pos <<
+      " is outside of range [ " << m_breakpoints[0] << ", " << m_breakpoints.back() << ']';
     throw std::runtime_error(oss.str());
   }
   auto it = std::upper_bound(m_breakpoints.begin(), m_breakpoints.end(), pos);
diff --git a/cpp/src/toppra/solver/seidel.cpp b/cpp/src/toppra/solver/seidel.cpp
index 83dade5c..303263d6 100644
--- a/cpp/src/toppra/solver/seidel.cpp
+++ b/cpp/src/toppra/solver/seidel.cpp
@@ -342,7 +342,13 @@ bool Seidel::solveStagewiseOptim(std::size_t i,
         active_c[k] = m_index_map[lpsol.active_c[k]];
     return true;
   }
-  TOPPRA_LOG_DEBUG("Seidel: solver fails (upper ? " << upper << ')');
+  TOPPRA_LOG_WARN("Seidel: solver fails"
+      "\ni = " << i <<
+      "\ng = " << g.transpose() <<
+      "\nx = " << x.transpose() <<
+      "\nxNext = " << xNext.transpose() <<
+      "\n"
+  );
   return false;
 }