From fdfe83d1915af024f09dc45da738c4d32b75b874 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 3 May 2022 20:33:01 +0200
Subject: [PATCH 1/8] emitting UserAbortEvent instead of GoalReached on stop()
---
.../components/Navigator/Navigator.cpp | 17 +++++++++--------
1 file changed, 9 insertions(+), 8 deletions(-)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 1482b945..08eac501 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -340,8 +340,6 @@ namespace armarx::navigation::components
updateContext();
visualizeSPFA();
navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
-
- // navigators.at(callerId).moveTo()
}
void
@@ -386,6 +384,11 @@ namespace armarx::navigation::components
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
navigators.at(configId).stop();
+
+ // emit UserAbortTriggered event
+ const core::UserAbortTriggeredEvent event{{scene.timeServer->now()},
+ core::Pose(scene.robot->getGlobalPose())};
+ memoryPublishers.at(configId)->userAbortTriggered(event);
}
void
@@ -399,14 +402,12 @@ namespace armarx::navigation::components
navigator.stop();
}
- const core::GoalReachedEvent evt{{scene.timeServer->now()},
- {core::Pose(scene.robot->getGlobalPose())}};
-
+ // emit UserAbortTriggered event
+ const core::UserAbortTriggeredEvent event{{scene.timeServer->now()},
+ core::Pose(scene.robot->getGlobalPose())};
for (auto& [callerId, memoryPublisher] : memoryPublishers)
{
- // TODO use different event, e.g. generic stopped event
- // TODO check if navigator was stopped
- memoryPublisher->goalReached(evt);
+ memoryPublisher->userAbortTriggered(event);
}
}
--
GitLab
From 9cc7cafcd1ac4f8f0b45c4457a1652700c5f7953 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 4 May 2022 18:22:32 +0200
Subject: [PATCH 2/8] using armarx::time
---
source/armarx/navigation/core/events.h | 7 +++----
.../core/time/ChronoMonotonicTimeServer.cpp | 17 +++--------------
.../core/time/ChronoMonotonicTimeServer.h | 2 +-
.../navigation/core/time/TimeServerInterface.h | 4 ++--
source/armarx/navigation/core/types.h | 3 ---
.../memory/client/parameterization/Writer.cpp | 11 +++--------
.../MemoryParameterizationService.cpp | 4 ++--
.../MemoryParameterizationService.h | 4 ++--
8 files changed, 16 insertions(+), 36 deletions(-)
diff --git a/source/armarx/navigation/core/events.h b/source/armarx/navigation/core/events.h
index f50fa3bb..fd95336f 100644
--- a/source/armarx/navigation/core/events.h
+++ b/source/armarx/navigation/core/events.h
@@ -1,19 +1,18 @@
#pragma once
-// STD/STL
#include <string>
-// Navigation
-#include <armarx/navigation/core/types.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <armarx/navigation/core/types.h>
namespace armarx::navigation::core
{
struct Event
{
- core::TimestampUs timestamp;
+ armarx::core::time::DateTime timestamp;
};
struct GlobalPlanningFailedEvent : public Event
diff --git a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp b/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp
index 4e619612..fb3db7cb 100644
--- a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp
+++ b/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp
@@ -1,24 +1,13 @@
#include "ChronoMonotonicTimeServer.h"
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
namespace armarx::navigation::core
{
- TimestampUs
+ armarx::core::time::DateTime
ChronoMonotonicTimeServer::now() const
{
- const auto tsNow = std::chrono::high_resolution_clock::now();
- // const auto tsNow = std::chrono::steady_clock::now();
- const auto tsSince = tsNow.time_since_epoch();
- const auto dur = std::chrono::duration_cast<std::chrono::microseconds>(tsSince);
-
- // ARMARX_INFO << "tsnow " << tsNow;
- // ARMARX_INFO << "tsSince " << tsSince;
- // ARMARX_INFO << "dur " << dur;
-
- return dur;
+ return armarx::Clock::Now();
}
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h b/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h
index 24bc5a12..31d6cf7e 100644
--- a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h
+++ b/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h
@@ -31,7 +31,7 @@ namespace armarx::navigation::core
class ChronoMonotonicTimeServer : virtual public TimeServerInterface
{
public:
- TimestampUs now() const override;
+ armarx::core::time::DateTime now() const override;
~ChronoMonotonicTimeServer() override = default;
};
diff --git a/source/armarx/navigation/core/time/TimeServerInterface.h b/source/armarx/navigation/core/time/TimeServerInterface.h
index f17665c8..fdb74e85 100644
--- a/source/armarx/navigation/core/time/TimeServerInterface.h
+++ b/source/armarx/navigation/core/time/TimeServerInterface.h
@@ -21,7 +21,7 @@
#pragma once
-#include <armarx/navigation/core/types.h>
+#include "ArmarXCore/core/time/DateTime.h"
namespace armarx::navigation::core
{
@@ -29,7 +29,7 @@ namespace armarx::navigation::core
class TimeServerInterface
{
public:
- virtual TimestampUs now() const = 0;
+ virtual armarx::core::time::DateTime now() const = 0;
virtual ~TimeServerInterface() = default;
};
diff --git a/source/armarx/navigation/core/types.h b/source/armarx/navigation/core/types.h
index 0e5b96fa..a0529cb0 100644
--- a/source/armarx/navigation/core/types.h
+++ b/source/armarx/navigation/core/types.h
@@ -95,9 +95,6 @@ namespace armarx::navigation::core
TimeServerInterface* timeServer;
};
- using TimestampUs = std::chrono::microseconds;
-
-
struct PIDParams
{
float Kp{1.0};
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.cpp b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
index 23a8d443..7becf03f 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.cpp
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
@@ -11,15 +11,10 @@ namespace armarx::navigation::memory::client::param
bool
Writer::store(const std::unordered_map<core::StackLayer, aron::data::DictPtr>& stack,
const std::string& clientID,
- const core::TimestampUs& timestamp)
+ const armarx::core::time::DateTime& timestamp)
{
ARMARX_CHECK(not stack.empty());
- const armem::Time ts(Duration::MicroSeconds(timestamp.count()));
-
- ARMARX_INFO << "timestamp chrono " << timestamp;
- ARMARX_INFO << "timestamp " << ts;
-
armem::Commit commit;
for (const auto& [layer, data] : stack)
@@ -32,8 +27,8 @@ namespace armarx::navigation::memory::client::param
.withCoreSegmentName(properties().coreSegmentName)
.withProviderSegmentName(clientID)
.withEntityName(layerName)
- .withTimestamp(ts);
- update.timeCreated = ts;
+ .withTimestamp(timestamp);
+ update.timeCreated = timestamp;
update.instancesData = {data};
commit.add(update);
}
diff --git a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp
index b4334831..89be35bb 100644
--- a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp
+++ b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp
@@ -13,7 +13,7 @@ namespace armarx::navigation::server
bool
MemoryParameterizationService::store(const aron::data::DictPtr& params,
const std::string& clientId,
- const core::TimestampUs& timestamp)
+ const armarx::core::time::DateTime& timestamp)
{
ARMARX_CHECK_NOT_NULL(writer);
@@ -63,7 +63,7 @@ namespace armarx::navigation::server
bool
MemoryParameterizationService::store(const aron::data::dto::DictPtr& params,
const std::string& clientId,
- const core::TimestampUs& timestamp)
+ const armarx::core::time::DateTime& timestamp)
{
const auto dict = aron::data::Dict::FromAronDictDTO(params);
return store(dict, clientId, timestamp);
diff --git a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.h b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.h
index 0eb1e0b7..ea81edee 100644
--- a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.h
+++ b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.h
@@ -41,11 +41,11 @@ namespace armarx::navigation::server
bool store(const aron::data::DictPtr& params,
const std::string& clientId,
- const core::TimestampUs& timestamp);
+ const armarx::core::time::DateTime& timestamp);
bool store(const aron::data::dto::DictPtr& params,
const std::string& clientId,
- const core::TimestampUs& timestamp);
+ const armarx::core::time::DateTime& timestamp);
protected:
--
GitLab
From 70cd3ab20ffd69249b1eeff5c55b07bde3f20092 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 4 May 2022 18:23:13 +0200
Subject: [PATCH 3/8] impl. aron events / conversion
---
source/armarx/navigation/core/aron/Events.xml | 12 ++++++
.../navigation/core/aron_conversions.cpp | 43 +++++++++++++++++++
.../armarx/navigation/core/aron_conversions.h | 21 ++++++++-
.../memory/client/events/Writer.cpp | 28 +++++++-----
.../navigation/memory/client/events/Writer.h | 5 +--
.../memory/client/parameterization/Writer.h | 2 +-
.../event_publishing/MemoryPublisher.cpp | 2 +-
7 files changed, 96 insertions(+), 17 deletions(-)
diff --git a/source/armarx/navigation/core/aron/Events.xml b/source/armarx/navigation/core/aron/Events.xml
index d2009212..d7d29939 100644
--- a/source/armarx/navigation/core/aron/Events.xml
+++ b/source/armarx/navigation/core/aron/Events.xml
@@ -41,5 +41,17 @@
</ObjectChild>
</Object>
+ <Object name='armarx::navigation::core::arondto::UserAbortTriggeredEvent'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::core::arondto::GlobalPlanningFailedEvent'>
+ <ObjectChild key='message'>
+ <String />
+ </ObjectChild>
+ </Object>
+
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/armarx/navigation/core/aron_conversions.cpp b/source/armarx/navigation/core/aron_conversions.cpp
index 84b847e3..4a974067 100644
--- a/source/armarx/navigation/core/aron_conversions.cpp
+++ b/source/armarx/navigation/core/aron_conversions.cpp
@@ -3,6 +3,7 @@
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
+#include "RobotAPI/libraries/aron/common/aron_conversions/core.h"
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/Trajectory.h>
@@ -121,6 +122,34 @@ namespace armarx::navigation::core
aron::fromAron(dto.pose, bo.pose.matrix());
}
+ void
+ fromAron(const armarx::navigation::core::arondto::GlobalPlanningFailedEvent& dto,
+ armarx::navigation::core::GlobalPlanningFailedEvent& bo)
+ {
+ aron::fromAron(dto.message, bo.message);
+ }
+
+ void
+ toAron(armarx::navigation::core::arondto::GlobalPlanningFailedEvent& dto,
+ const armarx::navigation::core::GlobalPlanningFailedEvent& bo)
+ {
+ aron::toAron(dto.message, bo.message);
+ }
+
+ void
+ toAron(armarx::navigation::core::arondto::MovementStartedEvent& dto,
+ const armarx::navigation::core::MovementStartedEvent& bo)
+ {
+ aron::toAron(dto.startPose, bo.startPose.matrix());
+ }
+
+ void
+ fromAron(const armarx::navigation::core::arondto::MovementStartedEvent& dto,
+ armarx::navigation::core::MovementStartedEvent& bo)
+ {
+ aron::fromAron(dto.startPose, bo.startPose.matrix());
+ }
+
void
toAron(armarx::navigation::core::arondto::WaypointReachedEvent& dto,
const armarx::navigation::core::WaypointReachedEvent& bo)
@@ -153,4 +182,18 @@ namespace armarx::navigation::core
aron::fromAron(dto.message, bo.message);
}
+ void
+ toAron(armarx::navigation::core::arondto::UserAbortTriggeredEvent& dto,
+ const armarx::navigation::core::UserAbortTriggeredEvent& bo)
+ {
+ aron::toAron(dto.pose, bo.pose.matrix());
+ }
+
+ void
+ fromAron(const armarx::navigation::core::arondto::UserAbortTriggeredEvent& dto,
+ armarx::navigation::core::UserAbortTriggeredEvent& bo)
+ {
+ aron::fromAron(dto.pose, bo.pose.matrix());
+ }
+
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/aron_conversions.h b/source/armarx/navigation/core/aron_conversions.h
index 4e0a7bd4..654fd409 100644
--- a/source/armarx/navigation/core/aron_conversions.h
+++ b/source/armarx/navigation/core/aron_conversions.h
@@ -63,20 +63,39 @@ namespace armarx::navigation::core
// Events
void toAron(armarx::navigation::core::arondto::GoalReachedEvent& dto,
const armarx::navigation::core::GoalReachedEvent& bo);
+
void fromAron(const armarx::navigation::core::arondto::GoalReachedEvent& dto,
armarx::navigation::core::GoalReachedEvent& bo);
+ void toAron(armarx::navigation::core::arondto::MovementStartedEvent& dto,
+ const armarx::navigation::core::MovementStartedEvent& bo);
+
+ void fromAron(const armarx::navigation::core::arondto::GlobalPlanningFailedEvent& dto,
+ armarx::navigation::core::GlobalPlanningFailedEvent& bo);
+
+ void toAron(armarx::navigation::core::arondto::GlobalPlanningFailedEvent& dto,
+ const armarx::navigation::core::GlobalPlanningFailedEvent& bo);
+
+ void fromAron(const armarx::navigation::core::arondto::MovementStartedEvent& dto,
+ armarx::navigation::core::MovementStartedEvent& bo);
+
void toAron(armarx::navigation::core::arondto::WaypointReachedEvent& dto,
const armarx::navigation::core::WaypointReachedEvent& bo);
+
void fromAron(const armarx::navigation::core::arondto::WaypointReachedEvent& dto,
armarx::navigation::core::WaypointReachedEvent& bo);
-
void toAron(armarx::navigation::core::arondto::InternalErrorEvent& dto,
const armarx::navigation::core::InternalErrorEvent& bo);
+
void fromAron(const armarx::navigation::core::arondto::InternalErrorEvent& dto,
armarx::navigation::core::InternalErrorEvent& bo);
+ void toAron(armarx::navigation::core::arondto::UserAbortTriggeredEvent& dto,
+ const armarx::navigation::core::UserAbortTriggeredEvent& bo);
+
+ void fromAron(const armarx::navigation::core::arondto::UserAbortTriggeredEvent& dto,
+ armarx::navigation::core::UserAbortTriggeredEvent& bo);
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/memory/client/events/Writer.cpp b/source/armarx/navigation/memory/client/events/Writer.cpp
index b15a93ca..4325c44a 100644
--- a/source/armarx/navigation/memory/client/events/Writer.cpp
+++ b/source/armarx/navigation/memory/client/events/Writer.cpp
@@ -64,35 +64,41 @@ namespace armarx::navigation::memory::client::events
bool
Writer::store(const core::GoalReachedEvent& event, const std::string& clientID)
{
- return storeImpl(event, "GoalReachedEvent", clientID);
+ return storeImpl<core::arondto::GoalReachedEvent>(event, "GoalReachedEvent", clientID);
}
bool
Writer::store(const core::WaypointReachedEvent& event, const std::string& clientID)
{
- return storeImpl(event, "WaypointReachedEvent", clientID);
+ return storeImpl<core::arondto::WaypointReachedEvent>(event, "WaypointReachedEvent", clientID);
}
bool
Writer::store(const core::InternalErrorEvent& event, const std::string& clientID)
{
- return storeImpl(event, "InternalErrorEvent", clientID);
+ return storeImpl<core::arondto::InternalErrorEvent>(event, "InternalErrorEvent", clientID);
}
bool
Writer::store(const core::GlobalPlanningFailedEvent& event, const std::string& clientID)
{
- return storeImpl(event, "GlobalPlanningFailedEvent", clientID);
+ return storeImpl<core::arondto::GlobalPlanningFailedEvent>(event, "GlobalPlanningFailedEvent", clientID);
}
bool
Writer::store(const core::MovementStartedEvent& event, const std::string& clientID)
{
- return storeImpl(event, "MovementStartedEvent", clientID);
+ return storeImpl<core::arondto::MovementStartedEvent>(event, "MovementStartedEvent", clientID);
+ }
+
+ bool
+ Writer::store(const core::UserAbortTriggeredEvent& event, const std::string& clientID)
+ {
+ return storeImpl<core::arondto::UserAbortTriggeredEvent>(event, "UserAbortTriggered", clientID);
}
- template <typename EventT>
+ template <typename AronEventT, typename EventT>
bool
Writer::storeImpl(const EventT& event,
const std::string& eventName,
@@ -102,7 +108,7 @@ namespace armarx::navigation::memory::client::events
ARMARX_INFO << "Storing event `" << eventName << "` in memory.";
- armem::Time ts = armem::Time(armem::Duration::MicroSeconds(event.timestamp.count()));
+ const auto& timestamp = event.timestamp;
armem::EntityUpdate update;
update.entityID = armem::MemoryID()
@@ -110,14 +116,14 @@ namespace armarx::navigation::memory::client::events
.withCoreSegmentName(properties().coreSegmentName)
.withProviderSegmentName(clientID)
.withEntityName(eventName)
- .withTimestamp(ts);
- update.timeCreated = ts;
+ .withTimestamp(timestamp);
+ update.timeCreated = timestamp;
- // FIXME auto dto = core::toAron(event);
+ const auto dto = core::toAron<AronEventT>(event);
aron::data::DictPtr element(new aron::data::Dict);
element->addElement("event", std::make_shared<aron::data::String>(eventName));
- // FIXME element->addElement("data", dto.toAron());
+ element->addElement("data", dto.toAron());
update.instancesData = {element};
diff --git a/source/armarx/navigation/memory/client/events/Writer.h b/source/armarx/navigation/memory/client/events/Writer.h
index 833f3705..16812c15 100644
--- a/source/armarx/navigation/memory/client/events/Writer.h
+++ b/source/armarx/navigation/memory/client/events/Writer.h
@@ -43,7 +43,7 @@ namespace armarx::navigation::memory::client::events
bool store(const core::InternalErrorEvent& event, const std::string& clientID);
- // bool store(const core::GoalReachedEvent& event, const std::string& clientID);
+ bool store(const core::UserAbortTriggeredEvent& event, const std::string& clientID);
bool store(const core::GlobalPlanningFailedEvent& event, const std::string& clientID);
bool store(const core::MovementStartedEvent& event, const std::string& clientID);
@@ -57,8 +57,7 @@ namespace armarx::navigation::memory::client::events
bool store(const armem::Commit& commit);
bool store(const armem::EntityUpdate& commit);
-
- template <typename EventT>
+ template <typename AronEventT, typename EventT>
bool
storeImpl(const EventT& event, const std::string& eventName, const std::string& clientID);
};
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.h b/source/armarx/navigation/memory/client/parameterization/Writer.h
index 58c6cd22..73b927d7 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.h
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.h
@@ -38,7 +38,7 @@ namespace armarx::navigation::memory::client::param
bool store(const std::unordered_map<core::StackLayer, aron::data::DictPtr>& stack,
const std::string& clientID,
- const core::TimestampUs& timestamp);
+ const armarx::core::time::DateTime& timestamp);
std::string propertyPrefix() const override;
Properties defaultProperties() const override;
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
index 60adb3a6..8f3b9d42 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
@@ -35,7 +35,7 @@ namespace armarx::navigation::server
void
MemoryPublisher::userAbortTriggered(const core::UserAbortTriggeredEvent& event)
{
- // eventsWriter->store(event, clientId);
+ eventsWriter->store(event, clientId);
}
--
GitLab
From fe49a3ea195237ebdd78ea263e4e3350caf1e05f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 4 May 2022 18:38:32 +0200
Subject: [PATCH 4/8] implementing additional events
---
source/armarx/navigation/core/aron/Events.xml | 15 +++++++++
.../navigation/core/aron_conversions.cpp | 31 +++++++++++++++++++
.../armarx/navigation/core/aron_conversions.h | 16 ++++++++--
.../memory/client/events/Writer.cpp | 26 +++++++++++++---
.../navigation/memory/client/events/Writer.h | 6 ++--
.../event_publishing/MemoryPublisher.cpp | 8 ++---
6 files changed, 88 insertions(+), 14 deletions(-)
diff --git a/source/armarx/navigation/core/aron/Events.xml b/source/armarx/navigation/core/aron/Events.xml
index d7d29939..a0be50ec 100644
--- a/source/armarx/navigation/core/aron/Events.xml
+++ b/source/armarx/navigation/core/aron/Events.xml
@@ -53,5 +53,20 @@
</ObjectChild>
</Object>
+ <Object name='armarx::navigation::core::arondto::SafetyThrottlingTriggeredEvent'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='throttlingFactor'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::core::arondto::SafetyStopTriggeredEvent'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ </Object>
+
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/armarx/navigation/core/aron_conversions.cpp b/source/armarx/navigation/core/aron_conversions.cpp
index 4a974067..0aeaa75b 100644
--- a/source/armarx/navigation/core/aron_conversions.cpp
+++ b/source/armarx/navigation/core/aron_conversions.cpp
@@ -196,4 +196,35 @@ namespace armarx::navigation::core
aron::fromAron(dto.pose, bo.pose.matrix());
}
+
+ void
+ toAron(armarx::navigation::core::arondto::SafetyThrottlingTriggeredEvent& dto,
+ const armarx::navigation::core::SafetyThrottlingTriggeredEvent& bo)
+ {
+ aron::toAron(dto.pose, bo.pose.matrix());
+ aron::toAron(dto.throttlingFactor, bo.throttlingFactor);
+ }
+
+ void
+ fromAron(const armarx::navigation::core::arondto::SafetyThrottlingTriggeredEvent& dto,
+ armarx::navigation::core::SafetyThrottlingTriggeredEvent& bo)
+ {
+ aron::fromAron(dto.pose, bo.pose.matrix());
+ aron::fromAron(dto.throttlingFactor, bo.throttlingFactor);
+ }
+
+ void
+ toAron(armarx::navigation::core::arondto::SafetyStopTriggeredEvent& dto,
+ const armarx::navigation::core::SafetyStopTriggeredEvent& bo)
+ {
+ aron::toAron(dto.pose, bo.pose.matrix());
+ }
+
+ void
+ fromAron(const armarx::navigation::core::arondto::SafetyStopTriggeredEvent& dto,
+ armarx::navigation::core::SafetyStopTriggeredEvent& bo)
+ {
+ aron::fromAron(dto.pose, bo.pose.matrix());
+ }
+
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/aron_conversions.h b/source/armarx/navigation/core/aron_conversions.h
index 654fd409..50f6cb44 100644
--- a/source/armarx/navigation/core/aron_conversions.h
+++ b/source/armarx/navigation/core/aron_conversions.h
@@ -63,7 +63,7 @@ namespace armarx::navigation::core
// Events
void toAron(armarx::navigation::core::arondto::GoalReachedEvent& dto,
const armarx::navigation::core::GoalReachedEvent& bo);
-
+
void fromAron(const armarx::navigation::core::arondto::GoalReachedEvent& dto,
armarx::navigation::core::GoalReachedEvent& bo);
@@ -75,7 +75,7 @@ namespace armarx::navigation::core
void toAron(armarx::navigation::core::arondto::GlobalPlanningFailedEvent& dto,
const armarx::navigation::core::GlobalPlanningFailedEvent& bo);
-
+
void fromAron(const armarx::navigation::core::arondto::MovementStartedEvent& dto,
armarx::navigation::core::MovementStartedEvent& bo);
@@ -97,5 +97,17 @@ namespace armarx::navigation::core
void fromAron(const armarx::navigation::core::arondto::UserAbortTriggeredEvent& dto,
armarx::navigation::core::UserAbortTriggeredEvent& bo);
+ void toAron(armarx::navigation::core::arondto::SafetyThrottlingTriggeredEvent& dto,
+ const armarx::navigation::core::SafetyThrottlingTriggeredEvent& bo);
+
+ void fromAron(const armarx::navigation::core::arondto::SafetyThrottlingTriggeredEvent& dto,
+ armarx::navigation::core::SafetyThrottlingTriggeredEvent& bo);
+
+ void toAron(armarx::navigation::core::arondto::SafetyStopTriggeredEvent& dto,
+ const armarx::navigation::core::SafetyStopTriggeredEvent& bo);
+
+ void fromAron(const armarx::navigation::core::arondto::SafetyStopTriggeredEvent& dto,
+ armarx::navigation::core::SafetyStopTriggeredEvent& bo);
+
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/memory/client/events/Writer.cpp b/source/armarx/navigation/memory/client/events/Writer.cpp
index 4325c44a..ab64f942 100644
--- a/source/armarx/navigation/memory/client/events/Writer.cpp
+++ b/source/armarx/navigation/memory/client/events/Writer.cpp
@@ -70,7 +70,8 @@ namespace armarx::navigation::memory::client::events
bool
Writer::store(const core::WaypointReachedEvent& event, const std::string& clientID)
{
- return storeImpl<core::arondto::WaypointReachedEvent>(event, "WaypointReachedEvent", clientID);
+ return storeImpl<core::arondto::WaypointReachedEvent>(
+ event, "WaypointReachedEvent", clientID);
}
bool
@@ -82,19 +83,36 @@ namespace armarx::navigation::memory::client::events
bool
Writer::store(const core::GlobalPlanningFailedEvent& event, const std::string& clientID)
{
- return storeImpl<core::arondto::GlobalPlanningFailedEvent>(event, "GlobalPlanningFailedEvent", clientID);
+ return storeImpl<core::arondto::GlobalPlanningFailedEvent>(
+ event, "GlobalPlanningFailedEvent", clientID);
}
bool
Writer::store(const core::MovementStartedEvent& event, const std::string& clientID)
{
- return storeImpl<core::arondto::MovementStartedEvent>(event, "MovementStartedEvent", clientID);
+ return storeImpl<core::arondto::MovementStartedEvent>(
+ event, "MovementStartedEvent", clientID);
}
bool
Writer::store(const core::UserAbortTriggeredEvent& event, const std::string& clientID)
{
- return storeImpl<core::arondto::UserAbortTriggeredEvent>(event, "UserAbortTriggered", clientID);
+ return storeImpl<core::arondto::UserAbortTriggeredEvent>(
+ event, "UserAbortTriggered", clientID);
+ }
+
+ bool
+ Writer::store(const core::SafetyThrottlingTriggeredEvent& event, const std::string& clientID)
+ {
+ return storeImpl<core::arondto::SafetyThrottlingTriggeredEvent>(
+ event, "SafetyThrottlingTriggeredEvent", clientID);
+ }
+
+ bool
+ Writer::store(const core::SafetyStopTriggeredEvent& event, const std::string& clientID)
+ {
+ return storeImpl<core::arondto::SafetyStopTriggeredEvent>(
+ event, "SafetyStopTriggeredEvent", clientID);
}
diff --git a/source/armarx/navigation/memory/client/events/Writer.h b/source/armarx/navigation/memory/client/events/Writer.h
index 16812c15..7ad07d7d 100644
--- a/source/armarx/navigation/memory/client/events/Writer.h
+++ b/source/armarx/navigation/memory/client/events/Writer.h
@@ -38,15 +38,13 @@ namespace armarx::navigation::memory::client::events
using armem::client::util::SimpleWriterBase::SimpleWriterBase;
bool store(const core::GoalReachedEvent& event, const std::string& clientID);
-
bool store(const core::WaypointReachedEvent& event, const std::string& clientID);
-
bool store(const core::InternalErrorEvent& event, const std::string& clientID);
-
bool store(const core::UserAbortTriggeredEvent& event, const std::string& clientID);
-
bool store(const core::GlobalPlanningFailedEvent& event, const std::string& clientID);
bool store(const core::MovementStartedEvent& event, const std::string& clientID);
+ bool store(const core::SafetyThrottlingTriggeredEvent& event, const std::string& clientID);
+ bool store(const core::SafetyStopTriggeredEvent& event, const std::string& clientID);
std::string propertyPrefix() const override;
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
index 8f3b9d42..6226169b 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
@@ -21,14 +21,14 @@ namespace armarx::navigation::server
void
MemoryPublisher::safetyThrottlingTriggered(const core::SafetyThrottlingTriggeredEvent& event)
{
- // eventsWriter->store(event, clientId);
+ eventsWriter->store(event, clientId);
}
void
MemoryPublisher::safetyStopTriggered(const core::SafetyStopTriggeredEvent& event)
{
- // eventsWriter->store(event, clientId);
+ eventsWriter->store(event, clientId);
}
@@ -54,7 +54,7 @@ namespace armarx::navigation::server
void
MemoryPublisher::localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res)
{
- // TODO resultWriter->store(res, clientId);
+ // resultWriter->store(res, clientId);
}
void
@@ -63,7 +63,7 @@ namespace armarx::navigation::server
resultWriter->store(res, clientId);
}
- // TODO event with message or reason (enum)
+ // TODO(fabian.reister): event with message or reason (enum)
void
MemoryPublisher::globalPlanningFailed(const core::GlobalPlanningFailedEvent& event)
{
--
GitLab
From f3cd80fb09066d18fc79a60aee8cbf19371fd205 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 4 May 2022 19:06:56 +0200
Subject: [PATCH 5/8] moving hard coded names to events.h (will be used
elsewhere as well...)
---
source/armarx/navigation/core/events.h | 12 ++++++++++++
.../navigation/memory/client/events/Writer.cpp | 18 ++++++++++--------
2 files changed, 22 insertions(+), 8 deletions(-)
diff --git a/source/armarx/navigation/core/events.h b/source/armarx/navigation/core/events.h
index fd95336f..09ef7781 100644
--- a/source/armarx/navigation/core/events.h
+++ b/source/armarx/navigation/core/events.h
@@ -10,6 +10,18 @@
namespace armarx::navigation::core
{
+ namespace event_names
+ {
+ inline const std::string GlobalPlanningFailed = "GlobalPlanningFailedEvent";
+ inline const std::string MovementStarted = "MovementStartedEvent";
+ inline const std::string GoalReached = "GoalReachedEvent";
+ inline const std::string WaypointReached = "WaypointReachedEvent";
+ inline const std::string SafetyThrottlingTriggered = "SafetyThrottlingTriggeredEvent";
+ inline const std::string SafetyStopTriggered = "SafetyStopTriggeredEvent";
+ inline const std::string UserAbortTriggered = "UserAbortTriggeredEvent";
+ inline const std::string InternalError = "InternalErrorEventEvent";
+ } // namespace event_names
+
struct Event
{
armarx::core::time::DateTime timestamp;
diff --git a/source/armarx/navigation/memory/client/events/Writer.cpp b/source/armarx/navigation/memory/client/events/Writer.cpp
index ab64f942..18220950 100644
--- a/source/armarx/navigation/memory/client/events/Writer.cpp
+++ b/source/armarx/navigation/memory/client/events/Writer.cpp
@@ -64,55 +64,57 @@ namespace armarx::navigation::memory::client::events
bool
Writer::store(const core::GoalReachedEvent& event, const std::string& clientID)
{
- return storeImpl<core::arondto::GoalReachedEvent>(event, "GoalReachedEvent", clientID);
+ return storeImpl<core::arondto::GoalReachedEvent>(
+ event, core::event_names::GoalReached, clientID);
}
bool
Writer::store(const core::WaypointReachedEvent& event, const std::string& clientID)
{
return storeImpl<core::arondto::WaypointReachedEvent>(
- event, "WaypointReachedEvent", clientID);
+ event, core::event_names::WaypointReached, clientID);
}
bool
Writer::store(const core::InternalErrorEvent& event, const std::string& clientID)
{
- return storeImpl<core::arondto::InternalErrorEvent>(event, "InternalErrorEvent", clientID);
+ return storeImpl<core::arondto::InternalErrorEvent>(
+ event, core::event_names::InternalError, clientID);
}
bool
Writer::store(const core::GlobalPlanningFailedEvent& event, const std::string& clientID)
{
return storeImpl<core::arondto::GlobalPlanningFailedEvent>(
- event, "GlobalPlanningFailedEvent", clientID);
+ event, core::event_names::GlobalPlanningFailed, clientID);
}
bool
Writer::store(const core::MovementStartedEvent& event, const std::string& clientID)
{
return storeImpl<core::arondto::MovementStartedEvent>(
- event, "MovementStartedEvent", clientID);
+ event, core::event_names::MovementStarted, clientID);
}
bool
Writer::store(const core::UserAbortTriggeredEvent& event, const std::string& clientID)
{
return storeImpl<core::arondto::UserAbortTriggeredEvent>(
- event, "UserAbortTriggered", clientID);
+ event, core::event_names::UserAbortTriggered, clientID);
}
bool
Writer::store(const core::SafetyThrottlingTriggeredEvent& event, const std::string& clientID)
{
return storeImpl<core::arondto::SafetyThrottlingTriggeredEvent>(
- event, "SafetyThrottlingTriggeredEvent", clientID);
+ event, core::event_names::SafetyThrottlingTriggered, clientID);
}
bool
Writer::store(const core::SafetyStopTriggeredEvent& event, const std::string& clientID)
{
return storeImpl<core::arondto::SafetyStopTriggeredEvent>(
- event, "SafetyStopTriggeredEvent", clientID);
+ event, core::event_names::SafetyStopTriggered, clientID);
}
--
GitLab
From 3bdae14c6cc3f9a705fb55732cdfb209dfa1f47f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 4 May 2022 19:07:27 +0200
Subject: [PATCH 6/8] conversion fromAron for events in MemorySubscriber
---
.../client/services/MemorySubscriber.cpp | 46 ++++++++++++-------
1 file changed, 30 insertions(+), 16 deletions(-)
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.cpp b/source/armarx/navigation/client/services/MemorySubscriber.cpp
index 9f530dd2..efb052ee 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.cpp
+++ b/source/armarx/navigation/client/services/MemorySubscriber.cpp
@@ -1,5 +1,6 @@
#include <algorithm>
#include <mutex>
+#include <type_traits>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
@@ -10,7 +11,9 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include "armarx/navigation/core/aron_conversions.h"
#include <armarx/navigation/client/services/MemorySubscriber.h>
+#include <armarx/navigation/core/aron/Events.aron.generated.h>
#include <armarx/navigation/core/events.h>
@@ -54,6 +57,17 @@ namespace armarx::navigation::client
handleEvents(qResult.memory);
}
+ template <typename AronEventT, typename EventT>
+ auto
+ fromAron(const armem::wm::EntityInstance& entity, EventT& bo)
+ {
+ static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass,
+ AronEventT>::value);
+
+ const auto dto = AronEventT::FromAron(entity.data());
+ core::fromAron(dto, bo);
+ return bo;
+ }
void
MemorySubscriber::handleEvent(const armem::wm::EntityInstance& memoryEntity)
@@ -69,59 +83,59 @@ namespace armarx::navigation::client
ARMARX_IMPORTANT << "Handling event `" << eventName << "`";
// factory
- if (eventName == "GlobalPlanningFailedEvent")
+ if (eventName == core::event_names::GlobalPlanningFailed)
{
core::GlobalPlanningFailedEvent evt;
- // fromAron(memoryEntity.data(), evt);
+ fromAron<core::arondto::GlobalPlanningFailedEvent>(memoryEntity, evt);
globalPlanningFailed(evt);
}
- else if (eventName == "MovementStartedEvent")
+ else if (eventName == core::event_names::MovementStarted)
{
core::MovementStartedEvent evt;
- // fromAron(memoryEntity.data(), evt);
+ fromAron<core::arondto::MovementStartedEvent>(memoryEntity, evt);
movementStarted(evt);
}
- else if (eventName == "GoalReachedEvent")
+ else if (eventName == core::event_names::GoalReached)
{
core::GoalReachedEvent evt;
- // fromAron(memoryEntity.data(), evt);
+ fromAron<core::arondto::GoalReachedEvent>(memoryEntity, evt);
goalReached(evt);
}
- else if (eventName == "WaypointReachedEvent")
+ else if (eventName == core::event_names::WaypointReached)
{
core::WaypointReachedEvent evt;
- // fromAron(memoryEntity.data(), evt);
+ fromAron<core::arondto::WaypointReachedEvent>(memoryEntity, evt);
waypointReached(evt);
}
- else if (eventName == "SafetyThrottlingTriggeredEvent")
+ else if (eventName == core::event_names::SafetyThrottlingTriggered)
{
core::SafetyThrottlingTriggeredEvent evt;
- // fromAron(memoryEntity.data(), evt);
+ fromAron<core::arondto::SafetyThrottlingTriggeredEvent>(memoryEntity, evt);
safetyThrottlingTriggered(evt);
}
- else if (eventName == "SafetyStopTriggeredEvent")
+ else if (eventName == core::event_names::SafetyStopTriggered)
{
core::SafetyStopTriggeredEvent evt;
- // fromAron(memoryEntity.data(), evt);
+ fromAron<core::arondto::SafetyStopTriggeredEvent>(memoryEntity, evt);
safetyStopTriggered(evt);
}
- else if (eventName == "UserAbortTriggeredEvent")
+ else if (eventName == core::event_names::UserAbortTriggered)
{
core::UserAbortTriggeredEvent evt;
- // fromAron(memoryEntity.data(), evt);
+ fromAron<core::arondto::UserAbortTriggeredEvent>(memoryEntity, evt);
userAbortTriggered(evt);
}
- else if (eventName == "InternalErrorEvent")
+ else if (eventName == core::event_names::InternalError)
{
core::InternalErrorEvent evt;
- // fromAron(memoryEntity.data(), evt);
+ fromAron<core::arondto::InternalErrorEvent>(memoryEntity, evt);
internalError(evt);
}
--
GitLab
From 3c2eee4af9346d6e1a61fc90b162149160d22499 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Sat, 7 May 2022 11:59:04 +0200
Subject: [PATCH 7/8] fixing conversion of aron event
---
.../navigation/client/services/MemorySubscriber.cpp | 8 +++++++-
1 file changed, 7 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.cpp b/source/armarx/navigation/client/services/MemorySubscriber.cpp
index efb052ee..89769e9f 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.cpp
+++ b/source/armarx/navigation/client/services/MemorySubscriber.cpp
@@ -2,6 +2,7 @@
#include <mutex>
#include <type_traits>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
@@ -54,6 +55,7 @@ namespace armarx::navigation::client
return;
}
+ ARMARX_TRACE;
handleEvents(qResult.memory);
}
@@ -64,7 +66,11 @@ namespace armarx::navigation::client
static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass,
AronEventT>::value);
- const auto dto = AronEventT::FromAron(entity.data());
+ // see events::Writer::storeImpl
+ const auto dataDict = aron::data::Dict::DynamicCastAndCheck(entity.data()->getElement("data"));
+ ARMARX_CHECK_NOT_NULL(dataDict);
+ const auto dto = AronEventT::FromAron(dataDict);
+
core::fromAron(dto, bo);
return bo;
}
--
GitLab
From 72ed9c4ade6000f99945b622b53138de54a3f3f7 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Sat, 7 May 2022 12:07:20 +0200
Subject: [PATCH 8/8] reduced verbosity of navigator
---
scenarios/PlatformNavigation/config/navigator.cfg | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 4f2c5e40..f87a70d4 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -425,6 +425,6 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-ArmarX.Verbosity = Verbose
+# ArmarX.Verbosity = Info
--
GitLab