object memory: attachments segment

source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h

@@ -113,6 +113,7 @@ namespace armarx::armem::server::obj
         articulated_object_class::Segment articulatedObjectClassSegment;
         articulated_object_instance::Segment articulatedObjectInstanceSegment;
 
+        // associations::Segment associationsSegment;
 
         struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
         {

source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp

+#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_INFO << "Key is " << armem::MemoryID(entity.id());
+
+                objects.emplace(armem::MemoryID(entity.id()), *description);
+            }
+        }
+
+        ARMARX_IMPORTANT << "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

source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h

+/*
+ * 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_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<Attachments> getAttachments() const;
+
+
+    private:
+
+        server::MemoryToIceAdapter& iceMemory;
+        wm::CoreSegment* coreSegment = nullptr;
+        std::mutex& memoryMutex;
+
+        struct Properties
+        {
+            std::string coreClassSegmentName = "ArticulatedObjectClass";
+            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::articulated_object_class