diff --git a/source/Navigation/components/Navigator/Navigator.cpp b/source/Navigation/components/Navigator/Navigator.cpp index c5fbc1be65269c39eeeb442f59e003e2a84948e6..088a637ff0f7367b2dd99a467202609a9c7ef276 100644 --- a/source/Navigation/components/Navigator/Navigator.cpp +++ b/source/Navigation/components/Navigator/Navigator.cpp @@ -68,11 +68,14 @@ convert(const std::vector<Eigen::Matrix4f>& wps) } armarx::nav::components::Navigator::Navigator() : - writerGlobPlan(memoryNameSystem), parameterizationReader(memoryNameSystem), parameterizationWriter(memoryNameSystem), - parameterizationService(¶meterizationReader, ¶meterizationWriter) + eventsWriter(memoryNameSystem), + resultsWriter(memoryNameSystem), + parameterizationService(¶meterizationReader, ¶meterizationWriter), + publisher(&resultsWriter, &eventsWriter) { + ; } @@ -108,7 +111,7 @@ armarx::nav::components::Navigator::onConnectComponent() // TODO dynamic scene // TODO memory // TODO param (10) - writerGlobPlan.connect(); + resultsWriter.connect(); // parameterizationReader.connect(); parameterizationWriter.connect(); @@ -131,7 +134,7 @@ armarx::nav::components::Navigator::onReconnectComponent() // TODO not in all cases meaningful //resume(); - writerGlobPlan.connect(); + resultsWriter.connect(); // parameterizationReader.connect(); parameterizationWriter.connect(); @@ -181,7 +184,7 @@ armarx::nav::components::Navigator::createConfig(const aron::data::AronDictPtr& server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene); memoryIntrospectors.emplace_back( - std::make_unique<server::MemoryIntrospector>(writerGlobPlan, callerId)); + std::make_unique<server::MemoryIntrospector>(resultsWriter, callerId)); navigators.emplace( std::piecewise_construct, @@ -295,7 +298,7 @@ armarx::nav::components::Navigator::createPropertyDefinitions() // Add an optionalproperty. // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz."); - writerGlobPlan.registerPropertyDefinitions(def); + resultsWriter.registerPropertyDefinitions(def); // parameterizationReader.registerPropertyDefinitions(def); parameterizationWriter.registerPropertyDefinitions(def); diff --git a/source/Navigation/components/Navigator/Navigator.h b/source/Navigation/components/Navigator/Navigator.h index ef8b6084c78ed2c9db6490f086bcc3fa2bc4fb51..a0415cad94365c92e8e604f6884d2b674ffaf572 100644 --- a/source/Navigation/components/Navigator/Navigator.h +++ b/source/Navigation/components/Navigator/Navigator.h @@ -159,7 +159,6 @@ namespace armarx::nav::components core::Scene scene; std::optional<server::PlatformUnitExecutor> executor; - server::MemoryPublisher publisher; // TODO: Implement. std::optional<server::ArvizIntrospector> introspector; std::unordered_map<std::string, server::Navigator> navigators; @@ -170,15 +169,17 @@ namespace armarx::nav::components // TODO maybe as optional, but requires some effort std::unique_ptr<NavigatorRemoteGui> navRemoteGui; - mem::client::stack_result::glob_plan::Writer writerGlobPlan; // unique_ptr to avoid dangling refs std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors; mem::client::param::Reader parameterizationReader; mem::client::param::Writer parameterizationWriter; + mem::client::events::Writer eventsWriter; + mem::client::stack_result::Writer resultsWriter; server::MemoryParameterizationService parameterizationService; + server::MemoryPublisher publisher; core::ChronoMonotonicTimeServer timeServer; }; diff --git a/source/Navigation/libraries/core/CMakeLists.txt b/source/Navigation/libraries/core/CMakeLists.txt index 2491642595950325c0be186450a58b0faa235a82..2e5f26a5838248fcdc0e7d9b4805712d34299546 100644 --- a/source/Navigation/libraries/core/CMakeLists.txt +++ b/source/Navigation/libraries/core/CMakeLists.txt @@ -10,6 +10,7 @@ armarx_add_library( ArmarXCoreObservers # variant # RobotAPI aron + aroncommon RobotAPIInterfaces VirtualRobot range-v3 @@ -48,6 +49,7 @@ armarx_enable_aron_file_generation_for_target( aron/TwistLimits.xml aron/PIDParams.xml aron/Trajectory.xml + aron/Events.xml ) # find_package(MyLib QUIET) armarx_build_if(MyLib_FOUND "MyLib not available") diff --git a/source/Navigation/libraries/core/Trajectory.cpp b/source/Navigation/libraries/core/Trajectory.cpp index b9068d38053558d5d5ca2cd8678a763e1ff80132..c57965b91bb7e8bd222eda965af07b67fd0bc9da 100644 --- a/source/Navigation/libraries/core/Trajectory.cpp +++ b/source/Navigation/libraries/core/Trajectory.cpp @@ -168,6 +168,13 @@ namespace armarx::nav::core const auto& wpBefore = pts.at(i); const auto& wpAfter = pts.at(i + 1); + if ((wpBefore.waypoint.pose.translation() - wpAfter.waypoint.pose.translation()) + .norm() < 1.F) + { + ARMARX_WARNING << "Trajectory segment " << i << " too small ..."; + continue; + } + const auto closestPoint = VirtualRobot::MathTools::nearestPointOnSegment<Position>( wpBefore.waypoint.pose.translation(), wpAfter.waypoint.pose.translation(), point); diff --git a/source/Navigation/libraries/core/aron/Events.xml b/source/Navigation/libraries/core/aron/Events.xml new file mode 100644 index 0000000000000000000000000000000000000000..1813f23b191a80dbd905ac1be6b01f1f15de5104 --- /dev/null +++ b/source/Navigation/libraries/core/aron/Events.xml @@ -0,0 +1,39 @@ +<?xml version="1.0" encoding="UTF-8" ?> +<AronTypeDefinition> + <CodeIncludes> + <!-- <Include include="<Navigation/libraries/core/aron/GlobalPlannerParams.aron.generated.h>" /> --> + </CodeIncludes> + <AronIncludes> + <!-- <Include include="<Navigation/libraries/global_planning/aron/GlobalPlannerParams.xml>" /> --> + </AronIncludes> + + <GenerateTypes> + + <Object name='armarx::nav::core::arondto::GoalReachedEvent'> + <ObjectChild key='pose'> + <Pose /> + </ObjectChild> + </Object> + + <Object name='armarx::nav::core::arondto::WaypointReachedEvent'> + <ObjectChild key='pose'> + <Pose /> + </ObjectChild> + + <ObjectChild key='waypointId'> + <Int /> + </ObjectChild> + </Object> + + <Object name='armarx::nav::core::arondto::InternalErrorEvent'> + <ObjectChild key='pose'> + <Pose /> + </ObjectChild> + + <ObjectChild key='message'> + <String /> + </ObjectChild> + </Object> + + </GenerateTypes> +</AronTypeDefinition> diff --git a/source/Navigation/libraries/core/aron_conversions.cpp b/source/Navigation/libraries/core/aron_conversions.cpp index a6d4617c181b4a57ad11a427063d2b745d6f758e..34b64c806c42ef3a67875051daa5cd6d4f27aeae 100644 --- a/source/Navigation/libraries/core/aron_conversions.cpp +++ b/source/Navigation/libraries/core/aron_conversions.cpp @@ -12,6 +12,8 @@ namespace armarx::nav::core { + + void toAron(arondto::TrajectoryPoint& dto, const TrajectoryPoint& bo) { @@ -43,15 +45,17 @@ namespace armarx::nav::core void fromAron(const arondto::Trajectory& dto, Trajectory& bo) { - bo = Trajectory(dto.points | - ranges::views::transform( - [](const arondto::TrajectoryPoint& dto) -> TrajectoryPoint - { - TrajectoryPoint bo; - fromAron(dto, bo); - return bo; - }) | - ranges::to_vector); + bo = Trajectory( + dto.points | + ranges::views:: + transform( //static_cast<TrajectoryPoint (*)(const arondto::TrajectoryPoint&)>(&fromAron) + [](const arondto::TrajectoryPoint& dto) -> TrajectoryPoint + { + TrajectoryPoint bo; + fromAron(dto, bo); + return bo; + }) | + ranges::to_vector); } void @@ -99,4 +103,50 @@ namespace armarx::nav::core aron::fromAron(dto.angular, bo.angular); } + void + toAron(armarx::nav::core::arondto::GoalReachedEvent& dto, + const armarx::nav::core::GoalReachedEvent& bo) + { + aron::toAron(dto.pose, bo.pose); + } + + void + fromAron(const armarx::nav::core::arondto::GoalReachedEvent& dto, + armarx::nav::core::GoalReachedEvent& bo) + { + aron::fromAron(dto.pose, bo.pose.matrix()); + } + + void + toAron(armarx::nav::core::arondto::WaypointReachedEvent& dto, + const armarx::nav::core::WaypointReachedEvent& bo) + { + aron::toAron(dto.pose, bo.pose); + aron::toAron(dto.waypointId, bo.waypointId); + } + + void + fromAron(const armarx::nav::core::arondto::WaypointReachedEvent& dto, + armarx::nav::core::WaypointReachedEvent& bo) + { + aron::fromAron(dto.pose, bo.pose.matrix()); + aron::fromAron(dto.waypointId, bo.waypointId); + } + + void + toAron(armarx::nav::core::arondto::InternalErrorEvent& dto, + const armarx::nav::core::InternalErrorEvent& bo) + { + aron::toAron(dto.pose, bo.pose); + aron::toAron(dto.message, bo.message); + } + + void + fromAron(const armarx::nav::core::arondto::InternalErrorEvent& dto, + armarx::nav::core::InternalErrorEvent& bo) + { + aron::fromAron(dto.pose, bo.pose.matrix()); + aron::fromAron(dto.message, bo.message); + } + } // namespace armarx::nav::core diff --git a/source/Navigation/libraries/core/aron_conversions.h b/source/Navigation/libraries/core/aron_conversions.h index eb36a46e81ec75087e62265254b9d2a4d7b134c6..44efbf7bbeef1878a58b63f94f0220b659408258 100644 --- a/source/Navigation/libraries/core/aron_conversions.h +++ b/source/Navigation/libraries/core/aron_conversions.h @@ -24,11 +24,23 @@ #include "Navigation/libraries/core/aron/PIDParams.aron.generated.h" #include "Navigation/libraries/core/aron/TwistLimits.aron.generated.h" #include <Navigation/libraries/core/Trajectory.h> +#include <Navigation/libraries/core/aron/Events.aron.generated.h> #include <Navigation/libraries/core/aron/Trajectory.aron.generated.h> #include <Navigation/libraries/core/aron/Twist.aron.generated.h> +#include <Navigation/libraries/core/events.h> namespace armarx::nav::core { + // TODO fix ADL and remove this function + template <class DtoT, class BoT> + DtoT + toAron(const BoT& bo) + { + DtoT dto; + toAron(dto, bo); + return dto; + } + void toAron(arondto::TrajectoryPoint& dto, const TrajectoryPoint& bo); void fromAron(const arondto::TrajectoryPoint& dto, TrajectoryPoint& bo); @@ -47,4 +59,21 @@ namespace armarx::nav::core void fromAron(const armarx::nav::core::arondto::TwistLimits& dto, armarx::nav::core::TwistLimits& bo); + // Events + void toAron(armarx::nav::core::arondto::GoalReachedEvent& dto, + const armarx::nav::core::GoalReachedEvent& bo); + void fromAron(const armarx::nav::core::arondto::GoalReachedEvent& dto, + armarx::nav::core::GoalReachedEvent& bo); + + void toAron(armarx::nav::core::arondto::WaypointReachedEvent& dto, + const armarx::nav::core::WaypointReachedEvent& bo); + void fromAron(const armarx::nav::core::arondto::WaypointReachedEvent& dto, + armarx::nav::core::WaypointReachedEvent& bo); + + + void toAron(armarx::nav::core::arondto::InternalErrorEvent& dto, + const armarx::nav::core::InternalErrorEvent& bo); + void fromAron(const armarx::nav::core::arondto::InternalErrorEvent& dto, + armarx::nav::core::InternalErrorEvent& bo); + } // namespace armarx::nav::core diff --git a/source/Navigation/libraries/core/events.h b/source/Navigation/libraries/core/events.h index 4c288c40077d81f44cdec8ecd8c5d08f684b32fb..6505e7a5f35eec98fad03547801edef1c29313de 100644 --- a/source/Navigation/libraries/core/events.h +++ b/source/Navigation/libraries/core/events.h @@ -11,10 +11,15 @@ namespace armarx::nav::core { + struct Event + { + core::TimestampUs timestamp; + }; + /** * @brief Event describing that the targeted goal was successfully reached. */ - struct GoalReachedEvent + struct GoalReachedEvent : public Event { core::Pose pose; }; @@ -22,7 +27,7 @@ namespace armarx::nav::core /** * @brief Event describing that a user-defined waypoint was successfully reached. */ - struct WaypointReachedEvent + struct WaypointReachedEvent : public Event { core::Pose pose; int waypointId; @@ -33,7 +38,7 @@ namespace armarx::nav::core * * Can be configured. */ - struct SafetyThrottlingTriggeredEvent + struct SafetyThrottlingTriggeredEvent : public Event { core::Pose pose; @@ -49,7 +54,7 @@ namespace armarx::nav::core /** * @brief Event describing that for security reasons, the robot was stopped completely. */ - struct SafetyStopTriggeredEvent + struct SafetyStopTriggeredEvent : public Event { core::Pose pose; // TODO: Direction where safety-critical obstacle is (or range or whatever...). @@ -58,7 +63,7 @@ namespace armarx::nav::core /** * @brief Event describing that the user aborted the current execution. */ - struct UserAbortTriggeredEvent + struct UserAbortTriggeredEvent : public Event { core::Pose pose; }; @@ -66,7 +71,7 @@ namespace armarx::nav::core /** * @brief Event describing the occurance of an internal unhandled error. */ - struct InternalErrorEvent + struct InternalErrorEvent : public Event { core::Pose pose; std::string message; diff --git a/source/Navigation/libraries/memory/CMakeLists.txt b/source/Navigation/libraries/memory/CMakeLists.txt index ee3b56c5a3019ef165c63d006b6937d41fa3d84a..1a3bd85bcafdd48b04c00a37f3591e84adbda620 100644 --- a/source/Navigation/libraries/memory/CMakeLists.txt +++ b/source/Navigation/libraries/memory/CMakeLists.txt @@ -14,11 +14,13 @@ armarx_add_library( ./client/stack_result/Writer.cpp ./client/parameterization/Writer.cpp ./client/parameterization/Reader.cpp + ./client/events/Writer.cpp HEADERS #./memory.h ./client/stack_result/Writer.h ./client/parameterization/Writer.h ./client/parameterization/Reader.h + ./client/events/Writer.h ) add_library(Navigation::Memory ALIAS Memory) diff --git a/source/Navigation/libraries/memory/client/events/Writer.cpp b/source/Navigation/libraries/memory/client/events/Writer.cpp new file mode 100644 index 0000000000000000000000000000000000000000..54b4a7ff1d6a3773a4cf5db4ceb032d5baf2ff21 --- /dev/null +++ b/source/Navigation/libraries/memory/client/events/Writer.cpp @@ -0,0 +1,112 @@ +#include "Writer.h" + +#include <type_traits> + +#include "RobotAPI/libraries/aron/common/aron_conversions/core.h" +#include "RobotAPI/libraries/aron/core/navigator/data/primitive/String.h" + +#include "Navigation/libraries/core/aron_conversions.h" +#include "Navigation/libraries/core/constants.h" +#include "Navigation/libraries/core/events.h" +#include <Navigation/libraries/core/aron/Events.aron.generated.h> + +namespace armarx::nav::mem::client::events +{ + + std::string + Writer::propertyPrefix() const + { + return "mem.nav.events."; + } + + Writer::Properties + Writer::defaultProperties() const + { + return Properties{ + .memoryName = "Navigation", + .coreSegmentName = "Events", + .providerName = "" // clientId + }; + } + + + bool + Writer::store(const armem::Commit& commit) + { + std::lock_guard g{memoryWriterMutex()}; + armem::CommitResult updateResult = memoryWriter().commit(commit); + + ARMARX_DEBUG << updateResult; + + if (not updateResult.allSuccess()) + { + ARMARX_ERROR << updateResult.allErrorMessages(); + } + return updateResult.allSuccess(); + } + + bool + Writer::store(const armem::EntityUpdate& commit) + { + std::lock_guard g{memoryWriterMutex()}; + const armem::EntityUpdateResult updateResult = memoryWriter().commit(commit); + + ARMARX_DEBUG << updateResult; + + if (not updateResult.success) + { + ARMARX_ERROR << updateResult.errorMessage; + } + return updateResult.success; + } + + bool + Writer::store(const core::GoalReachedEvent& event, const std::string& clientID) + { + return storeImpl(event, "GoalReachedEvent", clientID); + } + + bool + Writer::store(const core::WaypointReachedEvent& event, const std::string& clientID) + { + return storeImpl(event, "WaypointReachedEvent", clientID); + } + + bool + Writer::store(const core::InternalErrorEvent& event, const std::string& clientID) + { + return storeImpl(event, "InternalErrorEvent", clientID); + } + + + template <typename EventT> + bool + Writer::storeImpl(const EventT& event, + const std::string& eventName, + const std::string& clientID) + { + static_assert(std::is_base_of<core::Event, EventT>::value, "Only events supported."); + + armem::Time ts = armem::Time::microSeconds(event.timestamp.count()); + + armem::EntityUpdate update; + update.entityID = armem::MemoryID() + .withMemoryName(properties().memoryName) + .withCoreSegmentName(properties().coreSegmentName) + .withProviderSegmentName(clientID) + .withEntityName(eventName) + .withTimestamp(ts); + update.timeCreated = ts; + + // FIXME auto dto = core::toAron(event); + + aron::datanavigator::DictNavigatorPtr element(new aron::datanavigator::DictNavigator); + element->addElement("event", + std::make_shared<aron::datanavigator::StringNavigator>(eventName)); + // FIXME element->addElement("data", dto.toAron()); + + update.instancesData = {element}; + + return store(update); + } +} // namespace armarx::nav::mem::client::events diff --git a/source/Navigation/libraries/memory/client/events/Writer.h b/source/Navigation/libraries/memory/client/events/Writer.h new file mode 100644 index 0000000000000000000000000000000000000000..6b45f6fbfc33d6d935d83be05a01db0a414d6041 --- /dev/null +++ b/source/Navigation/libraries/memory/client/events/Writer.h @@ -0,0 +1,59 @@ +/** + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Fabian Reister ( fabian dot reister at kit dot edu ) + * @date 2021 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include "RobotAPI/libraries/armem/client/util/SimpleWriterBase.h" +#include "RobotAPI/libraries/armem/core/Commit.h" + +#include "Navigation/libraries/core/constants.h" +#include "Navigation/libraries/core/events.h" +#include "Navigation/libraries/core/types.h" + + +namespace armarx::nav::mem::client::events +{ + + class Writer : virtual public armem::client::util::SimpleWriterBase + { + public: + 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); + + std::string propertyPrefix() const override; + Properties defaultProperties() const override; + + protected: + private: + bool store(const armem::Commit& commit); + bool store(const armem::EntityUpdate& commit); + + + template <typename EventT> + bool + storeImpl(const EventT& event, const std::string& eventName, const std::string& clientID); + }; +} // namespace armarx::nav::mem::client::events diff --git a/source/Navigation/libraries/memory/client/stack_result/Writer.cpp b/source/Navigation/libraries/memory/client/stack_result/Writer.cpp index 820a331e42ee749acbba6c391921c599549ea096..1b0512ca63deb5d21f0c757baa33dc2911c004e2 100644 --- a/source/Navigation/libraries/memory/client/stack_result/Writer.cpp +++ b/source/Navigation/libraries/memory/client/stack_result/Writer.cpp @@ -8,7 +8,7 @@ #include <Navigation/libraries/core/aron/Trajectory.aron.generated.h> #include <Navigation/libraries/core/aron/Twist.aron.generated.h> -namespace armarx::nav::mem::client::stack_result::glob_plan +namespace armarx::nav::mem::client::stack_result { bool Writer::store(const server::StackResult& result, const std::string& clientID) @@ -129,4 +129,4 @@ namespace armarx::nav::mem::client::stack_result::glob_plan }; } -} // namespace armarx::nav::mem::client::stack_result::glob_plan +} // namespace armarx::nav::mem::client::stack_result diff --git a/source/Navigation/libraries/memory/client/stack_result/Writer.h b/source/Navigation/libraries/memory/client/stack_result/Writer.h index afd6c1cc1e79382103f62f147595287e120f0423..ccb28add70071da9c6dbd6f0b202e09432e6bc91 100644 --- a/source/Navigation/libraries/memory/client/stack_result/Writer.h +++ b/source/Navigation/libraries/memory/client/stack_result/Writer.h @@ -28,7 +28,7 @@ #include "Navigation/libraries/server/StackResult.h" #include "Navigation/libraries/trajectory_control/TrajectoryController.h" -namespace armarx::nav::mem::client::stack_result::glob_plan +namespace armarx::nav::mem::client::stack_result { class Writer : virtual public armem::client::util::SimpleWriterBase @@ -67,4 +67,4 @@ namespace armarx::nav::mem::client::stack_result::glob_plan private: }; -} // namespace armarx::nav::mem::client::stack_result::glob_plan +} // namespace armarx::nav::mem::client::stack_result diff --git a/source/Navigation/libraries/server/event_publishing/MemoryPublisher.cpp b/source/Navigation/libraries/server/event_publishing/MemoryPublisher.cpp index 8fc9ca6a213d538c7fc08e3a9d02c6287055e3c6..374ad2f89066e0734a258b459c0f708d315b52f5 100644 --- a/source/Navigation/libraries/server/event_publishing/MemoryPublisher.cpp +++ b/source/Navigation/libraries/server/event_publishing/MemoryPublisher.cpp @@ -4,14 +4,14 @@ void armarx::nav::server::MemoryPublisher::goalReached(const core::GoalReachedEvent& event) { - // TODO: Implement. + // eventsWriter->store(event, clientId); } void armarx::nav::server::MemoryPublisher::waypointReached(const core::WaypointReachedEvent& event) { - // TODO: Implement. + eventsWriter->store(event, clientId); } @@ -19,7 +19,7 @@ void armarx::nav::server::MemoryPublisher::safetyThrottlingTriggered( const core::SafetyThrottlingTriggeredEvent& event) { - // TODO: Implement. + // eventsWriter->store(event, clientId); } @@ -27,21 +27,21 @@ void armarx::nav::server::MemoryPublisher::safetyStopTriggered( const core::SafetyStopTriggeredEvent& event) { - // TODO: Implement. + // eventsWriter->store(event, clientId); } void armarx::nav::server::MemoryPublisher::userAbortTriggered(const core::UserAbortTriggeredEvent& event) { - // TODO: Implement. + // eventsWriter->store(event, clientId); } void armarx::nav::server::MemoryPublisher::internalError(const core::InternalErrorEvent& event) { - // TODO: Implement. + eventsWriter->store(event, clientId); } namespace armarx::nav::server @@ -49,24 +49,30 @@ namespace armarx::nav::server void MemoryPublisher::globalTrajectoryUpdated(const glob_plan::GlobalPlannerResult& res) { - // TODO: Implement. + resultWriter->store(res, clientId); } void MemoryPublisher::localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res) { - // TODO: Implement. + // TODO resultWriter->store(res, clientId); } void MemoryPublisher::trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res) { - // TODO: Implement. + resultWriter->store(res, clientId); } + // TODO event with message or reason (enum) void MemoryPublisher::globalPlanningFailed() { - // TODO: Implement. + // eventsWriter->store(event, clientId); + } + MemoryPublisher::MemoryPublisher(armarx::nav::mem::client::stack_result::Writer* resultWriter, + armarx::nav::mem::client::events::Writer* eventsWriter) : + resultWriter(resultWriter), eventsWriter(eventsWriter) + { } } // namespace armarx::nav::server diff --git a/source/Navigation/libraries/server/event_publishing/MemoryPublisher.h b/source/Navigation/libraries/server/event_publishing/MemoryPublisher.h index 3d1c55603743217b6278c3faf153d430a0ad39e0..5437b15f221cd17e64e1ad4d69af8fbe16d3d603 100644 --- a/source/Navigation/libraries/server/event_publishing/MemoryPublisher.h +++ b/source/Navigation/libraries/server/event_publishing/MemoryPublisher.h @@ -1,5 +1,7 @@ #pragma once +#include "Navigation/libraries/memory/client/events/Writer.h" +#include "Navigation/libraries/memory/client/stack_result/Writer.h" #include <Navigation/libraries/server/event_publishing/EventPublishingInterface.h> namespace armarx::nav::server @@ -9,6 +11,9 @@ namespace armarx::nav::server { public: + MemoryPublisher(armarx::nav::mem::client::stack_result::Writer* resultWriter, + armarx::nav::mem::client::events::Writer* eventsWriter); + void globalTrajectoryUpdated(const glob_plan::GlobalPlannerResult& res) override; void localTrajectoryUpdated(const loc_plan::LocalPlannerResult& res) override; void trajectoryControllerUpdated(const traj_ctrl::TrajectoryControllerResult& res) override; @@ -29,6 +34,12 @@ namespace armarx::nav::server // Non-API. public: ~MemoryPublisher() override = default; + + private: + armarx::nav::mem::client::stack_result::Writer* resultWriter; + armarx::nav::mem::client::events::Writer* eventsWriter; + + std::string clientId = "todo"; }; } // namespace armarx::nav::server diff --git a/source/Navigation/libraries/server/introspection/MemoryIntrospector.cpp b/source/Navigation/libraries/server/introspection/MemoryIntrospector.cpp index 1bdc4fe728d08ba23261416aa6a7ae1a44cb1451..bd59d66aebd37a657b0c5f5ccded7f0187a3c831 100644 --- a/source/Navigation/libraries/server/introspection/MemoryIntrospector.cpp +++ b/source/Navigation/libraries/server/introspection/MemoryIntrospector.cpp @@ -4,9 +4,8 @@ namespace armarx::nav::server { - MemoryIntrospector::MemoryIntrospector( - mem::client::stack_result::glob_plan::Writer& globPlanWriter, - const std::string& clientID) : + MemoryIntrospector::MemoryIntrospector(mem::client::stack_result::Writer& globPlanWriter, + const std::string& clientID) : globPlanWriter(globPlanWriter), clientID(clientID) { } diff --git a/source/Navigation/libraries/server/introspection/MemoryIntrospector.h b/source/Navigation/libraries/server/introspection/MemoryIntrospector.h index d4c6e6677c68663601236626927ae79678b399ea..9d99b9f5d2a584a9d7ab1dbb998178c098dc2b7d 100644 --- a/source/Navigation/libraries/server/introspection/MemoryIntrospector.h +++ b/source/Navigation/libraries/server/introspection/MemoryIntrospector.h @@ -33,7 +33,7 @@ namespace armarx::nav::server class MemoryIntrospector : virtual public IntrospectorInterface { public: - MemoryIntrospector(mem::client::stack_result::glob_plan::Writer& globPlanWriter, + MemoryIntrospector(mem::client::stack_result::Writer& globPlanWriter, const std::string& clientID); ~MemoryIntrospector() override = default; @@ -43,7 +43,7 @@ namespace armarx::nav::server protected: private: - mem::client::stack_result::glob_plan::Writer& globPlanWriter; + mem::client::stack_result::Writer& globPlanWriter; const std::string clientID; }; } // namespace armarx::nav::server