source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice

@@ -284,7 +284,9 @@ module armarx
             void deactivateAndDeleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
 
             //loading libs
+            ["deprecate:loadLibFromPath(string path) has dangerous implications on the RT thread. Use the scenario config instead to load additional libraries. See https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/documentation/-/issues/85"]
             bool loadLibFromPath(string path);
+            ["deprecate:loadLibFromPackage(string package, string libname) has dangerous implications on the RT thread. Use the scenario config instead to load additional libraries. See https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/documentation/-/issues/85"]
             bool loadLibFromPackage(string package, string libname);
         };
         interface RobotUnitSelfCollisionCheckerInterface

source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt

@@ -82,7 +82,6 @@ armarx_enable_aron_file_generation_for_target(
         "${LIB_NAME}"
     ARON_FILES
         aron/ObjectID.xml
-        aron/ObjectNames.xml
         aron/ObjectPose.xml
         aron/ObjectType.xml
         aron/PoseManifoldGaussian.xml

source/RobotAPI/libraries/armem/CMakeLists.txt

@@ -7,6 +7,7 @@ armarx_set_target("Library: ${LIB_NAME}")
 set(LIBS
     ArmarXCoreInterfaces
     ArmarXCore
+    DebugObserverHelper
     RemoteGui
     aron
     aroncommon
@@ -64,8 +65,9 @@ set(LIB_FILES
     client/plugins/PluginUser.cpp
     client/plugins/Plugin.cpp
 
-    client/util/SubscriptionHandle.cpp
     client/util/MemoryListener.cpp
+    client/util/MemoryToDebugObserver.cpp
+    client/util/SubscriptionHandle.cpp
     client/util/SimpleReaderBase.cpp
     client/util/SimpleWriterBase.cpp
 
@@ -157,10 +159,11 @@ set(LIB_HEADERS
     client/query/detail/NameSelectorOps.h
     client/query/detail/SelectorOps.h
 
-    client/util/SubscriptionHandle.h
+    client/util/MemoryToDebugObserver.h
     client/util/MemoryListener.h
     client/util/SimpleReaderBase.h
     client/util/SimpleWriterBase.h
+    client/util/SubscriptionHandle.h
 
     server.h
 

source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.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::ArmarXObjects::MemoryToDebugObserver
+ * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date       2023
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "MemoryToDebugObserver.h"
+
+#include <RobotAPI/libraries/armem/core/error/mns.h>
+#include <RobotAPI/libraries/armem/core/json_conversions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::armem::client::util
+{
+
+    MemoryToDebugObserver::MemoryToDebugObserver(const Properties& properties,
+                                                 const Services& services) :
+        properties(properties), services(services)
+    {
+        services.debugObserver.setDebugObserverBatchModeEnabled(true);
+    }
+
+    class Visitor : public aron::data::ConstVariantVisitor
+    {
+    public:
+        Visitor(armarx::DebugObserverHelper& debugObserver) : debugObserver{debugObserver}
+        {
+        }
+
+        // void visitAronVariant(const data::DictPtr&) override;
+        // void visitAronVariant(const data::ListPtr&) override;
+        // void visitAronVariant(const data::NDArrayPtr&) override;
+        void
+        visitAronVariant(const aron::data::IntPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::LongPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::FloatPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::DoublePtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::BoolPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::StringPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        template <class ValueT>
+        void
+        setDatafield(const ValueT& value)
+        {
+            debugObserver.setDebugObserverDatafield(channelName, datafieldName, value);
+            ++count;
+        }
+
+        armarx::DebugObserverHelper& debugObserver;
+        std::string channelName;
+        std::string datafieldName;
+
+        int count = 0;
+    };
+
+    void
+    MemoryToDebugObserver::pollOnce()
+    {
+        Visitor visitor(services.debugObserver);
+
+        std::stringstream log;
+
+        // Group by memory segment to reduce number of queries.
+        std::map<MemoryID, std::vector<const MemoryValueID*>> valuesPerProviderSegment;
+        for (const MemoryValueID& valueId : properties.plottedValues)
+        {
+            valuesPerProviderSegment[valueId.entityID.getProviderSegmentID()].push_back(&valueId);
+        }
+
+        for (const auto& [providerSegmentID, values] : valuesPerProviderSegment)
+        {
+            armem::client::Reader* reader = nullptr;
+            try
+            {
+                reader = getReader(providerSegmentID);
+            }
+            catch (armem::error::CouldNotResolveMemoryServer& e)
+            {
+                log << "\n" << e.what();
+                continue;
+            }
+            ARMARX_CHECK_NOT_NULL(reader);
+
+            const QueryResult result = reader->getLatestSnapshotsIn(providerSegmentID);
+            if (not result.success)
+            {
+                log << "Query to provider segment " << providerSegmentID
+                    << " failed: " << result.errorMessage;
+                continue;
+            }
+
+            for (const MemoryValueID* valueId : values)
+            {
+                const wm::Entity* entity = result.memory.findEntity(valueId->entityID);
+                if (entity == nullptr)
+                {
+                    log << "\nDid not find entity " << valueId->entityID << " in provider segment "
+                        << providerSegmentID << ".";
+                    continue;
+                }
+
+                const wm::EntityInstance& instance = entity->getLatestInstance();
+                aron::data::VariantPtr valueVariant =
+                    instance.data()->navigateAbsolute(valueId->aronPath);
+                if (not valueVariant)
+                {
+                    log << "\nDid not find " << valueId->aronPath.toString()
+                        << " in entity instance " << instance.id() << ".";
+                    continue;
+                }
+
+                visitor.channelName = makeChannelName(valueId->entityID);
+                visitor.datafieldName = makeDatafieldName(valueId->aronPath);
+
+                aron::data::visit(visitor, valueVariant);
+            }
+        }
+
+        services.debugObserver.sendDebugObserverBatch();
+
+        if (not log.str().empty())
+        {
+            ARMARX_INFO << deactivateSpam(60)
+                        << "Encountered issues while sending memory values to the debug observer "
+                           "for plotting: "
+                        << log.str();
+        }
+    }
+
+    std::string
+    MemoryToDebugObserver::makeChannelName(const MemoryID& memoryID)
+    {
+        return simox::alg::replace_all(memoryID.str(), "/", ">");
+    }
+
+    std::string
+    MemoryToDebugObserver::makeDatafieldName(const aron::Path& path)
+    {
+        // The first element is always "ARON->", which we can discard for readability.
+        std::string str = path.toString();
+        str = simox::alg::remove_prefix(str, path.getRootIdentifier() + path.getDelimeter());
+        str = simox::alg::replace_all(str, path.getDelimeter(), ">");
+        return str;
+    }
+
+    Reader*
+    MemoryToDebugObserver::getReader(const MemoryID& memoryID)
+    {
+        auto it = memoryReaders.find(memoryID);
+        if (it != memoryReaders.end())
+        {
+            return &it->second;
+        }
+        else
+        {
+            armem::client::Reader reader = services.memoryNameSystem.getReader(memoryID);
+
+            auto [it, _] = memoryReaders.emplace(memoryID, reader);
+            return &it->second;
+        }
+    }
+
+} // namespace armarx::armem::client::util
+
+namespace armarx::armem::client
+{
+
+    void
+    util::to_json(simox::json::json& j, const MemoryValueID& id)
+    {
+        j["entityID"] = id.entityID.getItems();
+        j["aronPath"] = id.aronPath.getPath();
+    }
+
+    void
+    util::from_json(const simox::json::json& j, MemoryValueID& id)
+    {
+        id.entityID = MemoryID::fromItems(j.at("entityID").get<std::vector<std::string>>());
+        id.aronPath = {j.at("aronPath").get<std::vector<std::string>>()};
+    }
+
+    void
+    util::to_json(simox::json::json& j, const MemoryToDebugObserver::Properties& p)
+    {
+        j["plottedValues"] = p.plottedValues;
+    }
+
+    void
+    util::from_json(const simox::json::json& j, MemoryToDebugObserver::Properties& p)
+    {
+        j.at("plottedValues").get_to(p.plottedValues);
+    }
+
+
+} // namespace armarx::armem::client

source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h

@@ -54,6 +54,7 @@ namespace armarx::armem::client::util
         };
 
         const Properties& properties() const;
+        void setProperties(const Properties& p);
 
         virtual std::string propertyPrefix() const = 0;
         virtual Properties defaultProperties() const = 0;
@@ -62,7 +63,7 @@ namespace armarx::armem::client::util
 
 
     private:
-        Properties props;
+        std::optional<Properties> props;
 
         armem::client::Writer memoryWriterClient;
     };