added planning timeout option

MotionPlanning/Planner/BiRrt.cpp

@@ -219,6 +219,15 @@ namespace Saba
             cycles++;
             switched = !switched;
 
+            clock_t currentClock = clock();
+
+            long diffClock = (long)(((float)(currentClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0);
+            if(diffClock > planningTimeout)
+            {
+                std::cout << "Encountered timeout of " << planningTimeout << " ms - aborting";
+                return false;
+            }
+
         }
         while (!stopSearch && cycles < maxCycles && !found);
 

MotionPlanning/Planner/GraspIkRrt.cpp

@@ -228,6 +228,14 @@ namespace Saba
             }
 
             cycles++;
+            clock_t currentClock = clock();
+
+            long diffClock = (long)(((float)(currentClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0);
+            if(diffClock > planningTimeout)
+            {
+                std::cout << "Encountered timeout of " << planningTimeout << " ms - aborting";
+                return false;
+            }
         }
         while (!stopSearch && cycles < maxCycles && !found);
 

MotionPlanning/Planner/MotionPlanner.cpp

@@ -22,6 +22,7 @@ namespace Saba
 
 
         planningTime = 0.0f;
+        planningTimeout = std::numeric_limits<float>::max();
 
         stopSearch = false;
         maxCycles = 50000;              // stop if cycles are exceeded
@@ -168,6 +169,11 @@ namespace Saba
         return true;
     }
 
+    void MotionPlanner::setPlanningTimeout(float timeoutMs)
+    {
+        this->planningTimeout = timeoutMs;
+    }
+
     void MotionPlanner::setMaxCycles(unsigned int mc)
     {
         maxCycles = mc;

MotionPlanning/Planner/MotionPlanner.h

@@ -133,6 +133,8 @@ namespace Saba
         //! returns true, when start and goal config have been set
         virtual bool isInitialized();
 
+        void setPlanningTimeout(float timeoutMs);
+
     protected:
 
         //! create the solution
@@ -155,6 +157,7 @@ namespace Saba
         std::string name;                   //!< Name of this planner (standard: "Motion Planner")
 
         float planningTime;                 //! Planning time in milliseconds
+        float planningTimeout;              //! Timeout after which the planning aboits
     };
 }
 

MotionPlanning/Planner/Rrt.cpp

@@ -186,6 +186,14 @@ namespace Saba
             }
 
             cycles++;
+            clock_t currentClock = clock();
+
+            long diffClock = (long)(((float)(currentClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0);
+            if(diffClock > planningTimeout)
+            {
+                std::cout << "Encountered timeout of " << planningTimeout << " ms - aborting";
+                return false;
+            }
 
         }
         while (!stopSearch && cycles < maxCycles && !found);