Update logging inside teb

source/armarx/navigation/local_planning/TimedElasticBands.cpp

@@ -87,6 +87,7 @@ namespace armarx::navigation::local_planning
     std::optional<LocalPlannerResult>
     TimedElasticBands::plan(const core::GlobalTrajectory& goal)
     {
+        TIMING_START(TEB_PLAN);
         const core::Pose currentPose{scene.robot->getGlobalPose()};
 
         // prune global trajectory
@@ -112,13 +113,13 @@ namespace armarx::navigation::local_planning
         }
         catch (std::exception& e)
         {
-            ARMARX_DEBUG << "Caugth exception while planning: " << e.what();
-            return {};
+            ARMARX_ERROR << "Caugth exception while planning: " << e.what();
+            return std::nullopt;
         }
 
         if (hcp_->getTrajectoryContainer().empty())
         {
-            ARMARX_VERBOSE << "Did not find any trajectory!";
+            ARMARX_INFO << deactivateSpam(5) << "Did not find any trajectory!";
             return std::nullopt;
         }
 
@@ -154,6 +155,7 @@ namespace armarx::navigation::local_planning
             arviz.value().commit(layer);
         }
 
+        TIMING_END_COMMENT_STREAM(TEB_PLAN, "Timer: teb planning", ARMARX_VERBOSE);
         return {{.trajectory = best}};
     }
 
@@ -193,7 +195,7 @@ namespace armarx::navigation::local_planning
             arviz.value().commit(visLayer);
         }
 
-        ARMARX_INFO << "TEB: added " << obstManager.size() << " obstacles";
+        ARMARX_VERBOSE << "TEB: added " << obstManager.size() << " obstacles";
     }
 
     //export algorithms::Costmap to costmap type of teb local planner and provide costmap to planner