From 6a1c01aed2ad1da97e84d4e188c7b5b03110f13b Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 20 May 2021 14:00:31 +0200
Subject: [PATCH] implemented reader
---
.../libraries/armem_objects/CMakeLists.txt | 4 +-
.../client/attachment/Reader.cpp | 287 ++++++------------
.../armem_objects/client/attachment/Reader.h | 33 +-
3 files changed, 104 insertions(+), 220 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index a08be696d..8c0f625f0 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -40,7 +40,7 @@ armarx_add_library(
client/articulated_object/Writer.h
client/articulated_object/interfaces.h
- # client/attachment/Reader.h
+ client/attachment/Reader.h
client/attachment/Writer.h
SOURCES
@@ -66,7 +66,7 @@ armarx_add_library(
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
- # client/attachment/Reader.cpp
+ client/attachment/Reader.cpp
client/attachment/Writer.cpp
)
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
index b80a0a07c..2e664a450 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
@@ -9,15 +9,68 @@
#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
+#include "RobotAPI/libraries/armem/util/util.h"
#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+namespace armarx::armem::attachment
+{
-namespace fs = ::std::filesystem;
+ namespace detail
+ {
-namespace armarx::armem::articulated_object
-{
+ template<typename AronClass, typename ArmemClass>
+ auto getAttachments(const armarx::armem::wm::Memory& memory)
+ {
+ // using ArmemClass = decltype(fromAron(AronClass()));
+ using ArmemClassVector = std::vector<ArmemClass>;
+
+ ArmemClassVector attachments;
+
+ for (const auto&[_, coreSegment] : memory.coreSegments())
+ {
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ {
+ for (const auto& [name, entity] : providerSegment.entities())
+ {
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ try
+ {
+ AronClass aronAttachment;
+ aronAttachment.fromAron(instance.data());
+
+ ArmemClass attachment;
+ fromAron(aronAttachment, attachment);
+
+ if (attachment.active)
+ {
+ attachments.push_back(attachment);
+ }
+
+ }
+ catch (const armarx::aron::error::AronException&)
+ {
+ continue;
+ }
+ }
+ }
+ }
+
+ return attachments;
+ }
+ } // namespace detail
Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
@@ -29,13 +82,9 @@ namespace armarx::armem::articulated_object
def->optional(properties.memoryName, prefix + "MemoryName");
- def->optional(properties.coreInstanceSegmentName,
- prefix + "CoreSegment",
- "Name of the memory core segment to use for object instances.");
- def->optional(properties.coreClassSegmentName,
+ def->optional(properties.coreAttachmentsSegmentName,
prefix + "CoreSegment",
- "Name of the memory core segment to use for object classes.");
- def->optional(properties.providerName, prefix + "ProviderName");
+ "Name of the memory core segment to use for object attachments.");
}
@@ -53,104 +102,20 @@ namespace armarx::armem::articulated_object
ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
memoryReader.setReadingMemory(result.proxy);
-
- armem::MemoryID id = armem::MemoryID();
- id.memoryName = properties.memoryName;
- id.coreSegmentName = properties.coreClassSegmentName;
- // listen to all provider segments!
-
- memoryReader.subscribe(id, this, &Reader::updateKnownObjects);
}
- void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
- {
- // const std::string& nameWithDataset = snapshotId.providerSegmentName;
-
- // arondto::RobotDescription aronArticulatedObjectDescription;
- // aronArticulatedObjectDescription.fromAron(snapshotId.);
- // TODO(fabian.reister): implement
- }
-
- void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp) const
{
- ARMARX_INFO << "New objects available!";
-
- // // Query all entities from provider.
- // armem::client::query::Builder qb;
-
- // // clang-format off
- // qb
- // .coreSegments().withName(properties.coreClassSegmentName)
- // .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
- // .entities().withName(name)
- // .snapshots().atTime(timestamp);
- // // clang-format on
-
- // const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
-
- // std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
- // {
- // updateKnownObject(snapshotID);
- // });
- }
-
-
- std::optional<ArticulatedObject> Reader::get(const std::string& name, const armem::Time& timestamp)
- {
- const auto description = queryDescription(name, timestamp);
-
- if (not description)
- {
- ARMARX_WARNING << "Unknown object " << name;
- return std::nullopt;
- }
-
- return get(*description, timestamp);
- }
-
-
- ArticulatedObject Reader::get(const ArticulatedObjectDescription& description,
- const armem::Time& timestamp)
- {
- ArticulatedObject obj
- {
- .description = description,
- .instance = "", // TODO(fabian.reister):
- .config = {}, // will be populated by synchronize
- .timestamp = timestamp
- };
-
- synchronize(obj, timestamp);
-
- return obj;
- }
-
- void Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
- {
- auto state = queryState(obj.description, timestamp);
-
- if (not state) /* c++20 [[unlikely]] */
- {
- ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
- return;
- }
-
- obj.config = std::move(*state);
- }
-
- std::vector<robot::RobotDescription> Reader::queryDescriptions(const armem::Time& timestamp)
- {
- // Query all entities from provider.
+ // Query all entities from all provider.
armem::client::query::Builder qb;
// clang-format off
qb
- .coreSegments().withName(properties.coreClassSegmentName)
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
.providerSegments().all()
.entities().all()
- .snapshots().latest(); // TODO beforeTime(timestamp);
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -162,20 +127,20 @@ namespace armarx::armem::articulated_object
return {};
}
- return getRobotDescriptions(qResult.memory);
+ return detail::getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
}
- std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
{
// Query all entities from provider.
armem::client::query::Builder qb;
// clang-format off
qb
- .coreSegments().withName(properties.coreClassSegmentName)
- .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
- .entities().withName(name)
- .snapshots().atTime(timestamp);
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
+ .providerSegments().withName(providerName)
+ .entities().all()
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -184,25 +149,23 @@ namespace armarx::armem::articulated_object
if (not qResult.success) /* c++20 [[unlikely]] */
{
- return std::nullopt;
+ return {};
}
- return getRobotDescription(qResult.memory);
+ return detail::getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
}
- std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
+ std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp) const
{
- // TODO(fabian.reister): how to deal with multiple providers?
-
- // Query all entities from provider.
+ // Query all entities from all provider.
armem::client::query::Builder qb;
// clang-format off
qb
- .coreSegments().withName(properties.coreInstanceSegmentName)
- .providerSegments().withName(properties.providerName) // agent
- .entities().withName(description.name)
- .snapshots().atTime(timestamp);
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -211,103 +174,37 @@ namespace armarx::armem::articulated_object
if (not qResult.success) /* c++20 [[unlikely]] */
{
- return std::nullopt;
+ return {};
}
- return getRobotState(qResult.memory);
+ return detail::getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
}
-
- std::optional<robot::RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+ std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
{
- // clang-format off
- const armem::wm::ProviderSegment& providerSegment = memory
- .getCoreSegment(properties.coreInstanceSegmentName)
- .getProviderSegment(properties.providerName);
- // clang-format on
- const auto entities = simox::alg::get_values(providerSegment.entities());
-
- if (entities.empty())
- {
- ARMARX_WARNING << "No entity found";
- return std::nullopt;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
-
- if (entitySnapshots.empty())
- {
- ARMARX_WARNING << "No entity snapshots found";
- return std::nullopt;
- }
-
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- return robot::convertRobotState(instance);
- }
-
-
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
- std::optional<robot::RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
- {
// clang-format off
- const armem::wm::ProviderSegment& providerSegment = memory
- .getCoreSegment(properties.coreClassSegmentName)
- .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
+ qb
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
+ .providerSegments().withName(providerName)
+ .entities().all()
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
- const auto entities = simox::alg::get_values(providerSegment.entities());
- if (entities.empty())
- {
- ARMARX_WARNING << "No entity found";
- return std::nullopt;
- }
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
- if (entitySnapshots.empty())
+ if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << "No entity snapshots found";
- return std::nullopt;
+ return {};
}
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- return robot::convertRobotDescription(instance);
+ return detail::getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
}
- std::vector<robot::RobotDescription> Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
- {
- std::vector<robot::RobotDescription> descriptions;
-
- const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
-
- for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
- {
- for (const auto& [name, entity] : providerSegment.entities())
- {
- if (entity.empty())
- {
- ARMARX_WARNING << "No entity found";
- continue;
- }
-
- const auto entitySnapshots = simox::alg::get_values(entity.history());
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- const auto robotDescription = robot::convertRobotDescription(instance);
-
- if (robotDescription)
- {
- descriptions.push_back(*robotDescription);
- }
- }
- }
-
- return descriptions;
- }
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::attachment
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
index a7b33dfbf..bcfb3d1a1 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
@@ -26,6 +26,7 @@
#include "RobotAPI/libraries/armem/client.h"
#include "RobotAPI/libraries/armem/client/Reader.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
namespace armarx::armem::attachment
{
@@ -38,36 +39,22 @@ namespace armarx::armem::attachment
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect();
- // void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
+ std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp) const;
+ std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
- // std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) override;
- // ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) override;
-
- // std::optional<robot::RobotState> queryState(const robot::RobotDescription& description, const armem::Time& timestamp);
- // std::optional<robot::RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
-
- // std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
-
- // TODO(fabian.reister): register property defs
-
- protected:
- std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
- std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
- std::vector<robot::RobotDescription> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+ std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp) const;
+ std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
private:
- void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
- void updateKnownObject(const armem::MemoryID& snapshotId);
+
struct Properties
{
- std::string memoryName = "Object";
- std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
- std::string coreClassSegmentName = "ArticulatedObjectClass";
- std::string providerName = "ArmarXObjects";
+ std::string memoryName = "Object";
+ std::string coreAttachmentsSegmentName = "Attachments";
} properties;
- const std::string propertyPrefix = "mem.obj.articulated.";
+ const std::string propertyPrefix = "mem.obj.attachment.";
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
@@ -76,4 +63,4 @@ namespace armarx::armem::attachment
};
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::attachment
\ No newline at end of file
--
GitLab