diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg index 4f2c5e40eb5b4bf29345341a9c49c5866f23f45a..f87a70d420635f34b2461efe27ffc658e5e727fa 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 diff --git a/source/armarx/navigation/client/services/MemorySubscriber.cpp b/source/armarx/navigation/client/services/MemorySubscriber.cpp index 9f530dd216430cb505379f1cadbba6b3c561b58f..89769e9f6887fb4177c5922fb9f8185fef4479bd 100644 --- a/source/armarx/navigation/client/services/MemorySubscriber.cpp +++ b/source/armarx/navigation/client/services/MemorySubscriber.cpp @@ -1,6 +1,8 @@ #include <algorithm> #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> @@ -10,7 +12,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> @@ -51,9 +55,25 @@ namespace armarx::navigation::client return; } + ARMARX_TRACE; 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); + + // 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; + } void MemorySubscriber::handleEvent(const armem::wm::EntityInstance& memoryEntity) @@ -69,59 +89,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); } diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp index 29845189fd78ee82f4066c6d945c746aac47c57d..890ffc259c094f90e8398051aa68ddb815b22911 100644 --- a/source/armarx/navigation/components/Navigator/Navigator.cpp +++ b/source/armarx/navigation/components/Navigator/Navigator.cpp @@ -349,8 +349,6 @@ namespace armarx::navigation::components visualizeSPFA(); navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode)); - - // navigators.at(callerId).moveTo() } void @@ -395,6 +393,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 @@ -408,15 +411,12 @@ namespace armarx::navigation::components navigator.stop(); } - // FIXME use another event type e.g. StoppedEvent - 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); } } diff --git a/source/armarx/navigation/core/aron/Events.xml b/source/armarx/navigation/core/aron/Events.xml index d2009212663e3380c1c1aac26b3a3146f2a0e09d..a0be50ecbc393daf60798934b61cde23862f8835 100644 --- a/source/armarx/navigation/core/aron/Events.xml +++ b/source/armarx/navigation/core/aron/Events.xml @@ -41,5 +41,32 @@ </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> + + <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 84b847e3b427dc34f891d948709ae5007d4f6104..0aeaa75b3a936246a3a700348d4733f96478dff1 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,49 @@ 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()); + } + + + 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 4e0a7bd4e9ae9cb2cc4a4119b8a7a5216bbcda5d..50f6cb44c5ad62cdf76d14927010042206c30d4a 100644 --- a/source/armarx/navigation/core/aron_conversions.h +++ b/source/armarx/navigation/core/aron_conversions.h @@ -63,20 +63,51 @@ 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); + + 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/core/events.h b/source/armarx/navigation/core/events.h index f50fa3bb2417cbee4e4f4472e81c8836ff79d95c..09ef7781be7fc2f55e481d199c851f89b3d2d4db 100644 --- a/source/armarx/navigation/core/events.h +++ b/source/armarx/navigation/core/events.h @@ -1,19 +1,30 @@ #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 { + 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 { - 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 4e619612a1a3c1ad37d7712b2e65b66ba6ab80b2..fb3db7cb1dc0f96915ed982c820690a6fc5ad1e1 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 24bc5a127ce0ec0e50272d3dd66a5a67227a204f..31d6cf7e5f7c521aead76f179ef56b83f20f6f2a 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 f17665c8b5e6cf6afa2f988bf20620940e2a9bc9..fdb74e854d19b41fd47a0db71a3e7a71f22fe0dd 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 0e5b96facb6dd53297fab1935525624a86b61ac3..a0529cb0f42b37215936a30fd3038d486ab80492 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/events/Writer.cpp b/source/armarx/navigation/memory/client/events/Writer.cpp index b15a93ca4cf232f4b304129ea51b3e462f592811..182209501e810a9c3e62709764501743674eaa45 100644 --- a/source/armarx/navigation/memory/client/events/Writer.cpp +++ b/source/armarx/navigation/memory/client/events/Writer.cpp @@ -64,35 +64,61 @@ 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, core::event_names::GoalReached, clientID); } bool Writer::store(const core::WaypointReachedEvent& event, const std::string& clientID) { - return storeImpl(event, "WaypointReachedEvent", clientID); + return storeImpl<core::arondto::WaypointReachedEvent>( + event, core::event_names::WaypointReached, clientID); } bool Writer::store(const core::InternalErrorEvent& event, const std::string& clientID) { - return storeImpl(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(event, "GlobalPlanningFailedEvent", clientID); + return storeImpl<core::arondto::GlobalPlanningFailedEvent>( + event, core::event_names::GlobalPlanningFailed, clientID); } bool Writer::store(const core::MovementStartedEvent& event, const std::string& clientID) { - return storeImpl(event, "MovementStartedEvent", clientID); + return storeImpl<core::arondto::MovementStartedEvent>( + event, core::event_names::MovementStarted, clientID); + } + + bool + Writer::store(const core::UserAbortTriggeredEvent& event, const std::string& clientID) + { + return storeImpl<core::arondto::UserAbortTriggeredEvent>( + event, core::event_names::UserAbortTriggered, clientID); + } + + bool + Writer::store(const core::SafetyThrottlingTriggeredEvent& event, const std::string& clientID) + { + return storeImpl<core::arondto::SafetyThrottlingTriggeredEvent>( + event, core::event_names::SafetyThrottlingTriggered, clientID); + } + + bool + Writer::store(const core::SafetyStopTriggeredEvent& event, const std::string& clientID) + { + return storeImpl<core::arondto::SafetyStopTriggeredEvent>( + event, core::event_names::SafetyStopTriggered, clientID); } - template <typename EventT> + template <typename AronEventT, typename EventT> bool Writer::storeImpl(const EventT& event, const std::string& eventName, @@ -102,7 +128,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 +136,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 833f37052d34829fc98092320a4e9827717da937..7ad07d7d4ebc2cbe051c59b8b7dedb5d5d158e98 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::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); + 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; @@ -57,8 +55,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.cpp b/source/armarx/navigation/memory/client/parameterization/Writer.cpp index 23a8d443d51a244aaa53bef002aeb1b7599b8322..7becf03f333189bb4372307e83096064b68970a3 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/memory/client/parameterization/Writer.h b/source/armarx/navigation/memory/client/parameterization/Writer.h index 58c6cd2263061b2a37d55c134d688b1efb1ed76f..73b927d7fd0b6c644fde50164ed6b7129cc9e56b 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 60adb3a685b6f88f9f1976b891db36fe6d4e1978..6226169b07d7b9f6daf5d38aa7b584ba90748df4 100644 --- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp +++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp @@ -21,21 +21,21 @@ 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); } void MemoryPublisher::userAbortTriggered(const core::UserAbortTriggeredEvent& 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) { diff --git a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp index b4334831882a68d6947b90459b0ca03e72c0939d..89be35bb4b75a76404109bb1bad79ad15691535d 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 0eb1e0b7637c9fa0c37edbc5ea768e048150a698..ea81edee3ccf232b7404c34b7e56432354a5d8a2 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: