Added new Memory to GraspCandidateObserver

source/RobotAPI/components/units/CMakeLists.txt

@@ -6,6 +6,7 @@ set(LIBS
     RobotAPICore
     ArmarXCoreObservers
     ArmarXCoreEigen3Variants
+    GraspingUtility
 )
 
 set(LIB_HEADERS

source/RobotAPI/components/units/GraspCandidateObserver.cpp

@@ -38,7 +38,7 @@
 using namespace armarx;
 using namespace armarx::grasping;
 
-GraspCandidateObserver::GraspCandidateObserver()
+GraspCandidateObserver::GraspCandidateObserver() : graspCandidateWriter(memoryNameSystem())
 {
 }
 
@@ -53,6 +53,7 @@ void GraspCandidateObserver::onInitObserver()
 void GraspCandidateObserver::onConnectObserver()
 {
     configTopic = getTopic<GraspCandidateProviderInterfacePrx>(getProperty<std::string>("ConfigTopicName").getValue());
+    graspCandidateWriter.connect();
 }
 
 PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
@@ -125,6 +126,7 @@ void GraspCandidateObserver::reportGraspCandidates(const std::string& providerNa
 {
     std::unique_lock lock(dataMutex);
     this->candidates[providerName] = candidates;
+    graspCandidateWriter.commitGraspCandidateSeq(candidates, armarx::armem::Time::now(), providerName);
     handleProviderUpdate(providerName, candidates.size());
 }
 

source/RobotAPI/components/units/GraspCandidateObserver.h

@@ -25,6 +25,8 @@
 
 #include <ArmarXCore/observers/Observer.h>
 #include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h>
 
 #include <mutex>
 
@@ -52,6 +54,7 @@ namespace armarx
      */
     class GraspCandidateObserver :
         virtual public Observer,
+        virtual public armarx::armem::ClientPluginUser,
         virtual public grasping::GraspCandidateObserverInterface
     {
     public:
@@ -124,7 +127,7 @@ namespace armarx
         grasping::GraspCandidateSeq selectedCandidates;
 
         grasping::BimanualGraspCandidateSeq selectedBimanualCandidates;
-
+        armarx::armem::GraspCandidateWriter graspCandidateWriter;
 
         void handleProviderUpdate(const std::string& providerName, int candidateCount);
     };