unify rw interfaces

source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp

@@ -5,74 +5,73 @@
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/time/Metronome.h>
 
-#include <RobotAPI/libraries/core/Pose.h>
-
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/query/query_fns.h>
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
-
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/core/Pose.h>
 
 namespace armarx
 {
-    GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(std::string prefix) :
+    GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(
+        std::string prefix) :
         armarx::ComponentPropertyDefinitions(prefix)
     {
     }
 
-    armarx::PropertyDefinitionsPtr GraspProviderExample::createPropertyDefinitions()
+    armarx::PropertyDefinitionsPtr
+    GraspProviderExample::createPropertyDefinitions()
     {
         ARMARX_IMPORTANT << "Prperty defs";
-        armarx::PropertyDefinitionsPtr defs = new GraspProviderExamplePropertyDefinitions(getConfigIdentifier());
+        armarx::PropertyDefinitionsPtr defs =
+            new GraspProviderExamplePropertyDefinitions(getConfigIdentifier());
 
         defs->topic(debugObserver);
 
         defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
 
         return defs;
-
     }
 
-    std::string GraspProviderExample::getDefaultName() const
+    std::string
+    GraspProviderExample::getDefaultName() const
     {
         return "GraspProviderExample";
     }
 
-    GraspProviderExample::GraspProviderExample() : writer(memoryNameSystem()), reader(memoryNameSystem())
+    GraspProviderExample::GraspProviderExample()
     {
-
     }
 
-
-    void GraspProviderExample::onInitComponent()
+    void
+    GraspProviderExample::onInitComponent()
     {
         ARMARX_IMPORTANT << "Init";
     }
 
-
-    void GraspProviderExample::onConnectComponent()
+    void
+    GraspProviderExample::onConnectComponent()
     {
-        writer.connect();
-        reader.connect();
+        writer.connect(memoryNameSystem());
+        reader.connect(memoryNameSystem());
         task = new RunningTask<GraspProviderExample>(this, &GraspProviderExample::run);
         task->start();
     }
 
-
-    void GraspProviderExample::onDisconnectComponent()
+    void
+    GraspProviderExample::onDisconnectComponent()
     {
         task->stop();
     }
 
-
-    void GraspProviderExample::onExitComponent()
+    void
+    GraspProviderExample::onExitComponent()
     {
     }
 
-
-    void GraspProviderExample::run()
+    void
+    GraspProviderExample::run()
     {
         ARMARX_IMPORTANT << "Running example.";
 
@@ -90,9 +89,11 @@ namespace armarx
 
             // initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
             grasping::BimanualGraspCandidate bimanualCandidate = makeDummyBimanualGraspCandidate();
-            bimanualCandidate.groupNr = i; //non-necessary field, but used to commit different candidates
+            bimanualCandidate.groupNr =
+                i; //non-necessary field, but used to commit different candidates
 
-            writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::Now(), "bimanualProvider");
+            writer.commitBimanualGraspCandidate(
+                bimanualCandidate, armem::Time::Now(), "bimanualProvider");
 
 
             //test for writing Seqs, candidates from the same object appear as instances of the same snapshot
@@ -102,7 +103,8 @@ namespace armarx
             candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
 
 
-            writer.commitGraspCandidateSeq(candidatesToWrite, armem::Time::Now(), "candidateProvider");
+            writer.commitGraspCandidateSeq(
+                candidatesToWrite, armem::Time::Now(), "candidateProvider");
 
             // test reader and debug by logging the group number of the candidate
 
@@ -112,15 +114,15 @@ namespace armarx
             {
                 candidates = reader.queryLatestGraspCandidates();
             }
-            catch (armem::error::QueryFailed &e)
+            catch (armem::error::QueryFailed& e)
             {
                 ARMARX_ERROR << e.makeMsg(memoryName);
             }
 
 
-            for (auto &[id, ca] : candidates)
+            for (auto& [id, ca] : candidates)
             {
-                ARMARX_INFO << "candidate with ID " << id << " has group number " <<  ca->groupNr ;
+                ARMARX_INFO << "candidate with ID " << id << " has group number " << ca->groupNr;
             }
 
             std::map<std::string, grasping::BimanualGraspCandidatePtr> bimanualCandidates;
@@ -129,21 +131,23 @@ namespace armarx
             {
                 bimanualCandidates = reader.queryLatestBimanualGraspCandidates();
             }
-            catch (armem::error::QueryFailed &e)
+            catch (armem::error::QueryFailed& e)
             {
                 ARMARX_ERROR << e.makeMsg(memoryName);
             }
 
-            for (auto &[id, ca] : bimanualCandidates)
+            for (auto& [id, ca] : bimanualCandidates)
             {
-                ARMARX_INFO << "bimanual candidate with ID " << id << " has group number " <<  ca->groupNr ;
+                ARMARX_INFO << "bimanual candidate with ID " << id << " has group number "
+                            << ca->groupNr;
             }
 
             m.waitForNextTick();
         }
     }
 
-    grasping::GraspCandidate GraspProviderExample::makeDummyGraspCandidate()
+    grasping::GraspCandidate
+    GraspProviderExample::makeDummyGraspCandidate()
     {
         armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate();
 
@@ -168,9 +172,11 @@ namespace armarx
         return candidate;
     }
 
-    grasping::BimanualGraspCandidate GraspProviderExample::makeDummyBimanualGraspCandidate()
+    grasping::BimanualGraspCandidate
+    GraspProviderExample::makeDummyBimanualGraspCandidate()
     {
-        armarx::grasping::BimanualGraspCandidate bimanualCandidate = armarx::grasping::BimanualGraspCandidate();
+        armarx::grasping::BimanualGraspCandidate bimanualCandidate =
+            armarx::grasping::BimanualGraspCandidate();
 
         bimanualCandidate.approachVectorLeft = Vector3BasePtr(toIce(zeroVector));
         bimanualCandidate.approachVectorRight = Vector3BasePtr(toIce(zeroVector));
@@ -185,4 +191,4 @@ namespace armarx
         return bimanualCandidate;
     }
 
-}
+} // namespace armarx

source/RobotAPI/components/units/GraspCandidateObserver.cpp

@@ -24,45 +24,51 @@
 #include "GraspCandidateObserver.h"
 
 //#include <ArmarXCore/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
 #include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
 #include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
 #include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+
 #include <RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
 #include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 #define TCP_POSE_CHANNEL "TCPPose"
 #define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
 using namespace armarx;
 using namespace armarx::grasping;
 
-GraspCandidateObserver::GraspCandidateObserver() : graspCandidateWriter(memoryNameSystem())
+GraspCandidateObserver::GraspCandidateObserver()
 {
 }
 
-void GraspCandidateObserver::onInitObserver()
+void
+GraspCandidateObserver::onInitObserver()
 {
     usingTopic(getProperty<std::string>("GraspCandidatesTopicName").getValue());
     offeringTopic(getProperty<std::string>("ConfigTopicName").getValue());
-
-
 }
 
-void GraspCandidateObserver::onConnectObserver()
+void
+GraspCandidateObserver::onConnectObserver()
 {
-    configTopic = getTopic<GraspCandidateProviderInterfacePrx>(getProperty<std::string>("ConfigTopicName").getValue());
-    graspCandidateWriter.connect();
+    configTopic = getTopic<GraspCandidateProviderInterfacePrx>(
+        getProperty<std::string>("ConfigTopicName").getValue());
+    graspCandidateWriter.connect(memoryNameSystem());
 }
 
-PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
+PropertyDefinitionsPtr
+GraspCandidateObserver::createPropertyDefinitions()
 {
-    return PropertyDefinitionsPtr(new GraspCandidateObserverPropertyDefinitions(
-                                      getConfigIdentifier()));
+    return PropertyDefinitionsPtr(
+        new GraspCandidateObserverPropertyDefinitions(getConfigIdentifier()));
 }
 
-bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter, const std::string& providerName, const GraspCandidatePtr& candidate)
+bool
+GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter,
+                                      const std::string& providerName,
+                                      const GraspCandidatePtr& candidate)
 {
     if (filter->providerName != "*" && filter->providerName != providerName)
     {
@@ -72,11 +78,13 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
     {
         return false;
     }
-    if (filter->approach != AnyApproach && (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
+    if (filter->approach != AnyApproach &&
+        (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
     {
         return false;
     }
-    if (filter->preshape != AnyAperture && (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
+    if (filter->preshape != AnyAperture &&
+        (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
     {
         return false;
     }
@@ -87,7 +95,8 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
     return true;
 }
 
-std::string GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
+std::string
+GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
 {
     switch (type)
     {
@@ -102,7 +111,8 @@ std::string GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
     }
 }
 
-void GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
+void
+GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
 {
     if (updateCounters.count(providerName) == 0)
     {
@@ -118,26 +128,40 @@ void GraspCandidateObserver::handleProviderUpdate(const std::string& providerNam
     {
         offerChannel(providerName, "Channel of " + providerName);
     }
-    offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters[providerName]), "Counter that increases for each update");
-    offerOrUpdateDataField(providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
+    offerOrUpdateDataField(providerName,
+                           "updateCounter",
+                           Variant(updateCounters[providerName]),
+                           "Counter that increases for each update");
+    offerOrUpdateDataField(
+        providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
 }
 
-void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::reportGraspCandidates(const std::string& providerName,
+                                              const GraspCandidateSeq& candidates,
+                                              const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     this->candidates[providerName] = candidates;
-    graspCandidateWriter.commitGraspCandidateSeq(candidates, armarx::armem::Time::Now(), providerName);
+    graspCandidateWriter.commitGraspCandidateSeq(
+        candidates, armarx::armem::Time::Now(), providerName);
     handleProviderUpdate(providerName, candidates.size());
 }
 
-void GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName, const BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName,
+                                                      const BimanualGraspCandidateSeq& candidates,
+                                                      const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     this->bimanualCandidates[providerName] = candidates;
     handleProviderUpdate(providerName, candidates.size());
 }
 
-void GraspCandidateObserver::reportProviderInfo(const std::string& providerName, const ProviderInfoPtr& info, const Ice::Current&)
+void
+GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
+                                           const ProviderInfoPtr& info,
+                                           const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     providers[providerName] = info;
@@ -154,36 +178,37 @@ void GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
     offerOrUpdateDataField(providerName, "objectType", ObjectTypeToString(info->objectType), "");
 }
 
-InfoMap GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
+InfoMap
+GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     return providers;
 }
 
-StringSeq GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
+StringSeq
+GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     return getAvailableProviderNames();
 }
 
-
-
-ProviderInfoPtr GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
+ProviderInfoPtr
+GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     checkHasProvider(providerName);
     return providers[providerName];
 }
 
-bool GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
+bool
+GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
 {
     std::unique_lock lock(dataMutex);
     return hasProvider(providerName);
 }
 
-
-
-GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getAllCandidates(const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     GraspCandidateSeq all;
@@ -194,11 +219,16 @@ GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
     return all;
 }
 
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName, const Ice::Current& c)
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName,
+                                                const Ice::Current& c)
 {
     return getCandidatesByProviders(Ice::StringSeq{providerName});
 }
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByProviders(const Ice::StringSeq& providerNames, const Ice::Current& c)
+
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByProviders(const Ice::StringSeq& providerNames,
+                                                 const Ice::Current& c)
 {
     std::unique_lock lock(dataMutex);
     GraspCandidateSeq all;
@@ -213,7 +243,9 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByProviders(const Ice::St
     return all;
 }
 
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter, const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter,
+                                              const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     GraspCandidateSeq matching;
@@ -230,20 +262,26 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateF
     return matching;
 }
 
-Ice::Int GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
+Ice::Int
+GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName,
+                                                   const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     checkHasProvider(providerName);
     return updateCounters[providerName];
 }
 
-IntMap GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
+IntMap
+GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
 {
     std::unique_lock lock(dataMutex);
     return updateCounters;
 }
 
-bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&)
+bool
+GraspCandidateObserver::setProviderConfig(const std::string& providerName,
+                                          const StringVariantBaseMap& config,
+                                          const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     if (providers.count(providerName) == 0)
@@ -254,19 +292,23 @@ bool GraspCandidateObserver::setProviderConfig(const std::string& providerName,
     return true;
 }
 
-void GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates,
+                                              const Ice::Current&)
 {
     std::unique_lock lock(selectedCandidatesMutex);
     selectedCandidates = candidates;
 }
 
-GraspCandidateSeq GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
 {
     std::unique_lock lock(selectedCandidatesMutex);
     return selectedCandidates;
 }
 
-BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
+BimanualGraspCandidateSeq
+GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     BimanualGraspCandidateSeq all;
@@ -277,19 +319,25 @@ BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const
     return all;
 }
 
-void GraspCandidateObserver::setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::setSelectedBimanualCandidates(
+    const grasping::BimanualGraspCandidateSeq& candidates,
+    const Ice::Current&)
 {
     std::unique_lock lock(selectedCandidatesMutex);
     selectedBimanualCandidates = candidates;
 }
 
-BimanualGraspCandidateSeq GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
+BimanualGraspCandidateSeq
+GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
 {
     std::unique_lock lock(selectedCandidatesMutex);
     return selectedBimanualCandidates;
 }
 
-void GraspCandidateObserver::clearCandidatesByProvider(const std::string& providerName, const Ice::Current&)
+void
+GraspCandidateObserver::clearCandidatesByProvider(const std::string& providerName,
+                                                  const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
 
@@ -299,18 +347,24 @@ void GraspCandidateObserver::clearCandidatesByProvider(const std::string& provid
     }
 }
 
-bool GraspCandidateObserver::hasProvider(const std::string& providerName)
+bool
+GraspCandidateObserver::hasProvider(const std::string& providerName)
 {
     return providers.count(providerName) > 0;
 }
-void GraspCandidateObserver::checkHasProvider(const std::string& providerName)
+
+void
+GraspCandidateObserver::checkHasProvider(const std::string& providerName)
 {
     if (!hasProvider(providerName))
     {
-        throw LocalException("Unknown provider name '") << providerName << "'. Available providers: " << getAvailableProviderNames();
+        throw LocalException("Unknown provider name '")
+            << providerName << "'. Available providers: " << getAvailableProviderNames();
     }
 }
-StringSeq GraspCandidateObserver::getAvailableProviderNames()
+
+StringSeq
+GraspCandidateObserver::getAvailableProviderNames()
 {
     StringSeq names;
     for (const auto& pair : providers)

source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h

@@ -13,7 +13,7 @@ namespace armarx::armem
     class GraspCandidateWriter
     {
     public:
-        GraspCandidateWriter();
+        GraspCandidateWriter() = default;
 
         void connect(armem::client::MemoryNameSystem& memoryNameSystem);