show more info upon exception

MRPT · Nov 29, 2022 · 11b14df · 11b14df
1 parent 322b0cc
commit 11b14df
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 3 deletions.
diff --git a/libselfdriving/src/algos/refine_trajectory.cpp b/libselfdriving/src/algos/refine_trajectory.cpp
@@ -53,7 +53,16 @@ void selfdriving::refine_trajectory(
 
         const bool ok = ptg->inverseMap_WS2TP(
             deltaNodes.x, deltaNodes.y, newK, newNormDist);
-        ASSERT_(ok);
+        if (!ok)
+        {
+            std::stringstream ss;
+            ss << "Assert failed: ptg->inverseMap_WS2TP() => returned "
+                  "ok=false. More info:\n";
+            ss << " - PTG: " << ptg->getDescription() << "\n";
+            ss << " - deltaNodes: " << deltaNodes.asString() << "\n";
+            ss << " - edge: " << edge.asString() << "\n";
+            THROW_EXCEPTION(ss.str());
+        }
 
         distance_t newDist = newNormDist * ptg->getRefDistance();
 

diff --git a/libselfdriving/src/ptgs/HolonomicBlend.cpp b/libselfdriving/src/ptgs/HolonomicBlend.cpp
@@ -21,6 +21,8 @@
 #include <mrpt/system/CTimeLogger.h>
 #include <selfdriving/ptgs/HolonomicBlend.h>
 
+#include <iostream>  // debug only, remove!
+
 using namespace mrpt::nav;
 using namespace selfdriving::ptg;
 using namespace mrpt::system;
@@ -272,8 +274,8 @@ void HolonomicBlend::saveToConfigFile(
 std::string HolonomicBlend::getDescription() const
 {
     return mrpt::format(
-        "PTG_Holo_Blend_Tramp=%.03f_Vmax=%.03f_Wmax=%.03f", T_ramp_max, V_MAX,
-        W_MAX);
+        "selfdriving_HolonomicBlend=%.03f_Vmax=%.03f_Wmax=%.03f", T_ramp_max,
+        V_MAX, W_MAX);
 }
 
 void HolonomicBlend::serializeFrom(
@@ -397,6 +399,7 @@ bool HolonomicBlend::inverseMap_WS2TP(
         const double       found_dist  = this->getPathDist(out_k, solved_step);
 
         out_d = found_dist / this->refDistance;
+
         return true;
     }
     else