From 3143ce7e6161bc9dfae4cb200e7e0034013d052f Mon Sep 17 00:00:00 2001
From: alissa <alissamueller@outlook.de>
Date: Fri, 9 Jul 2021 16:33:56 +0200
Subject: [PATCH] GraspReader now uses FunctionalVisitor
---
.../GraspProviderExample.cpp | 4 +-
.../GraspingUtility/GraspCandidateReader.cpp | 145 +++++++++++-------
.../GraspingUtility/GraspCandidateReader.h | 8 +-
.../workingmemory/visitor/FunctionalVisitor.h | 12 +-
4 files changed, 104 insertions(+), 65 deletions(-)
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
index 31b7f565b..300be5e5c 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
@@ -79,7 +79,7 @@ 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
@@ -104,7 +104,7 @@ namespace armarx
bimanualCandidate.inwardsVectorRight = Vector3BasePtr(toIce(Eigen::Vector3f()));
writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::now());
- candidates = reader.queryLatest();
+ grasping::GraspCandidateSeq candidates = reader.queryLatestGraspCandidates();
for (auto ca : candidates)
{
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index 9fae422a7..8c707b853 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -4,6 +4,7 @@
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
namespace armarx::armem
{
@@ -32,87 +33,127 @@ namespace armarx::armem
}
- armarx::grasping::GraspCandidateSeq asGraspCandidates(const std::map<std::string, wm::Entity>& entities)
+
+
+ armarx::grasping::GraspCandidate asGraspCandidate(const armem::wm::EntityInstance& instance)
{
- armarx::grasping::GraspCandidateSeq outV;
+ armarx::grasping::GraspCandidate candidate;
- if (entities.empty())
- {
- ARMARX_WARNING << "No entities!";
- }
+ grasping::arondto::GraspCandidate aronTransform;
+ aronTransform.fromAron(instance.data());
- const auto convert = [](const armarx::grasping::arondto::GraspCandidate & aronGraspCandidate,
- const wm::EntityInstance & ei) -> armarx::grasping::GraspCandidate
- {
- armarx::grasping::GraspCandidate graspCandidate;
- fromAron(aronGraspCandidate, graspCandidate);
+ fromAron(aronTransform, candidate);
- //const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(ei.data()->getElement("scan"));
+ return candidate;
+ }
- //ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+ armarx::grasping::BimanualGraspCandidate asBimanualGraspCandidate(const armem::wm::EntityInstance& instance)
+ {
+ armarx::grasping::BimanualGraspCandidate candidate;
- //graspCandidate.data = fromAron<GraspCandidate>(ndArrayNavigator);
+ grasping::arondto::BimanualGraspCandidate aronTransform;
+ aronTransform.fromAron(instance.data());
+ fromAron(aronTransform, candidate);
- return graspCandidate;
+ return candidate;
+ }
- };
- ARMARX_INFO << "Before for loop";
- // loop over all entities and their snapshots
- for (const auto &[s, entity] : entities)
+ grasping::GraspCandidateSeq GraspCandidateReader::queryLatestGraspCandidates(std::string provider) const
+ {
+
+ armem::client::query::Builder qb;
+
+ ARMARX_INFO << "Query for memory name: " << properties.memoryName;
+
+
+ if (provider != "")
+ {
+ qb
+ .coreSegments().withName(properties.graspCandidateMemoryName)
+ .providerSegments().withName(provider)
+ .entities().all()
+ .snapshots().latest();
+ }
+ else
+ {
+ qb
+ .coreSegments().withName(properties.graspCandidateMemoryName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest();
+ }
+
+ const armem::client::QueryResult qResult =
+ memoryReader.query(qb.buildQueryInput());
+
+ if (!qResult.success)
{
- if (entity.empty())
- {
- ARMARX_WARNING << "Empty history for " << s;
- }
-
- ARMARX_INFO << "History size: " << entity.size();
-
- for (const auto &[ss, entitySnapshot] : entity)
- {
- for (const auto& entityInstance : entitySnapshot.instances())
- {
- const auto o = tryCast<armarx::grasping::arondto::GraspCandidate>(entityInstance);
-
- if (o)
- {
- armarx::grasping::GraspCandidate candidate = convert(*o, entityInstance);
- armarx::grasping::GraspCandidatePtr ptr = armarx::grasping::GraspCandidatePtr(&candidate);
- outV.push_back(ptr);
- }
- }
- }
+ // todo catch in provider
+ throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
}
- return outV;
+
+ armarx::grasping::GraspCandidateSeq candidates;
+
+ armem::wm::FunctionalVisitor visitor;
+ visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
+ {
+ candidates.push_back(new grasping::GraspCandidate(asGraspCandidate(instance)));
+ return true;
+ };
+
+ visitor.applyTo(qResult.memory);
+
+
+ return candidates;
}
- grasping::GraspCandidateSeq GraspCandidateReader::queryLatest() const
+ grasping::BimanualGraspCandidateSeq GraspCandidateReader::queryLatestBimanualGraspCandidates(std::string provider) const
{
armem::client::query::Builder qb;
ARMARX_INFO << "Query for memory name: " << properties.memoryName;
- // clang-format off
- qb
- .coreSegments().withName(properties.graspCandidateMemoryName)
- .providerSegments().all()
- .entities().all()
- .snapshots().all();
+ if (provider != "")
+ {
+ qb
+ .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments().withName(provider)
+ .entities().all()
+ .snapshots().latest();
+ }
+ else
+ {
+ qb
+ .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest();
+ }
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
if (!qResult.success)
{
+ // todo catch in provider
throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
}
- const auto& entities =
- qResult.memory.getCoreSegment(properties.graspCandidateMemoryName).getProviderSegment("Example")
- .entities();
- const auto candidates = asGraspCandidates(entities);
+
+ armarx::grasping::BimanualGraspCandidateSeq candidates;
+
+ armem::wm::FunctionalVisitor visitor;
+ visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
+ {
+ candidates.push_back(new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance)));
+ return true;
+ };
+
+ visitor.applyTo(qResult.memory);
+
return candidates;
}
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
index d0a66f036..5204b6dd8 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -14,15 +14,13 @@ namespace armarx::armem
void connect();
+ grasping::GraspCandidateSeq queryLatestGraspCandidates(std::string provider = "") const;
-
- grasping::GraspCandidateSeq queryLatest() const;
+ grasping::BimanualGraspCandidateSeq queryLatestBimanualGraspCandidates(std::string provider = "") const;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- //client::query::Builder buildQuery(const Query& query) const ;
-
private:
armem::client::Reader memoryReader;
@@ -30,7 +28,7 @@ namespace armarx::armem
// Properties
struct Properties
{
- std::string memoryName = "Grasp";
+ std::string memoryName = "Grasp";
std::string graspCandidateMemoryName = "GraspCandidate";
std::string bimanualGraspCandidateMemoryName = "BimanualGraspCandidate";
} properties;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
index 1c1b46e13..34390a182 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
@@ -53,28 +53,28 @@ namespace armarx::armem::wm
bool visitEnter(const Memory& memory) override
{
- return memoryFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
+ return memoryConstFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
}
bool visitEnter(const CoreSegment& coreSegment) override
{
- return coreSegmentFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ return coreSegmentConstFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
}
bool visitEnter(const ProviderSegment& providerSegment) override
{
- return providerSegmentFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ return providerSegmentConstFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
}
bool visitEnter(const Entity& entity) override
{
- return entityFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
+ return entityConstFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
}
bool visitEnter(const EntitySnapshot& snapshot) override
{
- return memoryFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
+ return snapshotConstFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
}
bool visit(const EntityInstance& instance) override
{
- return instanceFn ? instanceConstFn(instance) : Visitor::visit(instance);
+ return instanceConstFn ? instanceConstFn(instance) : Visitor::visit(instance);
}
--
GitLab