From 85f92ab4a9f8c45406364e80732977e4ad4695e3 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 7 Jan 2022 11:43:50 +0100
Subject: [PATCH] waitforstop with timeout
---
source/armarx/navigation/client/Navigator.cpp | 51 +++++++++++++++++--
source/armarx/navigation/client/Navigator.h | 2 +-
2 files changed, 49 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/client/Navigator.cpp b/source/armarx/navigation/client/Navigator.cpp
index 9780c8c0..6054d098 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 77547742..836ebffb 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:
--
GitLab