From 5958c6a07088ab768d4fcbc3635f1351c39df266 Mon Sep 17 00:00:00 2001
From: alissa <alissamueller@outlook.de>
Date: Tue, 29 Jun 2021 18:04:45 +0200
Subject: [PATCH] Adding functionality to GraspReader (WIP)
---
.../GraspProviderExample.cpp | 14 +++-
.../GraspProviderExample.h | 4 +-
.../GraspingUtility/GraspCandidateReader.cpp | 80 +++++++------------
.../GraspingUtility/GraspCandidateReader.h | 47 +----------
4 files changed, 46 insertions(+), 99 deletions(-)
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
index 347069f94..31b7f565b 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
@@ -40,7 +40,7 @@ namespace armarx
return "GraspProviderExample";
}
- GraspProviderExample::GraspProviderExample() : writer(*this)
+ GraspProviderExample::GraspProviderExample() : writer(memoryNameSystem), reader(memoryNameSystem)
{
}
@@ -55,7 +55,7 @@ namespace armarx
void GraspProviderExample::onConnectComponent()
{
writer.connect();
-
+ reader.connect();
task = new RunningTask<GraspProviderExample>(this, &GraspProviderExample::run);
task->start();
}
@@ -79,10 +79,11 @@ namespace armarx
CycleUtil c(1000);
int i = 0;
-
+ grasping::GraspCandidateSeq candidates;
while (!task->isStopped() && i++ < 100)
{
// initialize all necessary fields of a grasp candidate and use writer to commit it to memory
+
armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate();
candidate.groupNr = i; //non-necessary field, but used to commit different candidates
candidate.approachVector = Vector3BasePtr(toIce(Eigen::Vector3f()));
@@ -90,7 +91,6 @@ namespace armarx
candidate.providerName = "Example";
candidate.robotPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
writer.commitGraspCandidate(candidate, armem::Time::now());
-
// initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
armarx::grasping::BimanualGraspCandidate bimanualCandidate = armarx::grasping::BimanualGraspCandidate();
bimanualCandidate.groupNr = i; //non-necessary field, but used to commit different candidates
@@ -104,6 +104,12 @@ namespace armarx
bimanualCandidate.inwardsVectorRight = Vector3BasePtr(toIce(Eigen::Vector3f()));
writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::now());
+ candidates = reader.queryLatest();
+
+ for (auto ca : candidates)
+ {
+ ARMARX_INFO << ca->groupNr;
+ }
c.waitForCycleDuration();
}
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
index 119918274..fea966e87 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
@@ -1,6 +1,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
+#include <RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
@@ -22,7 +23,7 @@ namespace armarx
class GraspProviderExample :
virtual public armarx::Component,
- virtual public armarx::armem::ClientWriterComponentPluginUser
+ virtual public armarx::armem::ClientComponentPluginUser
{
public:
@@ -50,5 +51,6 @@ namespace armarx
std::string memoryName = "Grasp";
armarx::armem::GraspCandidateWriter writer;
+ armarx::armem::GraspCandidateReader reader;
};
}
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index a726ed708..9fae422a7 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -2,12 +2,13 @@
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error/mns.h>
#include <RobotAPI/libraries/armem/util/util.h>
namespace armarx::armem
{
- GraspCandidateReader::GraspCandidateReader(ClientReaderComponentPluginUser& memoryClient)
- : memoryClient(memoryClient)
+ GraspCandidateReader::GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem)
+ : memoryNameSystem(memoryNameSystem)
{
}
@@ -16,17 +17,19 @@ namespace armarx::armem
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '"
<< properties.memoryName << "' ...";
- auto result = memoryClient.useMemory(properties.memoryName);
- if (not result.success)
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "GraspCandidateReader: Connected to memory '"
+ << properties.memoryName;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
return;
}
- ARMARX_IMPORTANT << "GraspCandidateReader: Connected to memory '"
- << properties.memoryName;
- memoryReader.setReadingMemory(result.proxy);
}
armarx::grasping::GraspCandidateSeq asGraspCandidates(const std::map<std::string, wm::Entity>& entities)
@@ -54,7 +57,7 @@ namespace armarx::armem
return graspCandidate;
};
-
+ ARMARX_INFO << "Before for loop";
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
@@ -63,7 +66,7 @@ namespace armarx::armem
ARMARX_WARNING << "Empty history for " << s;
}
- ARMARX_DEBUG << "History size: " << entity.size();
+ ARMARX_INFO << "History size: " << entity.size();
for (const auto &[ss, entitySnapshot] : entity)
{
@@ -84,41 +87,37 @@ namespace armarx::armem
return outV;
}
-
- GraspCandidateReader::GraspCandidateResult GraspCandidateReader::queryData(
- const GraspCandidateReader::Query& query) const
+ grasping::GraspCandidateSeq GraspCandidateReader::queryLatest() const
{
- const auto qb = buildQuery(query);
+ armem::client::query::Builder qb;
+
+ ARMARX_INFO << "Query for memory name: " << properties.memoryName;
+
- ARMARX_IMPORTANT << "[GraspCandidateReader] query ... ";
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.graspCandidateMemoryName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().all();
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
- ARMARX_DEBUG << "[GraspCandidateReader] result: " << qResult;
-
- if (not qResult.success)
+ if (!qResult.success)
{
- ARMARX_WARNING << "Failed to query data from memory: "
- << qResult.errorMessage;
- return {.candidate = {},
- .status = GraspCandidateResult::Status::Error,
- .errorMessage = qResult.errorMessage};
+ throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
}
- // now create result from memory
const auto& entities =
- qResult.memory.getCoreSegment(properties.graspCandidateMemoryName)
- .getProviderSegment(query.providerName)
+ qResult.memory.getCoreSegment(properties.graspCandidateMemoryName).getProviderSegment("Example")
.entities();
-
const auto candidates = asGraspCandidates(entities);
- return {.candidate = candidates,
- .status = GraspCandidateResult::Status::Success,
- .errorMessage = ""};
+ return candidates;
}
+
void GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "GraspCandidateReader: registerPropertyDefinitions";
@@ -133,25 +132,4 @@ namespace armarx::armem
}
- client::query::Builder GraspCandidateReader::buildQuery(const GraspCandidateReader::Query& query) const
- {
- armem::client::query::Builder qb;
-
- ARMARX_INFO << "Query for provider: " << query.providerName
- << " memory name: " << properties.graspCandidateMemoryName;
-
-
- // clang-format off
- qb
- .coreSegments().withName(properties.graspCandidateMemoryName)
- .providerSegments().withName(query.providerName)
- .entities().all()
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
- // clang-format on
-
-
- return qb;
- }
-
-
}
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
index 2c7d40a9b..d0a66f036 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -6,61 +6,22 @@
namespace armarx::armem
{
- struct TimeRange
- {
- Time min;
- Time max;
- };
class GraspCandidateReader
{
public:
- GraspCandidateReader(ClientReaderComponentPluginUser& memoryClient);
+ GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem);
void connect();
- struct Query
- {
- std::string providerName;
-
- TimeRange timeRange;
-
- };
-
- struct GraspCandidateResult
- {
- armarx::grasping::GraspCandidateSeq candidate;
-
- enum Status
- {
- Error,
- Success
- } status;
-
- std::string errorMessage;
-
- };
-
- struct BimanualGraspCandidateResult
- {
- armarx::grasping::BimanualGraspCandidateSeq candidate;
-
- enum Status
- {
- Error,
- Success
- } status;
-
- std::string errorMessage;
- };
- GraspCandidateResult queryData(const Query& query) const;
+ grasping::GraspCandidateSeq queryLatest() const;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- client::query::Builder buildQuery(const Query& query) const ;
+ //client::query::Builder buildQuery(const Query& query) const ;
private:
@@ -77,7 +38,7 @@ namespace armarx::armem
const std::string propertyPrefix = "mem.grasping.";
- armem::ClientReaderComponentPluginUser& memoryClient;
+ armem::client::MemoryNameSystem& memoryNameSystem;
};
--
GitLab