Add ClassSegment

source/RobotAPI/libraries/armem_objects/CMakeLists.txt

@@ -6,6 +6,8 @@ armarx_set_target("Library: ${LIB_NAME}")
 
 armarx_add_library(
     LIBS
+        # ArmarXCore
+        ArmarXCore
         # ArmarXGui
         ArmarXGuiComponentPlugins
         # RobotAPI
@@ -16,16 +18,22 @@ armarx_add_library(
     HEADERS
         aron_conversions.h
 
-        server/instance/SegmentAdapter.h
+        server/class/Segment.h
+        server/class/SegmentAdapter.h
+
         server/instance/Segment.h
+        server/instance/SegmentAdapter.h
         server/instance/Decay.h
         server/instance/RobotHeadMovement.h
         server/instance/Visu.h
     SOURCES
         aron_conversions.cpp
 
-        server/instance/SegmentAdapter.cpp
+        server/class/Segment.cpp
+        server/class/SegmentAdapter.cpp
+
         server/instance/Segment.cpp
+        server/instance/SegmentAdapter.cpp
         server/instance/Decay.cpp
         server/instance/RobotHeadMovement.cpp
         server/instance/Visu.cpp
@@ -35,6 +43,7 @@ armarx_enable_aron_file_generation_for_target(
     TARGET_NAME
         "${LIB_NAME}"
     ARON_FILES
+        aron/ObjectClass.xml
         aron/ObjectInstance.xml
 )
 

source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml

+<!--
+Core segment type of Object/Class.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/AxisAlignedBoundingBox.aron.generated.h>" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.aron.generated.h>" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.aron.generated.h>" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/PackagePath.aron.generated.h>" />
+        <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+    </CodeIncludes>
+    <AronIncludes>
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/AxisAlignedBoundingBox.xml>" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml>" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.xml>" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/PackagePath.xml>" />
+        <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+    </AronIncludes>
+    <GenerateTypes>
+
+        <Object name="armarx::armem::arondto::ObjectClass">
+
+            <ObjectChild key="id">
+                <armarx::arondto::ObjectID />
+            </ObjectChild>
+
+            <ObjectChild key="simoxXmlPath">
+                <armarx::arondto::PackagePath />
+            </ObjectChild>
+
+            <ObjectChild key="meshWrlPath">
+                <armarx::arondto::PackagePath />
+            </ObjectChild>
+
+            <ObjectChild key="meshObjPath">
+                <armarx::arondto::PackagePath />
+            </ObjectChild>
+
+            <ObjectChild key="aabb">
+                <simox::arondto::AxisAlignedBoundingBox />
+            </ObjectChild>
+            <ObjectChild key="oobb">
+                <simox::arondto::OrientedBox />
+            </ObjectChild>
+
+            <ObjectChild key="names">
+                <armarx::arondto::ObjectNames />
+            </ObjectChild>
+
+        </Object>
+
+    </GenerateTypes>
+</AronTypeDefinition>
+
+

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

+#include "Segment.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+        iceMemory(memoryToIceAdapter)
+    {
+    }
+
+    void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+    {
+        defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object clazz 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.memory);
+
+        coreSegment = &iceMemory.memory->addCoreSegment(p.coreSegmentName, arondto::ObjectClass::toInitialAronType());
+        coreSegment->setMaxHistorySize(p.maxHistorySize);
+    }
+
+
+    armem::CoreSegment& Segment::getCoreSegment()
+    {
+        ARMARX_CHECK_NOT_NULL(coreSegment);
+        return *coreSegment;
+    }
+
+    const armem::CoreSegment& Segment::getCoreSegment() const
+    {
+        ARMARX_CHECK_NOT_NULL(coreSegment);
+        return *coreSegment;
+    }
+
+
+    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);
+
+        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++;
+
+        group.setLabel("Data");
+        group.addChild(grid);
+    }
+
+    void Segment::RemoteGui::update(Segment& data)
+    {
+        if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+        {
+            data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+            if (data.coreSegment)
+            {
+                data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+            }
+        }
+    }
+
+}

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

