diff --git a/libselfdriving/src/algos/refine_trajectory.cpp b/libselfdriving/src/algos/refine_trajectory.cpp index 1e82c65..b7e60cf 100644 --- 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 index 6cbb171..8b8d85d 100644 --- a/libselfdriving/src/ptgs/HolonomicBlend.cpp +++ b/libselfdriving/src/ptgs/HolonomicBlend.cpp @@ -21,6 +21,8 @@ #include #include +#include // 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