diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt index dd799ece020b5f868b1926464366218c3b5d0cc6..a08be696d21fc9548c370331a06296ab565df4d9 100644 --- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt @@ -36,9 +36,12 @@ armarx_add_library( server/attachments/Segment.h + client/articulated_object/Reader.h + client/articulated_object/Writer.h + client/articulated_object/interfaces.h - client/articulated_object/Reader.cpp - client/articulated_object/Writer.cpp + # client/attachment/Reader.h + client/attachment/Writer.h SOURCES aron_conversions.cpp @@ -60,10 +63,11 @@ armarx_add_library( server/attachments/Segment.cpp + client/articulated_object/Reader.cpp + client/articulated_object/Writer.cpp - client/articulated_object/Reader.h - client/articulated_object/Writer.h - client/articulated_object/interfaces.h + # 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 new file mode 100644 index 0000000000000000000000000000000000000000..b80a0a07c82ac6ee452b7cab489cee1f12e11b3b --- /dev/null +++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp @@ -0,0 +1,313 @@ +#include "Reader.h" + +#include <mutex> +#include <optional> + +#include "ArmarXCore/core/logging/Logging.h" +#include <ArmarXCore/core/PackagePath.h> + +#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_robot/robot_conversions.h" +#include "RobotAPI/libraries/armem_robot/aron_conversions.h" +#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h> + + +namespace fs = ::std::filesystem; + +namespace armarx::armem::articulated_object +{ + + Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {} + + void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) + { + ARMARX_DEBUG << "Reader: registerPropertyDefinitions"; + + const std::string prefix = propertyPrefix; + + 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, + prefix + "CoreSegment", + "Name of the memory core segment to use for object classes."); + def->optional(properties.providerName, prefix + "ProviderName"); + } + + + void Reader::connect() + { + // Wait for the memory to become available and add it as dependency. + ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ..."; + auto result = component.useMemory(properties.memoryName); + if (not result.success) + { + ARMARX_ERROR << result.errorMessage; + return; + } + + 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) + { + 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. + armem::client::query::Builder qb; + + // clang-format off + qb + .coreSegments().withName(properties.coreClassSegmentName) + .providerSegments().all() + .entities().all() + .snapshots().latest(); // TODO beforeTime(timestamp); + // clang-format on + + const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput()); + + ARMARX_DEBUG << "Lookup result in reader: " << qResult; + + if (not qResult.success) /* c++20 [[unlikely]] */ + { + return {}; + } + + return getRobotDescriptions(qResult.memory); + } + + std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp) + { + // 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()); + + ARMARX_DEBUG << "Lookup result in reader: " << qResult; + + if (not qResult.success) /* c++20 [[unlikely]] */ + { + return std::nullopt; + } + + return getRobotDescription(qResult.memory); + } + + std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp) + { + // TODO(fabian.reister): how to deal with multiple providers? + + // Query all entities from 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); + // clang-format on + + const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput()); + + ARMARX_DEBUG << "Lookup result in reader: " << qResult; + + if (not qResult.success) /* c++20 [[unlikely]] */ + { + return std::nullopt; + } + + return getRobotState(qResult.memory); + } + + + std::optional<robot::RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) 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); + } + + + + 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 + // 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::convertRobotDescription(instance); + } + + 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 diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h new file mode 100644 index 0000000000000000000000000000000000000000..a7b33dfbfcbf6c9dedb02b654dde0c2912500642 --- /dev/null +++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h @@ -0,0 +1,79 @@ +/* + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Fabian Reister ( fabian dot reister at kit dot edu ) + * @date 2021 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include <mutex> +#include <optional> + +#include "RobotAPI/libraries/armem/client.h" +#include "RobotAPI/libraries/armem/client/Reader.h" + +namespace armarx::armem::attachment +{ + class Reader + { + public: + Reader(armem::ClientReaderComponentPluginUser& component); + virtual ~Reader() = default; + + void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def); + void connect(); + + // void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override; + + // 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; + + 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"; + } properties; + + const std::string propertyPrefix = "mem.obj.articulated."; + + armem::client::Reader memoryReader; + std::mutex memoryWriterMutex; + + armem::ClientReaderComponentPluginUser& component; + }; + + +} // namespace armarx::armem::articulated_object \ No newline at end of file diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp new file mode 100644 index 0000000000000000000000000000000000000000..774b282fbe2cac8de48aaf5092332ca6a48e4daf --- /dev/null +++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp @@ -0,0 +1,164 @@ +#include "Writer.h" + +#include <IceUtil/Time.h> +#include <SimoxUtility/algorithm/get_map_keys_values.h> +#include <mutex> +#include <optional> + +#include "ArmarXCore/core/logging/Logging.h" + +#include "RobotAPI/libraries/armem/core/MemoryID.h" +#include "RobotAPI/libraries/armem_objects/aron_conversions.h" +#include "RobotAPI/libraries/armem_robot/aron_conversions.h" +#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h> +#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h> +#include <RobotAPI/libraries/armem/core/aron_conversions.h> +#include "RobotAPI/libraries/armem_robot/robot_conversions.h" + + +namespace armarx::armem::attachment +{ + Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {} + + void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) + { + ARMARX_DEBUG << "Writer: registerPropertyDefinitions"; + + const std::string prefix = propertyPrefix; + + def->optional(properties.memoryName, prefix + "MemoryName"); + + def->optional(properties.coreAttachmentsSegmentName, + prefix + "CoreSegment", + "Name of the memory core segment to use for object attachments."); + def->optional(properties.providerName, prefix + "ProviderName"); + } + + void Writer::connect() + { + // Wait for the memory to become available and add it as dependency. + ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ..."; + auto result = component.useMemory(properties.memoryName); + if (not result.success) + { + ARMARX_ERROR << result.errorMessage; + return; + } + + ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName; + + memoryWriter.setWritingMemory(result.proxy); + memoryReader.setReadingMemory(result.proxy); + } + + std::optional<armem::MemoryID> Writer::add(const ObjectAttachment& attachment) + { + return commit(attachment, true); + } + + std::optional<armem::MemoryID> Writer::remove(const ObjectAttachment& attachment) + { + return commit(attachment, false); + } + + std::optional<armem::MemoryID> Writer::add(const ArticulatedObjectAttachment& attachment) + { + return commit(attachment, true); + } + + std::optional<armem::MemoryID> Writer::remove(const ArticulatedObjectAttachment& attachment) + { + return commit(attachment, false); + } + + + std::optional<armem::MemoryID> Writer::commit(const ObjectAttachment& attachment, const bool active){ + std::lock_guard g{memoryWriterMutex}; + + const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName); + + if (not result.success) + { + ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage; + return std::nullopt; + } + + const auto& timestamp = attachment.timestamp; + + const auto providerId = armem::MemoryID(result.segmentID); + const auto entityID = + providerId + .withEntityName(attachment.object.entityName) // TODO check if meaningful + .withTimestamp(timestamp); + + armem::EntityUpdate update; + update.entityID = entityID; + + arondto::attachment::ObjectAttachment aronAttachment; + toAron(aronAttachment, attachment); + aronAttachment.active = active; + + update.instancesData = {aronAttachment.toAron()}; + update.timeCreated = timestamp; + + ARMARX_DEBUG << "Committing " << update << " at time " << timestamp; + armem::EntityUpdateResult updateResult = memoryWriter.commit(update); + + ARMARX_DEBUG << updateResult; + + if (not updateResult.success) + { + ARMARX_ERROR << updateResult.errorMessage; + return std::nullopt; + } + + return updateResult.snapshotID; + + } + + std::optional<armem::MemoryID> Writer::commit(const ArticulatedObjectAttachment& attachment, const bool active){ + std::lock_guard g{memoryWriterMutex}; + + const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName); + + if (not result.success) + { + ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage; + return std::nullopt; + } + + const auto& timestamp = attachment.timestamp; + + const auto providerId = armem::MemoryID(result.segmentID); + const auto entityID = + providerId + .withEntityName(attachment.object.id.entityName) // TODO check if meaningful + .withTimestamp(timestamp); + + armem::EntityUpdate update; + update.entityID = entityID; + + arondto::attachment::ArticulatedObjectAttachment aronAttachment; + toAron(aronAttachment, attachment); + aronAttachment.active = active; + + update.instancesData = {aronAttachment.toAron()}; + update.timeCreated = timestamp; + + ARMARX_DEBUG << "Committing " << update << " at time " << timestamp; + armem::EntityUpdateResult updateResult = memoryWriter.commit(update); + + ARMARX_DEBUG << updateResult; + + if (not updateResult.success) + { + ARMARX_ERROR << updateResult.errorMessage; + return std::nullopt; + } + + return updateResult.snapshotID; + } + + + +} // namespace armarx::armem::attachment \ No newline at end of file diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h new file mode 100644 index 0000000000000000000000000000000000000000..b464811ea7ee1e8ccbd8b92dc36923f82ba0579f --- /dev/null +++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h @@ -0,0 +1,80 @@ +/* + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Fabian Reister ( fabian dot reister at kit dot edu ) + * @date 2021 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include <mutex> + +#include "RobotAPI/libraries/armem/client/Reader.h" +#include "RobotAPI/libraries/armem/client/Writer.h" +#include "RobotAPI/libraries/armem/client.h" + +#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h" + +#include "RobotAPI/libraries/armem_objects/types.h" + +namespace armarx::armem::attachment +{ + + class Writer + { + public: + Writer(armem::ClientComponentPluginUser& component); + virtual ~Writer() = default; + + + void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def); + void connect(); + + std::optional<armem::MemoryID> add(const ObjectAttachment& attachment); + std::optional<armem::MemoryID> remove(const ObjectAttachment& attachment); + + std::optional<armem::MemoryID> add(const ArticulatedObjectAttachment& attachment); + std::optional<armem::MemoryID> remove(const ArticulatedObjectAttachment& attachment); + + // const std::string& getPropertyPrefix() const override; + + private: + std::optional<armem::MemoryID> commit(const ObjectAttachment& attachment, bool active); + std::optional<armem::MemoryID> commit(const ArticulatedObjectAttachment& attachment, bool active); + + struct Properties + { + std::string memoryName = "Object"; + std::string coreAttachmentsSegmentName = "Attachments"; + std::string providerName = "AttachmentProvider"; + + bool allowClassCreation = false; + } properties; + + const std::string propertyPrefix = "mem.obj.articulated."; + + armem::client::Writer memoryWriter; + std::mutex memoryWriterMutex; + + armem::client::Reader memoryReader; + std::mutex memoryReaderMutex; + + armem::ClientComponentPluginUser& component; + }; + + +} // namespace armarx::armem::attachment \ No newline at end of file