+#pragma once
+
+#include <string>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+    class Segment : public armarx::Logging
+    {
+    public:
+
+
+    public:
+
+        Segment(armem::server::MemoryToIceAdapter& iceMemory);
+
+
+        void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+        void init();
+
+        armem::CoreSegment& getCoreSegment();
+        const armem::CoreSegment& getCoreSegment() const;
+
+
+    private:
+
+        armem::server::MemoryToIceAdapter& iceMemory;
+        armem::CoreSegment* coreSegment = nullptr;
+
+        ObjectFinder objectFinder;
+
+
+        struct Properties
+        {
+            std::string coreSegmentName = "Class";
+            long maxHistorySize = -1;
+        };
+        Properties p;
+
+
+    public:
+
+        struct RemoteGui
+        {
+            armarx::RemoteGui::Client::GroupBox group;
+
+            armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+            armarx::RemoteGui::Client::CheckBox infiniteHistory;
+
+            void setup(const Segment& data);
+            void update(Segment& data);
+        };
+
+    };
+
+}

source/RobotAPI/libraries/armem_objects/server/class/SegmentAdapter.cpp

+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::armem_objects::SegmentAdapter
+ * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#if 0
+
+#include "SegmentAdapter.h"
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <VirtualRobot/Robot.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+    SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex) :
+        segment(iceMemory),
+        memoryMutex(memoryMutex)
+    {
+    }
+
+    std::string SegmentAdapter::getName() const
+    {
+        return Logging::tag.tagName;
+    }
+
+    void SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+    {
+        calibration.defineProperties(defs, prefix + "calibration.");
+        segment.defineProperties(defs, prefix);
+        robotHead.defineProperties(defs, prefix + "head.");
+        visu.defineProperties(defs, prefix + "visu.");
+    }
+
+    void SegmentAdapter::init()
+    {
+        segment.setTag(getName());
+        segment.decay.setTag(getName());
+        robotHead.setTag(getName());
+        visu.setTag(getName());
+
+        segment.init();
+    }
+
+    void SegmentAdapter::connect(
+        RobotStateComponentInterfacePrx robotStateComponent,
+        VirtualRobot::RobotPtr robot,
+        KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+        viz::Client arviz,
+        DebugObserverInterfacePrx debugObserver
+    )
+    {
+        this->debugObserver = debugObserver;
+        this->arviz = arviz;
+
+        segment.robot = robot;
+        segment.robotStateComponent = robotStateComponent;
+
+        robotHead.kinematicUnitObserver = kinematicUnitObserver;
+        robotHead.debugObserver = debugObserver;
+        robotHead.fetchDatafields();
+
+        visu.arviz = arviz;
+        if (!visu.updateTask)
+        {
+            visu.updateTask = new SimpleRunningTask<>([this]()
+            {
+                this->visualizeRun();
+            });
+            visu.updateTask->start();
+        }
+    }
+
+    void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
+    {
+        updateProviderInfo(providerName, info);
+    }
+
+
+    void SegmentAdapter::reportObjectPoses(
+        const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
+    {
+        ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
+        updateObjectPoses(providerName, providedPoses);
+    }
+
+
+    void SegmentAdapter::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
+    {
+        if (!info.proxy)
+        {
+            ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
+                           << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
+            return;
+        }
+        {
+            std::scoped_lock lock(memoryMutex);
+            std::stringstream ss;
+            for (const auto& id : info.supportedObjects)
+            {
+                ss << "- " << id << "\n";
+            }
+            ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
+                           << "Supported objects: \n" << ss.str();
+            segment.providers[providerName] = info;
+        }
+    }
+
+
+    void SegmentAdapter::updateObjectPoses(
+        const std::string& providerName,
+        const objpose::data::ProvidedObjectPoseSeq& providedPoses)
+    {
+        TIMING_START(tReportObjectPoses);
+
+        RobotHeadMovement::Discard discard;
+        {
+            std::scoped_lock lock(robotHeadMutex);
+            discard = robotHead.getDiscard();
+        }
+        if (debugObserver)
+        {
+            StringVariantBaseMap map;
+            map["Discarding All Updates"] = new Variant(discard.all ? 1.f : 0.f);
+            if (discard.all)
+            {
+                map["Proportion Updated Poses"] = new Variant(0.f);
+            }
+            debugObserver->setDebugChannel(getName(), map);
+        }
+
+        if (discard.all)
+        {
+            return;
+        }
+
+        {
+            std::scoped_lock lock(memoryMutex);
+            RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
+
+            if (segment.robot->hasRobotNode(calibration.robotNode))
+            {
+                VirtualRobot::RobotNodePtr robotNode = segment.robot->getRobotNode(calibration.robotNode);
+                float value = robotNode->getJointValue();
+                robotNode->setJointValue(value + calibration.offset);
+            }
+
+            TIMING_START(tCommitObjectPoses);
+            Segment::CommitStats stats =
+                segment.commitObjectPoses(providerName, providedPoses, discard.updatesUntil);
+            TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
+
+            if (debugObserver)
+            {
+                debugObserver->setDebugChannel(getName(),
+                {
+                    { "Discarding All Updates", new Variant(discard.all ? 1 : 0) },
+                    { "Proportion Updated Poses", new Variant(static_cast<float>(stats.numUpdated) / providedPoses.size()) }
+                });
+            }
+
+            handleProviderUpdate(providerName);
+
+            TIMING_END_STREAM(tReportObjectPoses, ARMARX_VERBOSE);
+            if (debugObserver)
+            {
+                debugObserver->setDebugChannel(getName(),
+                {
+                    { "t ReportObjectPoses [ms]", new Variant(tReportObjectPoses.toMilliSecondsDouble()) },
+                    { "t MemorySetObjectPoses [ms]", new Variant(tCommitObjectPoses.toMilliSecondsDouble()) },
+                });
+            }
+        }
+    }
+
+
+    void SegmentAdapter::handleProviderUpdate(const std::string& providerName)
+    {
+        // Initialized to 0 on first access.
+        if (segment.providers.count(providerName) == 0)
+        {
+            segment.providers[providerName] = objpose::ProviderInfo();
+        }
+    }
+
+
+    objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPoses(const Ice::Current&)
+    {
+        TIMING_START(tGetObjectPoses);
+
+        TIMING_START(tGetObjectPosesLock);
+        std::scoped_lock lock(memoryMutex);
+        TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
+
+        const IceUtil::Time now = TimeUtil::GetTime();
+        const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPoses(now));
+
+        TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
+
+        if (debugObserver)
+        {
+            debugObserver->setDebugChannel(getName(),
+            {
+                { "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
+                { "t GetObjectPoses() lock [ms]", new Variant(tGetObjectPosesLock.toMilliSecondsDouble()) }
+            });
+        }
+
+        return result;
+    }
+
+    objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
+    {
+        TIMING_START(GetObjectPoses);
+
+        TIMING_START(GetObjectPosesLock);
+        std::scoped_lock lock(memoryMutex);
+        TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
+
+        const IceUtil::Time now = TimeUtil::GetTime();
+        const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPosesByProvider(providerName, now));
+
+        TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
+
+        if (debugObserver)
+        {
+            debugObserver->setDebugChannel(getName(),
+            {
+                { "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
+                { "t GetObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
+            });
+        }
+
+        return result;
+    }
+
+
+    objpose::observer::RequestObjectsOutput SegmentAdapter::requestObjects(
+        const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
+    {
+        std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
+        std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
+
+        objpose::observer::RequestObjectsOutput output;
+
+        auto updateProxy = [&](const std::string & providerName)
+        {
+            if (proxies.count(providerName) == 0)
+            {
+                if (auto it = segment.providers.find(providerName); it != segment.providers.end())
+                {
+                    proxies[providerName] = it->second.proxy;
+                }
+                else
+                {
+                    ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
+                    proxies[providerName] = nullptr;
+                }
+            }
+        };
+
+        if (input.provider.size() > 0)
+        {
+            providerRequests[input.provider] = input.request;
+            updateProxy(input.provider);
+        }
+        else
+        {
+            std::scoped_lock lock(memoryMutex);
+            for (const auto& objectID : input.request.objectIDs)
+            {
+                bool found = true;
+                for (const auto& [providerName, info] : segment.providers)
+                {
+                    // ToDo: optimize look up.
+                    if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+                    {
+                        providerRequests[providerName].objectIDs.push_back(objectID);
+                        updateProxy(providerName);
+                        break;
+                    }
+                }
+                if (!found)
+                {
+                    ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
+                    output.results[objectID].providerName = "";
+                }
+            }
+        }
+
+        for (const auto& [providerName, request] : providerRequests)
+        {
+            if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
+            {
+                ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
+                            << providerName << "' for " << request.relativeTimeoutMS << " ms.";
+                objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
+
+                int successful = 0;
+                for (const auto& [objectID, result] : providerOutput.results)
+                {
+                    objpose::observer::ObjectRequestResult& res = output.results[objectID];
+                    res.providerName = providerName;
+                    res.result = result;
+                    successful += int(result.success);
+                }
+                ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
+            }
+        }
+        return output;
+    }
+
+    objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
+    {
+        std::scoped_lock lock(memoryMutex);
+        return segment.providers;
+    }
+
+    Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
+    {
+        std::scoped_lock lock(memoryMutex);
+        return simox::alg::get_keys(segment.providers);
+    }
+
+    objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
+    {
+        std::scoped_lock lock(memoryMutex);
+        return segment.getProviderInfo(providerName);
+    }
+
+    bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
+    {
+        std::scoped_lock lock(memoryMutex);
+        return segment.providers.count(providerName) > 0;
+    }
+
+
+    objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
+        const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
+    {
+        std::scoped_lock lock(memoryMutex);
+        return segment.attachObjectToRobotNode(input);
+    }
+
+    objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
+        const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
+    {
+        std::scoped_lock lock(memoryMutex);
+        return segment.detachObjectFromRobotNode(input);
+    }
+
+    objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
+        const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
+    {
+        std::scoped_lock lock(memoryMutex);
+        return segment.detachAllObjectsFromRobotNodes(input);
+    }
+
+
+    objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
+    {
+        std::scoped_lock lock(memoryMutex);
+
+        objpose::AgentFramesSeq output;
+        std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
+        for (VirtualRobot::RobotPtr agent : agents)
+        {
+            objpose::AgentFrames& frames = output.emplace_back();
+            frames.agent = agent->getName();
+            frames.frames = agent->getRobotNodeNames();
+        }
+        return output;
+    }
+
+    objpose::SignalHeadMovementOutput
+    SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
+    {
+        std::scoped_lock lock(robotHeadMutex);
+        return robotHead.signalHeadMovement(input);
+    }
+
+
+    void SegmentAdapter::visualizeRun()
+    {
+        CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
+        while (visu.updateTask && !visu.updateTask->isStopped())
+        {
+            {
+                std::scoped_lock lock(visuMutex);
+
+                if (visu.enabled)
+                {
+                    TIMING_START(Visu);
+
+                    objpose::ObjectPoseSeq objectPoses;
+                    ObjectFinder objectFinder;
+                    visu.minConfidence = -1;
+                    {
+                        std::scoped_lock lock(memoryMutex);
+
+                        const IceUtil::Time now = TimeUtil::GetTime();
+                        objectPoses = segment.getObjectPoses(now);
+                        objectFinder = segment.objectFinder;
+                        if (segment.decay.enabled)
+                        {
+                            visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
+                        }
+                    }
+                    const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
+                    arviz.commit(layers);
+
+                    TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+                    if (debugObserver)
+                    {
+                        debugObserver->setDebugChannel(getName(),
+                        {
+                            { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+                        });
+                    }
+                }
+            }
+            cycle.waitForCycleDuration();
+        }
+    }
+
+
+    void SegmentAdapter::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+    {
+        defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
+        defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
+    }
+
+
+
+    void SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
+    {
+        using namespace armarx::RemoteGui::Client;
+
+        this->visu.setup(adapter.visu);
+        this->segment.setup(adapter.segment);
+        this->decay.setup(adapter.segment.decay);
+        this->robotHead.setup(adapter.robotHead);
+
+        layout = VBoxLayout
+        {
+            this->visu.group, this->segment.group, this->decay.group, this->robotHead.group,
+            VSpacer()
+        };
+
+        group.setLabel("clazz");
+        group.addChild(layout);
+    }
+
+    void SegmentAdapter::RemoteGui::update(SegmentAdapter& adapter)
+    {
+        // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
+        {
+            std::scoped_lock lock(adapter.visuMutex);
+            this->visu.update(adapter.visu);
+        }
+        {
+            std::scoped_lock lock(adapter.memoryMutex);
+            this->segment.update(adapter.segment);
+            this->decay.update(adapter.segment.decay);
+        }
+        {
+            std::scoped_lock lock(adapter.robotHeadMutex);
+            this->robotHead.update(adapter.robotHead);
+        }
+    }
+
+}
+
+#endif

source/RobotAPI/libraries/armem_objects/server/class/SegmentAdapter.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/>.
+ *
+ * @package    RobotAPI::armem_objects::Adapter
+ * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#if 0
+#include <mutex>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/clazz/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/clazz/Decay.h>
+#include <RobotAPI/libraries/armem_objects/server/clazz/Visu.h>
+#include <RobotAPI/libraries/armem_objects/server/clazz/RobotHeadMovement.h>
+
+
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+    /**
+     * @brief Helps implementing the `armarx::armem::server::ObjectclazzSegmentInterface`.
+     */
+    class SegmentAdapter :
+        virtual public armarx::Logging
+        , virtual public armarx::armem::server::ObjectclazzSegmentInterface
+    {
+    public:
+
+        SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+
+        std::string getName() const;
+        void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+        void init();
+        void connect(
+            RobotStateComponentInterfacePrx robotStateComponent,
+            VirtualRobot::RobotPtr robot,
+            KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+            viz::Client arviz,
+            DebugObserverInterfacePrx debugObserver
+        );
+
+
+        // ObjectPoseTopic interface
+    public:
+        virtual void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
+        virtual void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
+
+        // ObjectclazzSegmentInterface interface
+    public:
+
+        // OBJECT POSES
+
+        virtual objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
+        virtual objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+
+        // PROVIDER INFORMATION
+
+        virtual bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+        virtual objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
+        virtual Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
+        virtual objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
+
+
+        // REQUESTING
+
+        virtual objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
+
+        // ATTACHING
+
+        virtual objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
+        virtual objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
+        virtual objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input, ICE_CURRENT_ARG) override;
+
+        virtual objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
+
+        // HEAD MOVEMENT SIGNALS
+
+        virtual objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
+
+
+    private:
+
+        void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
+
+        void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
+        void handleProviderUpdate(const std::string& providerName);
+
+
+        // Visualization
+
+        void visualizeRun();
+
+
+    private:
+
+        viz::Client arviz;
+        DebugObserverInterfacePrx debugObserver;
+
+        clazz::Segment segment;
+        std::mutex& memoryMutex;
+
+        clazz::RobotHeadMovement robotHead;
+        std::mutex robotHeadMutex;
+
+        clazz::Visu visu;
+        std::mutex visuMutex;
+
+
+        struct Calibration
+        {
+            std::string robotNode = "Neck_2_Pitch";
+            float offset = 0.0f;
+
+            void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
+        };
+        Calibration calibration;
+
+
+    public:
+
+        struct RemoteGui
+        {
+            armarx::RemoteGui::Client::GroupBox group;
+            armarx::RemoteGui::Client::VBoxLayout layout;
+
+            clazz::Visu::RemoteGui visu;
+            clazz::Segment::RemoteGui segment;
+            clazz::Decay::RemoteGui decay;
+            clazz::RobotHeadMovement::RemoteGui robotHead;
+
+            void setup(const SegmentAdapter& adapter);
+            void update(SegmentAdapter& adapter);
+        };
+
+    };
+
+}
+
+#undef ICE_CURRENT_ARG
+#endif