diff --git a/source/armarx/navigation/client/Navigator.cpp b/source/armarx/navigation/client/Navigator.cpp
index 9780c8c009164d4a5ea1753ac9089a48f632ecab..6054d0987dfb215589839a797d9c980d11344c4a 100644
--- a/source/armarx/navigation/client/Navigator.cpp
+++ b/source/armarx/navigation/client/Navigator.cpp
@@ -1,8 +1,12 @@
 #include "Navigator.h"
 
 #include <algorithm>
+#include <chrono>
+#include <future>
 
+#include "ArmarXCore/core/exceptions/LocalException.h"
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/util/CPPUtility/trace.h>
 
 #include <armarx/navigation/client/PathBuilder.h>
@@ -114,14 +118,55 @@ namespace armarx::navigation::client
 
 
     StopEvent
-    Navigator::waitForStop()
+    Navigator::waitForStop(const std::int64_t timeoutMs)
     {
         ARMARX_TRACE;
-        std::unique_lock l{stoppedInfo.m};
-        stoppedInfo.cv.wait(l, [&i = stoppedInfo] { return i.event.has_value(); });
+
+        std::future<void> future = std::async(
+            std::launch::async,
+            [&]()
+            {
+                std::unique_lock l{stoppedInfo.m};
+                stoppedInfo.cv.wait(l, [&i = stoppedInfo] { return i.event.has_value(); });
+            });
+
+
+        if (timeoutMs > 0)
+        {
+            ARMARX_INFO << "future.wait()";
+            auto status = future.wait_for(std::chrono::milliseconds(timeoutMs));
+            ARMARX_INFO << "done";
+
+            switch (status)
+            {
+                case std::future_status::ready:
+                    ARMARX_INFO << "waitForStop: terminated on goal reached";
+                    break;
+                case std::future_status::timeout:
+                    ARMARX_INFO << "waitForStop: terminated due to timeout";
+                    ARMARX_INFO << "Stopping robot due to timeout";
+                    stop();
+
+                    throw LocalException("Navigator::waitForStop: timeout");
+                    break;
+                case std::future_status::deferred:
+                    ARMARX_INFO << "waitForStop: deferred";
+                    break;
+            }
+        }
+        else
+        {
+            ARMARX_INFO << "future.wait()";
+            future.wait();
+            ARMARX_INFO << "done";
+        }
+
+        // only due to timeout, stoppedInfo.event should be nullopt
+        ARMARX_CHECK(stoppedInfo.event.has_value());
 
         StopEvent e = stoppedInfo.event.value();
         stoppedInfo.event.reset();
+
         return e;
     }
 
diff --git a/source/armarx/navigation/client/Navigator.h b/source/armarx/navigation/client/Navigator.h
index 77547742eed62f88b36494417fe735ab6b63b5b8..836ebffba750c9ee8a8ae2e7a10f14933b05bdbc 100644
--- a/source/armarx/navigation/client/Navigator.h
+++ b/source/armarx/navigation/client/Navigator.h
@@ -143,7 +143,7 @@ namespace armarx::navigation::client
 
         void onWaypointReached(const std::function<void(int)>& callback);
 
-        StopEvent waitForStop();
+        StopEvent waitForStop(std::int64_t timeoutMs = -1);
 
     protected:
     private: