From 0bf60a5dafe4bee15314bc7ea09d9d185ab66785 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 10 May 2021 08:25:37 +0200
Subject: [PATCH] armem_objects: attachments segment (first draft)
---
.../server/attachments/Segment.cpp | 127 ++++++++++++++++++
1 file changed, 127 insertions(+)
create mode 100644 source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
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 000000000..9dcea186b
--- /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/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/articulated_object_conversions.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::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+ {
+ std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+ 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 description = articulated_object::convertRobotDescription(entityInstance);
+
+ if (not description)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ continue;
+ }
+
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+ objects.emplace(armem::MemoryID(entity.id()), *description);
+ }
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Number of known articulated object classes: " << objects.size();
+
+ return objects;
+ }
+
+
+
+ // 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::articulated_object_class
--
GitLab