From 6dd7d72c406920bfcc52fbc59330159e236c602b Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Wed, 18 Oct 2023 12:46:02 +0200
Subject: [PATCH] unify rw interfaces
---
.../GraspProviderExample.cpp | 82 +++++-----
.../units/GraspCandidateObserver.cpp | 150 ++++++++++++------
.../GraspingUtility/GraspCandidateWriter.h | 2 +-
3 files changed, 147 insertions(+), 87 deletions(-)
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
index e4e5b22aa..605911657 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
+++ b/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
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index 41ffefe98..b434d1e39 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/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)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
index 9b823a635..526f16b0a 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
+++ b/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);
--
GitLab