Skip to content
Snippets Groups Projects
Commit 5d37e452 authored by Christoph Pohl's avatar Christoph Pohl Committed by ARMAR-DE
Browse files

Add queryGraspCandidatesNewerThan method for GraspCandidateReader 

Signed-off-by: ARMAR-DE <>
parent 991c106c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+ 139
− 82
View file @ 5d37e452
#include "GraspCandidateReader.h"
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#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>
#include <RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/libraries/armem/util/util.h>
namespace armarx::armem
{
GraspCandidateReader::GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem)
: memoryNameSystem(memoryNameSystem)
GraspCandidateReader::GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
{
}
void GraspCandidateReader::connect(bool use)
void
GraspCandidateReader::connect(bool use)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '"
<< properties.memoryName << "' ...";
ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '" << properties.memoryName
<< "' ...";
try
{
memoryReader = use ? memoryNameSystem.useReader(properties.memoryName)
......@@ -34,8 +35,8 @@ namespace armarx::armem
}
}
armarx::grasping::GraspCandidate asGraspCandidate(const armem::wm::EntityInstance& instance)
armarx::grasping::GraspCandidate
asGraspCandidate(const armem::wm::EntityInstance& instance)
{
armarx::grasping::GraspCandidate candidate;
......@@ -48,7 +49,8 @@ namespace armarx::armem
return candidate;
}
armarx::grasping::BimanualGraspCandidate asBimanualGraspCandidate(const armem::wm::EntityInstance& instance)
armarx::grasping::BimanualGraspCandidate
asBimanualGraspCandidate(const armem::wm::EntityInstance& instance)
{
armarx::grasping::BimanualGraspCandidate candidate;
......@@ -60,8 +62,8 @@ namespace armarx::armem
return candidate;
}
grasping::GraspCandidatePtr GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
grasping::GraspCandidatePtr
GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
{
auto dict = queryGraspCandidateInstancesByID({id});
if (auto it = dict.find(id.str()); it != dict.end())
......@@ -74,8 +76,8 @@ namespace armarx::armem
}
}
grasping::GraspCandidateDict GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
grasping::GraspCandidateDict
GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
{
armem::client::query::Builder qb;
......@@ -83,8 +85,7 @@ namespace armarx::armem
qb.multipleEntitySnapshots(ids);
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
if (!qResult.success)
{
......@@ -104,9 +105,8 @@ namespace armarx::armem
return candidates;
}
grasping::BimanualGraspCandidatePtr GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(
const armem::MemoryID& id) const
grasping::BimanualGraspCandidatePtr
GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(const armem::MemoryID& id) const
{
armem::client::query::Builder qb;
......@@ -114,8 +114,7 @@ namespace armarx::armem
qb.singleEntitySnapshot(id.getEntitySnapshotID());
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
if (!qResult.success)
{
......@@ -127,11 +126,12 @@ namespace armarx::armem
armarx::grasping::BimanualGraspCandidatePtr candidate;
armem::wm::FunctionalVisitor visitor;
visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const & instance)
visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const& instance)
{
if (instance.id() == id)
{
candidate = new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
candidate =
new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
}
return true;
};
......@@ -141,46 +141,52 @@ namespace armarx::armem
return candidate;
}
grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidateEntity(
const std::string& provider, const std::string& entity) const
grasping::GraspCandidateDict
GraspCandidateReader::queryLatestGraspCandidateEntity(const std::string& provider,
const std::string& entity) const
{
armem::client::query::Builder qb;
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
qb
.coreSegments().withName(properties.graspCandidateMemoryName)
.providerSegments().withName(provider)
.entities().withName(entity)
.snapshots().latest();
qb.coreSegments()
.withName(properties.graspCandidateMemoryName)
.providerSegments()
.withName(provider)
.entities()
.withName(entity)
.snapshots()
.latest();
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getGraspCandidatesFromResultSet(qResult);
}
std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::queryLatestBimanualGraspCandidateEntity(
const std::string& provider, const std::string& entity) const
std::map<std::string, grasping::BimanualGraspCandidatePtr>
GraspCandidateReader::queryLatestBimanualGraspCandidateEntity(const std::string& provider,
const std::string& entity) const
{
armem::client::query::Builder qb;
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
qb
.coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
.providerSegments().withName(provider)
.entities().withName(entity)
.snapshots().latest();
qb.coreSegments()
.withName(properties.bimanualGraspCandidateMemoryName)
.providerSegments()
.withName(provider)
.entities()
.withName(entity)
.snapshots()
.latest();
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getBimanualGraspCandidatesFromResultSet(qResult);
}
grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidates(
const std::string& provider) const
grasping::GraspCandidateDict
GraspCandidateReader::queryLatestGraspCandidates(const std::string& provider) const
{
armem::client::query::Builder qb;
......@@ -190,29 +196,34 @@ namespace armarx::armem
if (!provider.empty())
{
qb
.coreSegments().withName(properties.graspCandidateMemoryName)
.providerSegments().withName(provider)
.entities().all()
.snapshots().latest();
qb.coreSegments()
.withName(properties.graspCandidateMemoryName)
.providerSegments()
.withName(provider)
.entities()
.all()
.snapshots()
.latest();
}
else
{
qb
.coreSegments().withName(properties.graspCandidateMemoryName)
.providerSegments().all()
.entities().all()
.snapshots().latest();
qb.coreSegments()
.withName(properties.graspCandidateMemoryName)
.providerSegments()
.all()
.entities()
.all()
.snapshots()
.latest();
}
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getGraspCandidatesFromResultSet(qResult);
}
std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::queryLatestBimanualGraspCandidates(
const std::string& provider) const
std::map<std::string, grasping::BimanualGraspCandidatePtr>
GraspCandidateReader::queryLatestBimanualGraspCandidates(const std::string& provider) const
{
armem::client::query::Builder qb;
......@@ -221,42 +232,49 @@ namespace armarx::armem
if (!provider.empty())
{
qb
.coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
.providerSegments().withName(provider)
.entities().all()
.snapshots().latest();
qb.coreSegments()
.withName(properties.bimanualGraspCandidateMemoryName)
.providerSegments()
.withName(provider)
.entities()
.all()
.snapshots()
.latest();
}
else
{
qb
.coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
.providerSegments().all()
.entities().all()
.snapshots().latest();
qb.coreSegments()
.withName(properties.bimanualGraspCandidateMemoryName)
.providerSegments()
.all()
.entities()
.all()
.snapshots()
.latest();
}
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getBimanualGraspCandidatesFromResultSet(qResult);
}
void GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
void
GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "GraspCandidateReader: registerPropertyDefinitions";
const std::string prefix = propertyPrefix;
def->optional(properties.graspCandidateMemoryName, prefix + "GraspCandidateMemoryName",
def->optional(properties.graspCandidateMemoryName,
prefix + "GraspCandidateMemoryName",
"Name of the grasping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
grasping::GraspCandidateDict GraspCandidateReader::getGraspCandidatesFromResultSet(
const armem::client::QueryResult& qResult) const
grasping::GraspCandidateDict
GraspCandidateReader::getGraspCandidatesFromResultSet(
const armem::client::QueryResult& qResult) const
{
if (!qResult.success)
{
......@@ -267,9 +285,10 @@ namespace armarx::armem
std::map<std::string, armarx::grasping::GraspCandidatePtr> candidates;
armem::wm::FunctionalVisitor visitor;
visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
{
candidates[instance.id().str()] = new grasping::GraspCandidate(asGraspCandidate(instance));
candidates[instance.id().str()] =
new grasping::GraspCandidate(asGraspCandidate(instance));
return true;
};
......@@ -279,8 +298,9 @@ namespace armarx::armem
return candidates;
}
std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::getBimanualGraspCandidatesFromResultSet(
const armem::client::QueryResult& qResult) const
std::map<std::string, grasping::BimanualGraspCandidatePtr>
GraspCandidateReader::getBimanualGraspCandidatesFromResultSet(
const armem::client::QueryResult& qResult) const
{
if (!qResult.success)
{
......@@ -291,9 +311,10 @@ namespace armarx::armem
std::map<std::string, armarx::grasping::BimanualGraspCandidatePtr> candidates;
armem::wm::FunctionalVisitor visitor;
visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
{
candidates[instance.id().str()] = new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
candidates[instance.id().str()] =
new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
return true;
};
......@@ -303,5 +324,41 @@ namespace armarx::armem
return candidates;
}
grasping::GraspCandidateDict
GraspCandidateReader::queryGraspCandidatesNewerThan(const std::string& provider,
const DateTime& timestamp) const
{
armem::client::query::Builder qb;
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
if (!provider.empty())
{
qb.coreSegments()
.withName(properties.graspCandidateMemoryName)
.providerSegments()
.withName(provider)
.entities()
.all()
.snapshots()
.timeRange(timestamp, armarx::core::time::DateTime::Now());
}
else
{
qb.coreSegments()
.withName(properties.graspCandidateMemoryName)
.providerSegments()
.all()
.entities()
.all()
.snapshots()
.timeRange(timestamp, armarx::core::time::DateTime::Now());
}
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getGraspCandidatesFromResultSet(qResult);
}
}
} // namespace armarx::armem
source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+ 27
− 22
View file @ 5d37e452
......@@ -2,10 +2,9 @@
#include <vector>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
namespace armarx::armem
{
......@@ -17,35 +16,43 @@ namespace armarx::armem
void connect(bool use = true);
grasping::GraspCandidatePtr queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
grasping::GraspCandidateDict queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
grasping::GraspCandidatePtr
queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
grasping::BimanualGraspCandidatePtr queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
grasping::GraspCandidateDict
queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
grasping::GraspCandidateDict queryLatestGraspCandidateEntity(
std::string const& provider, std::string const& entity) const;
grasping::BimanualGraspCandidatePtr
queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidateEntity(
std::string const& provider, std::string const& entity) const;
grasping::GraspCandidateDict
queryLatestGraspCandidateEntity(std::string const& provider,
std::string const& entity) const;
grasping::GraspCandidateDict queryLatestGraspCandidates(
std::string const& provider = "") const;
std::map<std::string, grasping::BimanualGraspCandidatePtr>
queryLatestBimanualGraspCandidateEntity(std::string const& provider,
std::string const& entity) const;
std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidates(
std::string const& provider = "") const;
grasping::GraspCandidateDict
queryLatestGraspCandidates(std::string const& provider = "") const;
grasping::GraspCandidateDict queryGraspCandidatesNewerThan(
std::string const& provider = "",
const armarx::DateTime& timestamp = armarx::DateTime::Now()) const;
std::map<std::string, grasping::BimanualGraspCandidatePtr>
queryLatestBimanualGraspCandidates(std::string const& provider = "") const;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
private:
grasping::GraspCandidateDict
getGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
grasping::GraspCandidateDict getGraspCandidatesFromResultSet(
armem::client::QueryResult const& qResult) const;
std::map<std::string, grasping::BimanualGraspCandidatePtr> getBimanualGraspCandidatesFromResultSet(
armem::client::QueryResult const& qResult) const;
std::map<std::string, grasping::BimanualGraspCandidatePtr>
getBimanualGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
armem::client::Reader memoryReader;
......@@ -57,11 +64,9 @@ namespace armarx::armem
std::string bimanualGraspCandidateMemoryName = "BimanualGraspCandidate";
} properties;
const std::string propertyPrefix = "mem.grasping.";
armem::client::MemoryNameSystem& memoryNameSystem;
};
}
} // namespace armarx::armem
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment