diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
index 50dce2f4a5313f280f8f06bab180de49a4ce8098..82769e0f22bfa6de0ca678be094d396df07c89e0 100644
--- a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
@@ -102,6 +102,14 @@ ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = Exam
# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener = MemoryUpdates
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency: Memory update frequency (write).
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency = 25
+
+
# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
# Attributes:
# - Default: mongo/.cache
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 837fcbcf8278e12b120b3dc9ff0a0b1de514fc9a..b6d997f6882daa1b9ee95d97fb4502ae181aa9ab 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -57,6 +57,8 @@ namespace armarx::armem::server::obj
articulatedObjectInstanceSegment.defineProperties(defs, prefix + "articulated.inst.");
articulatedObjectClassSegment.defineProperties(defs, prefix + "articulated.cls.");
+ attachmentSegment.defineProperties(defs, prefix + "attachments.");
+
return defs;
}
@@ -69,7 +71,9 @@ namespace armarx::armem::server::obj
articulatedObjectClassSegment(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex),
articulatedObjectInstanceSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex)
+ server::ComponentPluginUser::workingMemoryMutex),
+ attachmentSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex)
{
}
@@ -120,6 +124,10 @@ namespace armarx::armem::server::obj
articulatedObjectInstanceSegment.init();
});
+ initSegmentWithCatch("attachment", [&]()
+ {
+ attachmentSegment.init();
+ });
}
@@ -155,12 +163,9 @@ namespace armarx::armem::server::obj
ArVizComponentPluginUser::getArvizClient()
);
-
- // {
- // std::lock_guard g(server::ComponentPluginUser::workingMemoryMutex);
- // // server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectInstance"); // TODO , arondto::::toInitialAronType());
- // server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectClass"); // TODO , arondto::::toInitialAronType());
- // }
+ attachmentSegment.connect(
+ ArVizComponentPluginUser::getArvizClient()
+ );
createRemoteGuiTab();
RemoteGui_startRunningTask();
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 822fabbe56fad5158337c81c9c0089aa5d7233c2..2dfd668d1e43df914ff7b2f02205e699ad2ea5e9 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -40,6 +40,7 @@
#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
#include <RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
@@ -112,7 +113,9 @@ namespace armarx::armem::server::obj
articulated_object_class::Segment articulatedObjectClassSegment;
articulated_object_instance::Segment articulatedObjectInstanceSegment;
+ attachments::Segment attachmentSegment;
+ // associations::Segment associationsSegment;
struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
{
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index ed02a9adf315ef755af2c8da64d4a37c3d4817b6..8c0f625f022e3efeb7cf7763841a25e6469b6ac8 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -34,8 +34,14 @@ armarx_add_library(
# server/articulated_object/SegmentAdapter.h
server/articulated_object_instance/Visu.h
- client/articulated_object/Reader.cpp
- client/articulated_object/Writer.cpp
+ server/attachments/Segment.h
+
+ client/articulated_object/Reader.h
+ client/articulated_object/Writer.h
+ client/articulated_object/interfaces.h
+
+ client/attachment/Reader.h
+ client/attachment/Writer.h
SOURCES
aron_conversions.cpp
@@ -55,9 +61,13 @@ armarx_add_library(
# server/articulated_object/SegmentAdapter.cpp
server/articulated_object_instance/Visu.cpp
- client/articulated_object/Reader.h
- client/articulated_object/Writer.h
- client/articulated_object/interfaces.h
+ server/attachments/Segment.cpp
+
+ client/articulated_object/Reader.cpp
+ client/articulated_object/Writer.cpp
+
+ client/attachment/Reader.cpp
+ client/attachment/Writer.cpp
)
@@ -70,8 +80,8 @@ armarx_enable_aron_file_generation_for_target(
aron/ObjectClass.xml
aron/ObjectInstance.xml
- # aron/Attachment.xml
- aron/Constraint.xml
+ aron/Attachment.xml
+ # aron/Constraint.xml
)
add_library(${PROJECT_NAME}::armem_objects ALIAS armem_objects)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/Attachment.xml b/source/RobotAPI/libraries/armem_objects/aron/Attachment.xml
index 6b0227772cb6c40b0b449dcd3e931e5314ad1548..76cb21c07a16e8f37c0fa00f6a4ab2b90cfd2983 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/Attachment.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/Attachment.xml
@@ -1,27 +1,71 @@
<!--This class contains the data structure for ObjectPose -->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- <Include include="<Eigen/Core>" />
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
</CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
<GenerateTypes>
- <Object name="armarx::armem::arondto::Attachment">
+ <Object name="armarx::armem::arondto::attachment::AgentDescription">
- <ObjectChild key="constraintType">
+ <!-- Memory link pointing to arondto::Robot or arondto::ArticulatedObject -->
+ <ObjectChild key="id">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <!-- one of the robot's frames -->
+ <ObjectChild key="frame">
+ <string/>
+ </ObjectChild>
+ </Object>
+ <!-- Fixed transformation between agent and object -->
+ <Object name="armarx::armem::arondto::attachment::ObjectAttachment">
+ <ObjectChild key="agent">
+ <armarx::armem::arondto::attachment::AgentDescription />
</ObjectChild>
<ObjectChild key="transformation">
<Pose/>
</ObjectChild>
- <ObjectChild key="objectName">
- <string/>
+ <ObjectChild key="object">
+ <armarx::armem::arondto::MemoryID />
</ObjectChild>
- <ObjectChild key="objectNode">
- <string/>
+ <ObjectChild key="active">
+ <Bool/>
+ </ObjectChild>
+
+ <ObjectChild key="timestamp">
+ <Time/>
+ </ObjectChild>
+ </Object>
+
+ <!-- Fixed transformation between agent and articulated object -->
+ <Object name="armarx::armem::arondto::attachment::ArticulatedObjectAttachment">
+
+ <ObjectChild key="agent">
+ <armarx::armem::arondto::attachment::AgentDescription />
+ </ObjectChild>
+
+ <ObjectChild key="transformation">
+ <Pose/>
+ </ObjectChild>
+
+ <ObjectChild key="object">
+ <armarx::armem::arondto::attachment::AgentDescription />
+ </ObjectChild>
+
+ <ObjectChild key="active">
+ <Bool/>
+ </ObjectChild>
+
+ <ObjectChild key="timestamp">
+ <Time/>
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 967060360c3bff360150f1c8988b5d40db4dbd5c..8602178224b696fdcf026f50184628ee7a870dae 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -2,9 +2,8 @@
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
namespace armarx::armem
{
@@ -30,6 +29,61 @@ namespace armarx::armem
objpose::toAron(dto.pose, bo);
}
+
+ /* Attachments */
+ void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo)
+ {
+ fromAron(dto.id, bo.id);
+ aron::fromAron(dto.frame, bo.frame);
+ }
+
+ void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo)
+ {
+ toAron(dto.id, bo.id);
+ aron::toAron(dto.frame, bo.frame);
+ }
+
+
+ void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo)
+ {
+ fromAron(dto.agent, bo.agent);
+ aron::fromAron(dto.transformation, bo.transformation);
+ fromAron(dto.object, bo.object);
+ aron::fromAron(dto.active, bo.active);
+ // TODO aron::fromAron(dto.timestamp, bo.timestamp);
+ }
+
+ void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo)
+ {
+ toAron(dto.agent, bo.agent);
+ aron::toAron(dto.transformation, bo.transformation);
+ toAron(dto.object, bo.object);
+ aron::toAron(dto.active, bo.active);
+ // TODO aron::toAron(dto.timestamp, bo.timestamp);
+ }
+
+
+ void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo)
+ {
+ fromAron(dto.agent, bo.agent);
+ aron::fromAron(dto.transformation, bo.transformation);
+ fromAron(dto.object, bo.object);
+ aron::fromAron(dto.active, bo.active);
+ // TODO aron::fromAron(dto.timestamp, bo.timestamp);
+ }
+
+ void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo)
+ {
+ toAron(dto.agent, bo.agent);
+ aron::toAron(dto.transformation, bo.transformation);
+ toAron(dto.object, bo.object);
+ aron::toAron(dto.active, bo.active);
+ // TODO aron::toAron(dto.timestamp, bo.timestamp);
+ }
+
+
+
+
} // namespace armarx::armem
armarx::armem::MemoryID
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index 1d74e95317521e27467753e46320914192558908..61e89c37ec00aaabed94e1b20a2ba1b50be976a8 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -3,6 +3,8 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem
@@ -13,6 +15,17 @@ namespace armarx::armem
void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
+
+ /* Attachments */
+ void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo);
+ void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo);
+
+ void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo);
+ void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo);
+
+ void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo);
+ void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo);
+
} // namespace armarx::armem
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..e3ce169b9ef41292b7ad565f94b74faf141ee7ad
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
@@ -0,0 +1,210 @@
+#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/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
+ {
+
+ 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
+
+ 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.coreAttachmentsSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object attachments.");
+ }
+
+
+ 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);
+ }
+
+
+ std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp) const
+ {
+ // Query all entities from all provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().beforeOrAtTime(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 getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
+ }
+
+ 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.coreAttachmentsSegmentName)
+ .providerSegments().withName(providerName)
+ .entities().all()
+ .snapshots().beforeOrAtTime(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 getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
+ }
+
+ std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp) const
+ {
+ // Query all entities from all provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().beforeOrAtTime(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 getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
+ }
+
+ std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(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.coreAttachmentsSegmentName)
+ .providerSegments().withName(providerName)
+ .entities().all()
+ .snapshots().beforeOrAtTime(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 getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
+ }
+
+
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..bcfb3d1a135a2446f255a0038a2aaad85c2a3412
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
@@ -0,0 +1,66 @@
+/*
+ * 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"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem::attachment
+{
+ class Reader
+ {
+ public:
+ Reader(armem::ClientReaderComponentPluginUser& component);
+ virtual ~Reader() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp) const;
+ std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
+
+ std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp) const;
+ std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
+
+ private:
+
+
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ std::string coreAttachmentsSegmentName = "Attachments";
+ } properties;
+
+ const std::string propertyPrefix = "mem.obj.attachment.";
+
+ armem::client::Reader memoryReader;
+ std::mutex memoryWriterMutex;
+
+ armem::ClientReaderComponentPluginUser& component;
+ };
+
+
+} // namespace armarx::armem::attachment
\ 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..f4d16070b84e4d29aca1742ce0356fb0c69ea5b0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
@@ -0,0 +1,144 @@
+#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::commit(const ObjectAttachment& attachment)
+ {
+ 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);
+
+ 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)
+ {
+ 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);
+
+ 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..f20421d4b27d9e2fc10e106e238a9684fca9d2cf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
@@ -0,0 +1,73 @@
+/*
+ * 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> commit(const ObjectAttachment& attachment);
+ std::optional<armem::MemoryID> commit(const ArticulatedObjectAttachment&);
+
+ private:
+
+ 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
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7f46c9b4509c54430eb2ddca0a10426d7ceab227
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -0,0 +1,127 @@
+#include "Segment.h"
+
+#include <sstream>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/armem/util/util.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+
+
+namespace armarx::armem::server::obj::attachments
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("Attachments");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ }
+
+ std::vector<armarx::armem::attachment::ObjectAttachment> Segment::getAttachments(const armem::Time& timestamp) const
+ {
+ std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
+
+ for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ {
+ for (const auto& [name, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
+
+ if (not aronAttachment)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
+ continue;
+ }
+
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+ armarx::armem::attachment::ObjectAttachment attachment;
+ fromAron(*aronAttachment, attachment);
+
+ attachments.push_back(attachment);
+ }
+ }
+
+ return attachments;
+ }
+
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..3817bc22a51227422848ebbe1e4296aa570fee24
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
@@ -0,0 +1,107 @@
+/*
+ * 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 <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::obj::attachments
+{
+ class Visu;
+
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::vector<armarx::armem::attachment::ObjectAttachment> getAttachments(const armem::Time& timestamp) const;
+
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ struct Properties
+ {
+ std::string coreClassSegmentName = "Attachments";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 855c855c4d36d5cf02785b6e34d652b1f82b1272..976f438e7c39009fd5d37fb6afdf19378da488c4 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -21,9 +21,75 @@
#pragma once
+#include "RobotAPI/libraries/armem/core/Time.h"
#include <vector>
+#include <Eigen/Geometry>
+
#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::attachment
+{
+ using AgentID = armem::MemoryID;
+ using ObjectID = armem::MemoryID;
+
+ struct AgentDescription
+ {
+ /**
+ * @brief id either pointing to a arondto::Robot or arondto::ArticulatedObject
+ *
+ */
+ AgentID id;
+
+ std::string frame;
+ };
+
+ /**
+ * @brief ObjectAttachment describes a fixed transformation between an agent and an object.
+ *
+ * The transformation is defined as follows:
+ *
+ * agent.frame -> object root frame
+ *
+ */
+ struct ObjectAttachment
+ {
+ AgentDescription agent;
+
+ Eigen::Affine3f transformation;
+
+ ObjectID object;
+
+ armem::Time timestamp;
+
+ bool active;
+ };
+
+ /**
+ * @brief ArticulatedObjectAttachment describes a fixed transformation between an agent and an articulated object.
+ *
+ * The transformation is defined as follows:
+ *
+ * agent.frame -> object.frame
+ *
+ */
+ struct ArticulatedObjectAttachment
+ {
+ AgentDescription agent;
+
+ Eigen::Affine3f transformation;
+
+ AgentDescription object;
+
+ armem::Time timestamp;
+
+ bool active;
+
+ };
+
+} // namespace armarx::armem::attachment
namespace armarx::armem::articulated_object
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 2eead70ac6845aedad5d4f1e5c69e9c0d8522347..6484c97ca0695848fc92dcb34008fc5b1d7c70f9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -43,6 +43,7 @@ armarx_add_library(
./aron_conversions.h
+ ./utils.h
SOURCES
./common/localization/TransformHelper.cpp
@@ -64,6 +65,8 @@ armarx_add_library(
./server/description/Segment.cpp
./aron_conversions.cpp
+ ./utils.cpp
+
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.cpp b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f4b84fdfbee87a4ca768837aa41039f1970115e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
@@ -0,0 +1,11 @@
+#include "utils.h"
+
+namespace armarx::armem::robot_state
+{
+ armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc)
+ {
+ return MemoryID("RobotState/Description")
+ .withProviderSegmentName(desc.name)
+ .withEntityName("description");
+ }
+}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.h b/source/RobotAPI/libraries/armem_robot_state/utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..239e0ddb2d1b99a513ed97a3e1b8fb63122a4c70
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+
+namespace armarx::armem::robot_state
+{
+ armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc);
+}
+