diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt index b0663babe7059ddbc2915869dc49d06a128f03e5..ac67c9ad6cc34d051144df93def583c94ce63187 100644 --- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt @@ -44,6 +44,7 @@ armarx_add_library( client/attachment/Writer.h client/instance/ObjectReader.h + client/instance/ObjectWriter.h SOURCES aron_conversions.cpp @@ -58,6 +59,7 @@ armarx_add_library( client/attachment/Writer.cpp client/instance/ObjectReader.cpp + client/instance/ObjectWriter.cpp ARON_FILES aron/ObjectClass.xml diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ad9c8c83f2ca8889292c8a2023a74fd54528e04a --- /dev/null +++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp @@ -0,0 +1,81 @@ +#include "ObjectWriter.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/core/error.h> +#include <RobotAPI/libraries/armem/client/query/Builder.h> +#include <RobotAPI/libraries/armem/core/wm/memory_definitions.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::obj::instance +{ + Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) : + memoryNameSystem(memoryNameSystem) + {} + + void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) + { + ARMARX_DEBUG << "Writer: registerPropertyDefinitions"; + + const std::string prefix = propertyPrefix; + + def->optional(properties.memoryName, prefix + "MemoryName"); + + def->optional(properties.coreSegmentName, + prefix + "CoreSegment", + "Name of the memory core segment to use for object instances."); + } + + + void Writer::connect() + { + // Wait for the memory to become available and add it as dependency. + ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ..."; + try + { + memoryWriter = memoryNameSystem.useWriter(properties.memoryName); + ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'"; + } + catch (const armem::error::CouldNotResolveMemoryServer& e) + { + ARMARX_ERROR << e.what(); + return; + } + } + + bool Writer::commitObject(const armem::arondto::ObjectInstance& inst, const std::string& provider, const armem::Time& t) + { + armem::Commit c; + auto& e = c.add(); + + e.entityID.memoryName = properties.memoryName; + e.entityID.coreSegmentName = properties.coreSegmentName; + e.entityID.providerSegmentName = provider; + e.entityID.entityName = inst.pose.objectID.dataset + "/" + inst.pose.objectID.className + "/" + inst.pose.objectID.instanceName; + e.timeCreated = t; + e.timeSent = IceUtil::Time::now(); + e.instancesData = { inst.toAron() }; + + auto res = memoryWriter.commit(c); + + if(!res.allSuccess()) + { + ARMARX_ERROR << "Failed to commit an ObjectInstance to memory: " << res.allErrorMessages(); + return false; + } + return true; + } + +} // namespace armarx::armem::instance diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h new file mode 100644 index 0000000000000000000000000000000000000000..1c9f82e01dd6e48f4264b5de597413fb0b2eb57c --- /dev/null +++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h @@ -0,0 +1,67 @@ +/* + * 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 <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h> + +#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h> +#include <RobotAPI/libraries/armem/client/Writer.h> +#include <RobotAPI/libraries/armem_objects/types.h> + +#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h> + + +namespace armarx::armem::obj::instance +{ + class Writer + { + public: + Writer(armem::client::MemoryNameSystem& memoryNameSystem); + virtual ~Writer() = default; + + void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def); + void connect(); + + bool commitObject(const armem::arondto::ObjectInstance& inst, const std::string& provider, const armem::Time&); + + + private: + + + struct Properties + { + std::string memoryName = "Object"; + std::string coreSegmentName = "Instance"; + } properties; + + const std::string propertyPrefix = "mem.obj.instance."; + + armem::client::MemoryNameSystem& memoryNameSystem; + armem::client::Writer memoryWriter; + std::mutex memoryWriterMutex; + }; + + +} // namespace armarx::armem::instance