Make teb more robust

source/armarx/navigation/local_planning/TimedElasticBands.cpp

@@ -23,6 +23,7 @@
 #include <armarx/navigation/local_planning/ros_conversions.h>
 // #include <teb_local_planner/extension/obstacles/EllipseObstacle.h>
 #include <armarx/navigation/local_planning/aron_conversions_teb.h>
+
 #include <teb_local_planner/homotopy_class_planner.h>
 
 namespace armarx::navigation::local_planning
@@ -106,21 +107,35 @@ namespace armarx::navigation::local_planning
 
         fillObstacles();
 
+        bool success;
         try
         {
-            if (!hcp_->plan(start, end, &velocity_start, !planToDest))
-            {
-                ARMARX_WARNING << deactivateSpam(5) << "Found trajectory is not feasible!";
-            }
+            success = hcp_->plan(start, end, &velocity_start, !planToDest);
         }
         catch (...)
         {
+            hcp_->clearPlanner();
             ARMARX_ERROR << "Caugth exception while planning: " << GetHandledExceptionString();
             return std::nullopt;
         }
 
+        if (!success)
+        {
+            hcp_->clearPlanner();
+            ARMARX_WARNING << deactivateSpam(5) << "Found trajectory is not feasible!";
+            return std::nullopt;
+        }
+
+        if (hcp_->hasDiverged())
+        {
+            hcp_->clearPlanner();
+            ARMARX_WARNING << deactivateSpam(5) << "Planner has diverged!";
+            return std::nullopt;
+        }
+
         if (hcp_->getTrajectoryContainer().empty())
         {
+            hcp_->clearPlanner();
             ARMARX_INFO << deactivateSpam(5) << "Did not find any trajectory!";
             return std::nullopt;
         }