diff --git a/source/Navigation/components/Navigator/Navigator.cpp b/source/Navigation/components/Navigator/Navigator.cpp
index 664b7fab95f102eec18efaacaa383c627a3cc7cc..29e0f63f8072354bceeaa9e9349f681d6ec9572e 100644
--- a/source/Navigation/components/Navigator/Navigator.cpp
+++ b/source/Navigation/components/Navigator/Navigator.cpp
@@ -171,20 +171,19 @@ armarx::nav::components::Navigator::createConfig(const aron::data::AronDictPtr&
server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
- server::GeneralConfig generalConfig;
-
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(writerGlobPlan, callerId));
navigators.emplace(
std::piecewise_construct,
std::forward_as_tuple(callerId),
- std::forward_as_tuple(server::NavigatorConfig{.stack = std::move(stack),
- .scene = &scene,
- .general = generalConfig},
- server::Services{.executor = &executor.value(),
- .publisher = &publisher,
- .introspector = &(introspector.value())}));
+ std::forward_as_tuple(
+ server::Navigator::Config{.stack = std::move(stack),
+ .scene = &scene,
+ .general = server::Navigator::Config::General{}},
+ server::Navigator::InjectedServices{.executor = &executor.value(),
+ .publisher = &publisher,
+ .introspector = &(introspector.value())}));
parameterizationService.store(stackConfig, callerId);
}
diff --git a/source/Navigation/libraries/client/ComponentPlugin.cpp b/source/Navigation/libraries/client/ComponentPlugin.cpp
index 8805b0d24c647d6832aa08d7fed08d21bd67df88..c817b7409e807dae554d2365fd57d2d40f867093 100644
--- a/source/Navigation/libraries/client/ComponentPlugin.cpp
+++ b/source/Navigation/libraries/client/ComponentPlugin.cpp
@@ -36,13 +36,13 @@ armarx::nav::client::ComponentPlugin::preOnConnectComponent()
ARMARX_TRACE;
const std::string componentName = parent().getName();
- IceNavigator& srv = parent<armarx::nav::client::ComponentPluginUser>().navigatorService;
+ IceNavigator& srv = parent<armarx::nav::client::ComponentPluginUser>().iceNavigator;
srv.setNavigatorComponent(navigatorPrx);
}
// ComponentPluginUser
-armarx::nav::client::ComponentPluginUser::ComponentPluginUser() : navigator{navigatorService}
+armarx::nav::client::ComponentPluginUser::ComponentPluginUser() : navigator{navigatorServices}
{
ARMARX_TRACE;
addPlugin(plugin);
@@ -60,7 +60,7 @@ armarx::nav::client::ComponentPluginUser::configureNavigator(
const client::NavigationStackConfig& stackConfig,
const std::string& configId)
{
- navigatorService.createConfig(stackConfig, configId);
+ iceNavigator.createConfig(stackConfig, configId);
}
armarx::nav::client::ComponentPluginUser::~ComponentPluginUser() = default;
diff --git a/source/Navigation/libraries/client/ComponentPlugin.h b/source/Navigation/libraries/client/ComponentPlugin.h
index 6124d4748fefb69d3a55989c4fa29c8d8ccca0ac..da4be5de0dd404d4be609b2ba08599cb26d887db 100644
--- a/source/Navigation/libraries/client/ComponentPlugin.h
+++ b/source/Navigation/libraries/client/ComponentPlugin.h
@@ -8,6 +8,7 @@
#include <Navigation/components/Navigator/NavigatorInterface.h>
#include <Navigation/libraries/client/Navigator.h>
#include <Navigation/libraries/client/services/IceNavigator.h>
+#include <Navigation/libraries/client/services/MemorySubscriber.h>
namespace armarx::nav::client
{
@@ -53,7 +54,9 @@ namespace armarx::nav::client
friend class ComponentPlugin;
private:
- IceNavigator navigatorService;
+ IceNavigator iceNavigator;
+ MemorySubscriber memorySubscriber;
+ Navigator::InjectedServices navigatorServices{.navigator = &iceNavigator, .subscriber = &memorySubscriber};
ComponentPlugin* plugin = nullptr;
};
diff --git a/source/Navigation/libraries/client/Navigator.cpp b/source/Navigation/libraries/client/Navigator.cpp
index e503fa95541fe4ec8d2450091505f581d0ae05bc..fae701c9dbc4bb0352a1b8df11baa598c4e916fc 100644
--- a/source/Navigation/libraries/client/Navigator.cpp
+++ b/source/Navigation/libraries/client/Navigator.cpp
@@ -3,10 +3,18 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-armarx::nav::client::Navigator::Navigator(core::NavigatorInterface& navigator) :
- navigator{&navigator}
+armarx::nav::client::Navigator::Navigator(const InjectedServices& services) :
+ srv{services}
{
- // pass
+ ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
+ ARMARX_CHECK_NOT_NULL(srv.subscriber) << "Subscriber service must not be null!";
+
+ auto stopped_callback = [this](const auto& e) { stopped(StopEvent(e)); };
+
+ srv.subscriber->onGoalReached(stopped_callback);
+ srv.subscriber->onSafetyStopTriggered(stopped_callback);
+ srv.subscriber->onUserAbortTriggered(stopped_callback);
+ srv.subscriber->onInternalError(stopped_callback);
}
@@ -21,8 +29,7 @@ void
armarx::nav::client::Navigator::moveTo(const std::vector<core::Pose>& waypoints,
core::NavigationFrame frame)
{
- ARMARX_CHECK_NOT_NULL(navigator);
- navigator->moveTo(waypoints, frame);
+ srv.navigator->moveTo(waypoints, frame);
}
@@ -30,45 +37,41 @@ void
armarx::nav::client::Navigator::moveTowards(const core::Direction& direction,
core::NavigationFrame frame)
{
- ARMARX_CHECK_NOT_NULL(navigator);
- navigator->moveTowards(direction, frame);
+ srv.navigator->moveTowards(direction, frame);
}
void
armarx::nav::client::Navigator::pause()
{
- ARMARX_CHECK_NOT_NULL(navigator);
- navigator->pause();
+ srv.navigator->pause();
}
void
armarx::nav::client::Navigator::resume()
{
- ARMARX_CHECK_NOT_NULL(navigator);
- navigator->resume();
+ srv.navigator->resume();
}
void
armarx::nav::client::Navigator::stop()
{
- ARMARX_CHECK_NOT_NULL(navigator);
- navigator->stop();
+ srv.navigator->stop();
}
void
armarx::nav::client::Navigator::onGoalReached(const std::function<void(void)>& callback)
{
- onGoalReached([&callback](const server::GoalReachedEvent&) { callback(); });
+ onGoalReached([&callback](const core::GoalReachedEvent&) { callback(); });
}
void
armarx::nav::client::Navigator::onGoalReached(
- const std::function<void(const server::GoalReachedEvent&)>& callback)
+ const std::function<void(const core::GoalReachedEvent&)>& callback)
{
}
@@ -79,21 +82,24 @@ armarx::nav::client::Navigator::onWaypointReached(const std::function<void(int)>
}
-// TODO: Remove after fixing waitForGoalReached
-#include <chrono>
-#include <thread>
-
armarx::nav::client::StopEvent
armarx::nav::client::Navigator::waitForStop()
{
- // TODO: Actually implement this.
- bool stopped = false;
- do
- {
- std::this_thread::sleep_for(std::chrono::milliseconds{10});
- stopped = navigator->isStopped();
- } while (not stopped);
+ std::unique_lock l{stoppedInfo.m};
+ stoppedInfo.cv.wait(l, [&i = stoppedInfo]{ return i.event.has_value(); });
+
+ StopEvent e = stoppedInfo.event.value();
+ stoppedInfo.event.reset();
+ return e;
+}
+
- StopEvent se = server::GoalReachedEvent{.pose = core::Pose::Identity()};
- return se;
+void
+armarx::nav::client::Navigator::stopped(const StopEvent& e)
+{
+ {
+ std::scoped_lock l{stoppedInfo.m};
+ stoppedInfo.event = e;
+ }
+ stoppedInfo.cv.notify_all();
}
diff --git a/source/Navigation/libraries/client/Navigator.h b/source/Navigation/libraries/client/Navigator.h
index 53617e7203645827976b81119521f9f9f2de1c16..07472bb917798da6ed573ede39426aaa4db2b6e3 100644
--- a/source/Navigation/libraries/client/Navigator.h
+++ b/source/Navigation/libraries/client/Navigator.h
@@ -23,7 +23,10 @@
#pragma once
// STD/STL
+#include <condition_variable>
#include <functional>
+#include <mutex>
+#include <optional>
#include <string>
#include <variant>
#include <vector>
@@ -31,7 +34,8 @@
// Navigation
#include <Navigation/libraries/core/NavigatorInterface.h>
#include <Navigation/libraries/core/types.h>
-#include <Navigation/libraries/server/event_publishing/EventPublishingInterface.h>
+#include <Navigation/libraries/core/events.h>
+#include <Navigation/libraries/client/services/EventSubscriptionInterface.h>
namespace armarx::nav::client
{
@@ -48,49 +52,49 @@ namespace armarx::nav::client
bool
isGoalReachedEvent() const
{
- return std::holds_alternative<server::GoalReachedEvent>(event);
+ return std::holds_alternative<core::GoalReachedEvent>(event);
}
- server::GoalReachedEvent&
+ core::GoalReachedEvent&
toGoalReachedEvent()
{
- return std::get<server::GoalReachedEvent>(event);
+ return std::get<core::GoalReachedEvent>(event);
}
bool
isSafetyStopTriggeredEvent() const
{
- return std::holds_alternative<server::SafetyStopTriggeredEvent>(event);
+ return std::holds_alternative<core::SafetyStopTriggeredEvent>(event);
}
- server::SafetyStopTriggeredEvent&
+ core::SafetyStopTriggeredEvent&
toSafetyStopTriggeredEvent()
{
- return std::get<server::SafetyStopTriggeredEvent>(event);
+ return std::get<core::SafetyStopTriggeredEvent>(event);
}
bool
isUserAbortTriggeredEvent() const
{
- return std::holds_alternative<server::UserAbortTriggeredEvent>(event);
+ return std::holds_alternative<core::UserAbortTriggeredEvent>(event);
}
- server::UserAbortTriggeredEvent&
+ core::UserAbortTriggeredEvent&
toUserAbortTriggeredEvent()
{
- return std::get<server::UserAbortTriggeredEvent>(event);
+ return std::get<core::UserAbortTriggeredEvent>(event);
}
bool
isInternalErrorEvent() const
{
- return std::holds_alternative<server::InternalErrorEvent>(event);
+ return std::holds_alternative<core::InternalErrorEvent>(event);
}
- server::InternalErrorEvent&
+ core::InternalErrorEvent&
toInternalErrorEvent()
{
- return std::get<server::InternalErrorEvent>(event);
+ return std::get<core::InternalErrorEvent>(event);
}
operator bool() const
@@ -99,10 +103,10 @@ namespace armarx::nav::client
}
private:
- using StopEventVar = std::variant<server::GoalReachedEvent,
- server::SafetyStopTriggeredEvent,
- server::UserAbortTriggeredEvent,
- server::InternalErrorEvent>;
+ using StopEventVar = std::variant<core::GoalReachedEvent,
+ core::SafetyStopTriggeredEvent,
+ core::UserAbortTriggeredEvent,
+ core::InternalErrorEvent>;
StopEventVar event;
};
@@ -111,7 +115,15 @@ namespace armarx::nav::client
{
public:
- Navigator(core::NavigatorInterface& navigator);
+
+ struct InjectedServices
+ {
+ core::NavigatorInterface* navigator;
+ EventSubscriptionInterface* subscriber;
+ };
+
+ public:
+ Navigator(const InjectedServices& services);
void moveTo(const core::Pose& pose, core::NavigationFrame frame);
@@ -127,7 +139,7 @@ namespace armarx::nav::client
void onGoalReached(const std::function<void(void)>& callback);
- void onGoalReached(const std::function<void(const server::GoalReachedEvent&)>& callback);
+ void onGoalReached(const std::function<void(const core::GoalReachedEvent&)>& callback);
void onWaypointReached(const std::function<void(int)>& callback);
@@ -135,12 +147,17 @@ namespace armarx::nav::client
protected:
private:
- core::NavigatorInterface* navigator;
- struct
- {
- std::vector<std::function<void(void)>> goalReached;
- } callbacks;
+ void stopped(const StopEvent&);
+
+ struct {
+ std::mutex m;
+ std::condition_variable cv;
+ std::optional<StopEvent> event;
+ } stoppedInfo;
+
+ InjectedServices srv;
+
};
} // namespace armarx::nav::client
diff --git a/source/Navigation/libraries/client/services/EventSubscriptionInterface.h b/source/Navigation/libraries/client/services/EventSubscriptionInterface.h
index ab74fbb8acf8ee815a6e50b0e206817e0ebb891c..1287989562b600cf6f01cbad6c056e1d7246fd96 100644
--- a/source/Navigation/libraries/client/services/EventSubscriptionInterface.h
+++ b/source/Navigation/libraries/client/services/EventSubscriptionInterface.h
@@ -1,18 +1,39 @@
#pragma once
-#include <Navigation/libraries/server/event_publishing/EventPublishingInterface.h>
+
+// STD/STL
+#include <functional>
+
+// Navigation
+#include <Navigation/libraries/core/events.h>
+
namespace armarx::nav::client
{
+ using OnGoalReachedCallback = std::function<void(const core::GoalReachedEvent&)>;
+ using OnWaypointReachedCallback = std::function<void(const core::WaypointReachedEvent&)>;
+ using OnSafetyThrottlingTriggeredCallback = std::function<void(const core::SafetyThrottlingTriggeredEvent&)>;
+ using OnSafetyStopTriggeredCallback = std::function<void(const core::SafetyStopTriggeredEvent&)>;
+ using OnUserAbortTriggeredCallback = std::function<void(const core::UserAbortTriggeredEvent&)>;
+ using OnInternalErrorCallback = std::function<void(const core::InternalErrorEvent&)>;
+
class EventSubscriptionInterface
{
public:
- virtual void onGoalReached(const server::GoalReachedEvent&) = 0;
+ virtual void onGoalReached(const OnGoalReachedCallback& callback) = 0;
+
+ virtual void onWaypointReached(const OnWaypointReachedCallback& callback) = 0;
+
+ virtual void onSafetyThrottlingTriggered(const OnSafetyThrottlingTriggeredCallback& callback) = 0;
+
+ virtual void onSafetyStopTriggered(const OnSafetyStopTriggeredCallback& callback) = 0;
+
+ virtual void onUserAbortTriggered(const OnUserAbortTriggeredCallback& callback) = 0;
- virtual void onWaypointReached(const server::WaypointReachedEvent&) = 0;
+ virtual void onInternalError(const OnInternalErrorCallback& callback) = 0;
// Non-API.
public:
diff --git a/source/Navigation/libraries/client/services/MemorySubscriber.cpp b/source/Navigation/libraries/client/services/MemorySubscriber.cpp
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..c7ff5ebd4574d9d55eeb7481dc0434f5543366d0 100644
--- a/source/Navigation/libraries/client/services/MemorySubscriber.cpp
+++ b/source/Navigation/libraries/client/services/MemorySubscriber.cpp
@@ -0,0 +1,43 @@
+#include <Navigation/libraries/client/services/MemorySubscriber.h>
+
+
+void
+armarx::nav::client::MemorySubscriber::onGoalReached(const OnGoalReachedCallback& callback)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::client::MemorySubscriber::onWaypointReached(const OnWaypointReachedCallback& callback)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::client::MemorySubscriber::onSafetyThrottlingTriggered(const OnSafetyThrottlingTriggeredCallback& callback)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::client::MemorySubscriber::onSafetyStopTriggered(const OnSafetyStopTriggeredCallback& callback)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::client::MemorySubscriber::onUserAbortTriggered(const OnUserAbortTriggeredCallback& callback)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::client::MemorySubscriber::onInternalError(const OnInternalErrorCallback& callback)
+{
+ // TODO: Implement.
+}
diff --git a/source/Navigation/libraries/client/services/MemorySubscriber.h b/source/Navigation/libraries/client/services/MemorySubscriber.h
index c8431ac7ef03957dcad1356293ec99f99b146f1c..1a9a63572d11cbf631c0a75b94d8e2dcaecd97b3 100644
--- a/source/Navigation/libraries/client/services/MemorySubscriber.h
+++ b/source/Navigation/libraries/client/services/MemorySubscriber.h
@@ -1,7 +1,9 @@
#pragma once
+
#include <Navigation/libraries/client/services/EventSubscriptionInterface.h>
+
namespace armarx::nav::client
{
@@ -10,15 +12,17 @@ namespace armarx::nav::client
public:
- void onGoalReached(const server::GoalReachedEvent&) override
- {
- // TODO: Implement.
- }
+ void onGoalReached(const OnGoalReachedCallback& callback) override;
+
+ void onWaypointReached(const OnWaypointReachedCallback& callback) override;
+
+ void onSafetyThrottlingTriggered(const OnSafetyThrottlingTriggeredCallback& callback) override;
+
+ void onSafetyStopTriggered(const OnSafetyStopTriggeredCallback& callback) override;
+
+ void onUserAbortTriggered(const OnUserAbortTriggeredCallback& callback) override;
- void onWaypointReached(const server::WaypointReachedEvent&) override
- {
- // TODO: Implement.
- }
+ void onInternalError(const OnInternalErrorCallback& callback) override;
// Non-API.
public:
diff --git a/source/Navigation/libraries/client/services/SimpleEventHandler.cpp b/source/Navigation/libraries/client/services/SimpleEventHandler.cpp
index a531180458ab108421040232f4a3d5d01d9a64c3..dcb21d0da18765bc13823b275d987a082ddc3af5 100644
--- a/source/Navigation/libraries/client/services/SimpleEventHandler.cpp
+++ b/source/Navigation/libraries/client/services/SimpleEventHandler.cpp
@@ -2,28 +2,102 @@
void
-armarx::nav::client::SimpleEventHandler::onGoalReached(const server::GoalReachedEvent&)
+armarx::nav::client::SimpleEventHandler::onGoalReached(const OnGoalReachedCallback& callback)
{
- // TODO: Implement.
+ subscriptions.goalReachedCallbacks.push_back(callback);
}
void
-armarx::nav::client::SimpleEventHandler::onWaypointReached(const server::WaypointReachedEvent&)
+armarx::nav::client::SimpleEventHandler::onWaypointReached(const OnWaypointReachedCallback& callback)
{
- // TODO: Implement.
+ subscriptions.waypointReachedCallbacks.push_back(callback);
}
void
-armarx::nav::client::SimpleEventHandler::goalReached(const server::GoalReachedEvent&)
+armarx::nav::client::SimpleEventHandler::onSafetyThrottlingTriggered(const OnSafetyThrottlingTriggeredCallback& callback)
{
- // TODO: Implement.
+ subscriptions.safetyThrottlingTriggeredCallbacks.push_back(callback);
}
void
-armarx::nav::client::SimpleEventHandler::waypointReached(const server::WaypointReachedEvent&)
+armarx::nav::client::SimpleEventHandler::onSafetyStopTriggered(const OnSafetyStopTriggeredCallback& callback)
{
- // TODO: Implement.
+ subscriptions.safetyStopTriggeredCallbacks.push_back(callback);
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::onUserAbortTriggered(const OnUserAbortTriggeredCallback& callback)
+{
+ subscriptions.userAbortTriggeredCallbacks.push_back(callback);
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::onInternalError(const OnInternalErrorCallback& callback)
+{
+ subscriptions.internalErrorCallbacks.push_back(callback);
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::goalReached(const core::GoalReachedEvent& event)
+{
+ for (const auto& callback : subscriptions.goalReachedCallbacks)
+ {
+ callback(event);
+ }
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::waypointReached(const core::WaypointReachedEvent& event)
+{
+ for (const auto& callback : subscriptions.waypointReachedCallbacks)
+ {
+ callback(event);
+ }
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::safetyThrottlingTriggered(const core::SafetyThrottlingTriggeredEvent& event)
+{
+ for (const auto& callback : subscriptions.safetyThrottlingTriggeredCallbacks)
+ {
+ callback(event);
+ }
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::safetyStopTriggered(const core::SafetyStopTriggeredEvent& event)
+{
+ for (const auto& callback : subscriptions.safetyStopTriggeredCallbacks)
+ {
+ callback(event);
+ }
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::userAbortTriggered(const core::UserAbortTriggeredEvent& event)
+{
+ for (const auto& callback : subscriptions.userAbortTriggeredCallbacks)
+ {
+ callback(event);
+ }
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::internalError(const core::InternalErrorEvent& event)
+{
+ for (const auto& callback : subscriptions.internalErrorCallbacks)
+ {
+ callback(event);
+ }
}
diff --git a/source/Navigation/libraries/client/services/SimpleEventHandler.h b/source/Navigation/libraries/client/services/SimpleEventHandler.h
index dc700f2a493f83d3fb0412c8a7375ba84400d831..3e52a36e8a3014d1d649420ca485562f1decfe4c 100644
--- a/source/Navigation/libraries/client/services/SimpleEventHandler.h
+++ b/source/Navigation/libraries/client/services/SimpleEventHandler.h
@@ -1,8 +1,10 @@
#pragma once
+
#include <Navigation/libraries/client/services/EventSubscriptionInterface.h>
#include <Navigation/libraries/server/event_publishing/EventPublishingInterface.h>
+
namespace armarx::nav::client
{
@@ -11,20 +13,35 @@ namespace armarx::nav::client
virtual public server::EventPublishingInterface
{
- // EventSubscriptionInterface interface
+ // EventSubscriptionInterface
public:
- void onGoalReached(const server::GoalReachedEvent&) override;
- void onWaypointReached(const server::WaypointReachedEvent&) override;
- // EventPublishingInterface interface
+ void onGoalReached(const OnGoalReachedCallback& callback) override;
+ void onWaypointReached(const OnWaypointReachedCallback& callback) override;
+ void onSafetyThrottlingTriggered(const OnSafetyThrottlingTriggeredCallback& callback) override;
+ void onSafetyStopTriggered(const OnSafetyStopTriggeredCallback& callback) override;
+ void onUserAbortTriggered(const OnUserAbortTriggeredCallback& callback) override;
+ void onInternalError(const OnInternalErrorCallback& callback) override;
+
+ // EventPublishingInterface
public:
- void goalReached(const server::GoalReachedEvent&) override;
- void waypointReached(const server::WaypointReachedEvent&) override;
+
+ void goalReached(const core::GoalReachedEvent& event) override;
+ void waypointReached(const core::WaypointReachedEvent& event) override;
+ void safetyThrottlingTriggered(const core::SafetyThrottlingTriggeredEvent& event) override;
+ void safetyStopTriggered(const core::SafetyStopTriggeredEvent& event) override;
+ void userAbortTriggered(const core::UserAbortTriggeredEvent& event) override;
+ void internalError(const core::InternalErrorEvent& event) override;
private:
struct {
- bool todo;
+ std::vector<OnGoalReachedCallback> goalReachedCallbacks;
+ std::vector<OnWaypointReachedCallback> waypointReachedCallbacks;
+ std::vector<OnSafetyThrottlingTriggeredCallback> safetyThrottlingTriggeredCallbacks;
+ std::vector<OnSafetyStopTriggeredCallback> safetyStopTriggeredCallbacks;
+ std::vector<OnUserAbortTriggeredCallback> userAbortTriggeredCallbacks;
+ std::vector<OnInternalErrorCallback> internalErrorCallbacks;
} subscriptions;
};
diff --git a/source/Navigation/libraries/core/CMakeLists.txt b/source/Navigation/libraries/core/CMakeLists.txt
index 5294b45f27ff1ff05778fc5892a93e56b8634f27..e0e3b1bcb689a5ffc572ac2f03380ee62805df7a 100644
--- a/source/Navigation/libraries/core/CMakeLists.txt
+++ b/source/Navigation/libraries/core/CMakeLists.txt
@@ -20,6 +20,7 @@ armarx_add_library(
aron_conversions.cpp
HEADERS
types.h
+ events.h
NavigatorInterface.h
MemoryReferencedElement.h
# scenes
diff --git a/source/Navigation/libraries/core/events.h b/source/Navigation/libraries/core/events.h
new file mode 100644
index 0000000000000000000000000000000000000000..29797d345cf540123bd95491cdf01c3eb0940cce
--- /dev/null
+++ b/source/Navigation/libraries/core/events.h
@@ -0,0 +1,49 @@
+#pragma once
+
+
+// STD/STL
+#include <string>
+
+// Navigation
+#include <Navigation/libraries/core/types.h>
+
+
+namespace armarx::nav::core
+{
+
+ struct GoalReachedEvent
+ {
+ core::Pose pose;
+ };
+
+ struct WaypointReachedEvent
+ {
+ core::Pose pose;
+ int waypointId;
+ };
+
+ struct SafetyThrottlingTriggeredEvent
+ {
+ core::Pose pose;
+ float throttlingFactor = 1.0;
+ // TODO: Direction where safety-critical obstacle is (or range or whatever...).
+ };
+
+ struct SafetyStopTriggeredEvent
+ {
+ core::Pose pose;
+ // TODO: Direction where safety-critical obstacle is (or range or whatever...).
+ };
+
+ struct UserAbortTriggeredEvent
+ {
+ core::Pose pose;
+ };
+
+ struct InternalErrorEvent
+ {
+ core::Pose pose;
+ std::string message;
+ };
+
+} // namespace armarx::nav::core
diff --git a/source/Navigation/libraries/server/CMakeLists.txt b/source/Navigation/libraries/server/CMakeLists.txt
index e825612271338f9cade4961fecaabec5701eba41..321f869496c883d768bdad3b2e91d3a9928d702d 100644
--- a/source/Navigation/libraries/server/CMakeLists.txt
+++ b/source/Navigation/libraries/server/CMakeLists.txt
@@ -19,6 +19,8 @@ armarx_add_library(
./StackResult.cpp
# Executors.
./execution/PlatformUnitExecutor.cpp
+ # Event publishing.
+ ./event_publishing/MemoryPublisher.cpp
# Introspection
./introspection/ArvizIntrospector.cpp
./introspection/MemoryIntrospector.cpp
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index cb67fcf1deab81d41ee4b3a0b92bda1bde911768..57950493f6e0f6703a0dd013d4be223591f6ddd4 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -25,7 +25,7 @@ namespace armarx::nav::server
}
}
- Navigator::Navigator(const NavigatorConfig& config, const Services& services) : config{config}, srv{services}
+ Navigator::Navigator(const Config& config, const InjectedServices& services) : config{config}, srv{services}
{
ARMARX_CHECK_NOT_NULL(config.scene) << "The scene must be set!";
ARMARX_CHECK_NOT_NULL(services.executor) << "The executor service must be set!";
diff --git a/source/Navigation/libraries/server/Navigator.h b/source/Navigation/libraries/server/Navigator.h
index 0c601b2802007dabc70026922e5b7009f543fb22..915bcb496d33f8d840e2281d33b1d24d1bb4273a 100644
--- a/source/Navigation/libraries/server/Navigator.h
+++ b/source/Navigation/libraries/server/Navigator.h
@@ -48,39 +48,42 @@
namespace armarx::nav::server
{
- struct GeneralConfig
- {
- struct
- {
- // TODO: Use chrono?
- int localPlanningUpdatePeriod{10}; // [ms]
- int trajectoryControllerUpdatePeriod{100}; // [ms]
- int safetyControllerUpdatePeriod{100}; // [ms]
- int introspectorUpdatePeriod{10}; // [ms]
- int executorUpdatePeriod{100}; // [ms]
- int monitorUpdatePeriod{20}; // [ms]
- } tasks;
- };
-
- struct NavigatorConfig
+ class Navigator : virtual public core::NavigatorInterface
{
- server::NavigationStack stack;
- core::Scene* const scene;
- GeneralConfig general;
- };
- struct Services
- {
- ExecutorInterface* executor;
- EventPublishingInterface* publisher;
- IntrospectorInterface* introspector = nullptr;
- };
+ public:
- class Navigator : virtual public core::NavigatorInterface
- {
+ struct Config
+ {
+ struct General
+ {
+ struct
+ {
+ // TODO: Use chrono?
+ int localPlanningUpdatePeriod{10}; // [ms]
+ int trajectoryControllerUpdatePeriod{100}; // [ms]
+ int safetyControllerUpdatePeriod{100}; // [ms]
+ int introspectorUpdatePeriod{10}; // [ms]
+ int executorUpdatePeriod{100}; // [ms]
+ int monitorUpdatePeriod{20}; // [ms]
+ } tasks;
+ };
+
+ server::NavigationStack stack;
+ core::Scene* const scene;
+ General general;
+ };
+
+ struct InjectedServices
+ {
+ ExecutorInterface* executor;
+ EventPublishingInterface* publisher;
+ IntrospectorInterface* introspector = nullptr;
+ };
public:
- Navigator(const NavigatorConfig& config, const Services& services);
+
+ Navigator(const Config& config, const InjectedServices& services);
void moveTo(const std::vector<core::Pose>& waypoints,
core::NavigationFrame navigationFrame) override;
@@ -121,9 +124,9 @@ namespace armarx::nav::server
void stopAllThreads();
- NavigatorConfig config;
+ Config config;
- Services srv;
+ InjectedServices srv;
std::atomic_bool executorEnabled = true;
diff --git a/source/Navigation/libraries/server/event_publishing/EventPublishingInterface.h b/source/Navigation/libraries/server/event_publishing/EventPublishingInterface.h
index b34774b531dbfdc51fee965a79e43aa69a008750..b0e6f9cf053ce85ded8350671790b2ce6ea6d2ff 100644
--- a/source/Navigation/libraries/server/event_publishing/EventPublishingInterface.h
+++ b/source/Navigation/libraries/server/event_publishing/EventPublishingInterface.h
@@ -1,43 +1,11 @@
#pragma once
-#include <Navigation/libraries/core/types.h>
-namespace armarx::nav::server
-{
-
- struct GoalReachedEvent
- {
- core::Pose pose;
- };
-
- struct WaypointReachedEvent
- {
- int waypointId;
- core::Pose pose;
- };
+#include <Navigation/libraries/core/events.h>
- struct SafetyThrottlingTriggeredEvent
- {
- core::Pose pose;
- float throttlingFactor = 1.0;
- // TODO: Direction where safety-critical obstacle is (or range or whatever...).
- };
-
- struct SafetyStopTriggeredEvent
- {
- core::Pose pose;
- // TODO: Direction where safety-critical obstacle is (or range or whatever...).
- };
-
- struct UserAbortTriggeredEvent
- {
- core::Pose pose;
- };
- struct InternalErrorEvent
- {
- std::string message;
- };
+namespace armarx::nav::server
+{
/**
* @brief A publisher the server navigator will use to notify others about events.
@@ -50,12 +18,32 @@ namespace armarx::nav::server
/**
* @brief Will be called whenever the navigator reached the goal.
*/
- virtual void goalReached(const GoalReachedEvent&) = 0;
+ virtual void goalReached(const core::GoalReachedEvent& event) = 0;
/**
* @brief Will be called whenever the navigator reached a user-defined waypoint.
*/
- virtual void waypointReached(const WaypointReachedEvent&) = 0;
+ virtual void waypointReached(const core::WaypointReachedEvent& event) = 0;
+
+ /**
+ * @brief Will be called whenever safety throttling is triggered to a certain degree (configurable).
+ */
+ virtual void safetyThrottlingTriggered(const core::SafetyThrottlingTriggeredEvent& event) = 0;
+
+ /**
+ * @brief Will be called whenever a safety stop is triggered.
+ */
+ virtual void safetyStopTriggered(const core::SafetyStopTriggeredEvent& event) = 0;
+
+ /**
+ * @brief Will be called whenever the user aborts the current navigation.
+ */
+ virtual void userAbortTriggered(const core::UserAbortTriggeredEvent& event) = 0;
+
+ /**
+ * @brief Will be called whenever an internal error occurs.
+ */
+ virtual void internalError(const core::InternalErrorEvent& event) = 0;
// Non-API.
public:
diff --git a/source/Navigation/libraries/server/event_publishing/MemoryPublisher.cpp b/source/Navigation/libraries/server/event_publishing/MemoryPublisher.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..710edcaffb9aaae90e7204543f131abf92a9a0ac
--- /dev/null
+++ b/source/Navigation/libraries/server/event_publishing/MemoryPublisher.cpp
@@ -0,0 +1,43 @@
+#include <Navigation/libraries/server/event_publishing/MemoryPublisher.h>
+
+
+void
+armarx::nav::server::MemoryPublisher::goalReached(const core::GoalReachedEvent& event)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::server::MemoryPublisher::waypointReached(const core::WaypointReachedEvent& event)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::server::MemoryPublisher::safetyThrottlingTriggered(const core::SafetyThrottlingTriggeredEvent& event)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::server::MemoryPublisher::safetyStopTriggered(const core::SafetyStopTriggeredEvent& event)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::server::MemoryPublisher::userAbortTriggered(const core::UserAbortTriggeredEvent& event)
+{
+ // TODO: Implement.
+}
+
+
+void
+armarx::nav::server::MemoryPublisher::internalError(const core::InternalErrorEvent& event)
+{
+ // TODO: Implement.
+}
diff --git a/source/Navigation/libraries/server/event_publishing/MemoryPublisher.h b/source/Navigation/libraries/server/event_publishing/MemoryPublisher.h
index b139b9e7a7fdaaf9714f84a28775fcd8b4ae6fdf..7c199d7625fcdd63d357b174682c7aed75119371 100644
--- a/source/Navigation/libraries/server/event_publishing/MemoryPublisher.h
+++ b/source/Navigation/libraries/server/event_publishing/MemoryPublisher.h
@@ -10,15 +10,17 @@ namespace armarx::nav::server
public:
- void goalReached(const GoalReachedEvent&) override
- {
- // TODO: Implement.
- }
-
- void waypointReached(const WaypointReachedEvent&) override
- {
- // TODO: Implement.
- }
+ void goalReached(const core::GoalReachedEvent& event) override;
+
+ void waypointReached(const core::WaypointReachedEvent& event) override;
+
+ void safetyThrottlingTriggered(const core::SafetyThrottlingTriggeredEvent& event) override;
+
+ void safetyStopTriggered(const core::SafetyStopTriggeredEvent& event) override;
+
+ void userAbortTriggered(const core::UserAbortTriggeredEvent& event) override;
+
+ void internalError(const core::InternalErrorEvent& event) override;
// Non-API.
public:
diff --git a/source/Navigation/libraries/server/test/serverTest.cpp b/source/Navigation/libraries/server/test/serverTest.cpp
index 74cde8395ad429f9df562d12eebd079e0c2b6be1..f3977f2619f879425f1b496d4aa18c19b7daed9f 100644
--- a/source/Navigation/libraries/server/test/serverTest.cpp
+++ b/source/Navigation/libraries/server/test/serverTest.cpp
@@ -65,11 +65,12 @@ BOOST_AUTO_TEST_CASE(testNavigator)
server::DummyExecutor executor{scene.robot, server::DummyExecutor::Params()};
client::SimpleEventHandler eventHandler;
- server::GeneralConfig generalConfig;
-
- server::Navigator navigator(server::NavigatorConfig{
- .stack = stack, .scene = &scene, .general = generalConfig},
- server::Services{
+ server::Navigator navigator(server::Navigator::Config{
+ .stack = stack,
+ .scene = &scene,
+ .general = server::Navigator::Config::General{}
+ },
+ server::Navigator::InjectedServices{
.executor = &executor,
.publisher = &eventHandler
});