diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt index 74da78adf1af125b3ccab6669b1b47a2a9d0d9e8..7451e6f55845a9d94db43fb789d63a68361a1f1c 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 71ec464eb16beb8ac72ddf32711a16a8c4a2e1e2..2cadd797b59ca0454c7271bcc6a5826c942b862d 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 22956a0d9e929eab8518ab5dff2c17af0c84cb0e..3c2844d23fc621727f25e993289fdb1410afddd4 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 be89f745d995ff82431d4b2fb285181a607730dc..90bd36463bb873eab94a6a790e0ec88a45e7fd46 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 b6cf8eb087b13f426c354880a8fef7b03c337258..edf489ffd583dc811a3b4bc83b91927c5cd09c28 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 32950130fb827b3958c5f56cff2e61d20a7187af..6fe63e5f97c3cdca0a17611f4dc7f5948c3564cd 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 51106bb79aa2ba3981231a271109fc9b3680273a..ac04344c87e0e17e8854b2e05a669370951567ca 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>