From fd3ebe7a857fabb2d9b9b4cf83b699c225ae331d Mon Sep 17 00:00:00 2001 From: Rainer Kartmann <rainer.kartmann@kit.edu> Date: Mon, 19 Apr 2021 11:31:03 +0200 Subject: [PATCH] Make ObjectPoseObserver a memory server. --- .../ObjectPoseObserver/CMakeLists.txt | 6 +++++- .../ObjectPoseObserver/ObjectPoseObserver.cpp | 2 ++ .../ObjectPoseObserver/ObjectPoseObserver.h | 5 ++++- .../ObjectPoseObserver/detail/Data.cpp | 17 +++++++++++++++++ .../components/ObjectPoseObserver/detail/Data.h | 16 ++++++++++++++++ .../objectpose/ObjectPoseObserverInterface.ice | 6 +++--- .../libraries/ArmarXObjects/aron/ObjectPose.xml | 2 +- 7 files changed, 48 insertions(+), 6 deletions(-) diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt index 74da78adf..7451e6f55 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt +++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt @@ -2,11 +2,15 @@ armarx_component_set_name("ObjectPoseObserver") set(COMPONENT_LIBS + # ArmarXCore ArmarXCore ArmarXCoreInterfaces + # ArmarXGui ArmarXGuiComponentPlugins + # RobotAPI RobotAPIArmarXObjects RobotAPIComponentPlugins - ArViz + ArViz armem + # This project ${PROJECT_NAME}Interfaces ) diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp index 71ec464eb..2cadd797b 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp @@ -67,6 +67,8 @@ namespace armarx robotHead.setTag(getName()); visu.setTag(getName()); + data.init(); + usingTopicFromProperty("ObjectPoseTopicName"); } diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h index 22956a0d9..3c2844d23 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h @@ -31,12 +31,14 @@ #include <RobotAPI/interface/objectpose/ObjectPoseObserverInterface.h> #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h> #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h> +#include <RobotAPI/libraries/armem/server/ComponentPlugin.h> #include <RobotAPI/components/ObjectPoseObserver/detail/Data.h> #include <RobotAPI/components/ObjectPoseObserver/detail/Decay.h> #include <RobotAPI/components/ObjectPoseObserver/detail/Visu.h> #include <RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h> + #define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent @@ -57,6 +59,7 @@ namespace armarx class ObjectPoseObserver : virtual public Observer , virtual public armarx::objpose::ObjectPoseObserverInterface + , virtual public armarx::armem::server::ComponentPluginUser , virtual public armarx::RobotStateComponentPluginUser , virtual public armarx::LightweightRemoteGuiComponentPluginUser , virtual public armarx::ArVizComponentPluginUser @@ -145,7 +148,7 @@ namespace armarx DebugObserverInterfacePrx debugObserver; - objpose::observer::Data data; + objpose::observer::Data data { memory }; std::mutex dataMutex; objpose::observer::RobotHeadMovement robotHead; diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp index be89f745d..90bd36463 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp @@ -4,6 +4,7 @@ #include <RobotAPI/libraries/core/FramedPose.h> #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h> #include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h> +#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h> #include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h> #include <ArmarXCore/core/time/TimeUtil.h> @@ -14,11 +15,27 @@ namespace armarx::objpose::observer { + const std::string Data::defaultMemoryName = "Object"; + + Data::Data(armem::Memory& memory) : memory(memory) + { + memory.name() = defaultMemoryName; + } + void Data::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix) { + defs->optional(memory.name(), "mem.MemoryName", "Name of this memory server."); + defs->optional(maxHistorySize, "mem.MaxHistorySize", "Maximal size of object poses history (-1 for infinite)."); + decay.defineProperties(defs, prefix + "decay."); } + void Data::init() + { + coreSegment = &memory.addCoreSegment("ObjectPose", objpose::arondto::ObjectPose::toInitialAronType()); + coreSegment->setMaxHistorySize(maxHistorySize); + } + ObjectPoseSeq Data::getObjectPoses(IceUtil::Time now) { bool synchronized = false; diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h index b6cf8eb08..edf489ffd 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h +++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h @@ -16,6 +16,8 @@ #include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h> #include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h> +#include <RobotAPI/libraries/armem/core/Memory.h> + #include "Decay.h" @@ -30,8 +32,18 @@ namespace armarx::objpose::observer { public: + static const std::string defaultMemoryName; + + + public: + + Data(armem::Memory& memory); + + void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = ""); + void init(); + ObjectPoseSeq getObjectPoses(IceUtil::Time now); ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now); @@ -81,6 +93,10 @@ namespace armarx::objpose::observer ProviderInfoMap providers; + armem::Memory& memory; + armem::CoreSegment* coreSegment = nullptr; + long maxHistorySize = -1; + std::map<std::string, ObjectPoseSeq> objectPoses; std::map<std::pair<std::string, ObjectID>, ObjectAttachmentInfo> attachments; diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice index 32950130f..6fe63e5f9 100644 --- a/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice +++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice @@ -138,9 +138,9 @@ module armarx }; interface ObjectPoseObserverInterface extends - ObserverInterface, - ObjectPoseTopic, - armem::server::MemoryInterface + ObserverInterface + , ObjectPoseTopic + , armem::server::MemoryInterface { // Object poses diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml index 51106bb79..ac04344c8 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml +++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml @@ -87,7 +87,7 @@ <ObjectChild key='localOOBBValid'> <bool /> - </ObjectChild> + </ObjectChild> <ObjectChild key='localOOBB'> <simox::arondto::OrientedBox /> </ObjectChild> -- GitLab