source/RobotAPI/components/RobotDefinition/RobotDefinition.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::ArmarXObjects::RobotNameService
+ * @author     [Author Name] ( [Author Email] )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <mutex>
+#include <string>
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/robot_name_service/RobotNameServiceInterface.h>
+#include <RobotAPI/libraries/robot_name_service/client/Plugin.h>
+#include <RobotAPI/libraries/robot_name_service/core/Robot.h>
+
+namespace armarx
+{
+    /**
+     * @defgroup Component-RobotNameService RobotNameService
+     * @ingroup RobotAPI-Components
+     * A description of the component RobotNameService.
+     *
+     * @class RobotNameService
+     * @ingroup Component-RobotNameService
+     * @brief Brief description of class RobotNameService.
+     *
+     * Detailed description of class RobotNameService.
+     */
+    class RobotDefinition :
+        virtual public armarx::Component,
+        virtual public armarx::RobotNameServiceComponentPluginUser
+    {
+    public:
+        /**
+         * @see armarx::ManagedIceObject::getDefaultName()
+         */
+        std::string
+        getDefaultName() const override
+        {
+            return "RobotDefinition";
+        }
+
+    protected:
+        /**
+         * @see armarx::ManagedIceObject::onInitComponent()
+         */
+        void onInitComponent() override;
+
+        /**
+         * @see armarx::ManagedIceObject::onConnectComponent()
+         */
+        void onConnectComponent() override;
+
+        /**
+         * @see armarx::ManagedIceObject::onDisconnectComponent()
+         */
+        void onDisconnectComponent() override;
+
+        /**
+         * @see armarx::ManagedIceObject::onExitComponent()
+         */
+        void onExitComponent() override;
+
+        /**
+         * @see PropertyUser::createPropertyDefinitions()
+         */
+        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+    };
+} // namespace armarx

source/RobotAPI/components/RobotDefinition/test/CMakeLists.txt

+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore RobotNameService)
+ 
+armarx_add_test(RobotNameServiceTest RobotNameServiceTest.cpp "${LIBS}")
\ No newline at end of file

source/RobotAPI/interface/components/RobotNameServiceInterface.ice → source/RobotAPI/components/RobotDefinition/test/RobotNameServiceTest.cpp

@@ -13,19 +13,25 @@
  * 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     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @package    RobotAPI::ArmarXObjects::RobotNameService
+ * @author     [Author Name] ( [Author Email] )
  * @date       2018
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
 
-#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::RobotNameService
 
-#pragma once
+#define ARMARX_BOOST_TEST
 
-module armarx
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/RobotNameService/RobotNameService.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
 {
-    interface RobotNameServiceInterface
-    {
-    };
-};
+    armarx::RobotNameService instance;
+
+    BOOST_CHECK_EQUAL(true, true);
+}

source/RobotAPI/components/RobotNameService/CMakeLists.txt

 armarx_component_set_name("RobotNameService")
-set(COMPONENT_LIBS ArmarXCore)
-set(SOURCES RobotNameService.cpp)
-set(HEADERS RobotNameService.h)
-armarx_add_component("${SOURCES}" "${HEADERS}")
 
-# add unit tests
-add_subdirectory(test)
+set(COMPONENT_LIBS
+    ArmarXCore
+    ArmarXCoreInterfaces  # for DebugObserverInterface
+    ArmarXGuiComponentPlugins
+    RobotAPICore
+    RobotAPIInterfaces
+    RobotAPISkills
+    armem
+    robot_name_service_core
+    robot_name_service_server
+)
+
+set(SOURCES
+    RobotNameService.cpp
+)
+
+set(HEADERS
+    RobotNameService.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
 
-armarx_generate_and_add_component_executable(APPLICATION_APP_SUFFIX)
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx")

source/RobotAPI/components/RobotNameService/RobotNameService.cpp

@@ -22,35 +22,76 @@
 
 #include "RobotNameService.h"
 
-
 namespace armarx
 {
-    void RobotNameService::onInitComponent()
+    void
+    RobotNameService::onInitComponent()
     {
-
     }
 
-
-    void RobotNameService::onConnectComponent()
+    void
+    RobotNameService::onConnectComponent()
     {
+    }
 
+    void
+    RobotNameService::onDisconnectComponent()
+    {
     }
 
+    void
+    RobotNameService::onExitComponent()
+    {
+    }
 
-    void RobotNameService::onDisconnectComponent()
+    armarx::PropertyDefinitionsPtr
+    RobotNameService::createPropertyDefinitions()
     {
+        armarx::PropertyDefinitionsPtr defs =
+            new ComponentPropertyDefinitions(getConfigIdentifier());
 
+        return defs;
     }
 
+    bool
+    RobotNameService::registerRobotInfo(const robot_name_service::dto::RobotInfo& robot,
+                                        const Ice::Current& current)
+    {
+        std::scoped_lock l(robotsMutex);
+        ARMARX_INFO << "Register a new robot with name '" << robot.name << "' in RNS";
+
+        if (auto it = robots.find(robot.name); it != robots.end())
+        {
+            ARMARX_ERROR << "The robot with name '" << robot.name
+                         << "' is already registered. Ignoring it.";
+            return false;
+        }
+
+        robots[robot.name].fromIce(robot);
+        return true;
+    }
 
-    void RobotNameService::onExitComponent()
+    void
+    RobotNameService::unregisterRobotInfo(const std::string& name, const Ice::Current& current)
     {
+        std::scoped_lock l(robotsMutex);
 
+        if (auto it = robots.find(name); it != robots.end())
+        {
+            robots.erase(it);
+        }
     }
 
-    armarx::PropertyDefinitionsPtr RobotNameService::createPropertyDefinitions()
+    IceUtil::Optional<robot_name_service::dto::RobotInfo>
+    RobotNameService::getRobotInfo(const std::string& name, const Ice::Current& current)
     {
-        return armarx::PropertyDefinitionsPtr(new RobotNameServicePropertyDefinitions(
-                getConfigIdentifier()));
+        std::scoped_lock l(robotsMutex);
+
+        if (auto it = robots.find(name); it == robots.end())
+        {
+            return {};
+        }
+
+        return robots[name].toIce();
     }
-}
+} // namespace armarx

source/RobotAPI/components/RobotNameService/RobotNameService.h

@@ -22,27 +22,17 @@
 
 #pragma once
 
+#include <map>
+#include <mutex>
+#include <string>
 
 #include <ArmarXCore/core/Component.h>
 
+#include <RobotAPI/interface/robot_name_service/RobotNameServiceInterface.h>
+#include <RobotAPI/libraries/robot_name_service/core/Robot.h>
+
 namespace armarx
 {
-    /**
-     * @class RobotNameServicePropertyDefinitions
-     * @brief
-     */
-    class RobotNameServicePropertyDefinitions:
-        public armarx::ComponentPropertyDefinitions
-    {
-    public:
-        RobotNameServicePropertyDefinitions(std::string prefix):
-            armarx::ComponentPropertyDefinitions(prefix)
-        {
-            //defineRequiredProperty<std::string>("PropertyName", "Description");
-            //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
-        }
-    };
-
     /**
      * @defgroup Component-RobotNameService RobotNameService
      * @ingroup RobotAPI-Components
@@ -55,13 +45,15 @@ namespace armarx
      * Detailed description of class RobotNameService.
      */
     class RobotNameService :
-        virtual public armarx::Component
+        virtual public armarx::Component,
+        virtual public armarx::robot_name_service::dti::RobotNameServiceInterface
     {
     public:
         /**
          * @see armarx::ManagedIceObject::getDefaultName()
          */
-        std::string getDefaultName() const override
+        std::string
+        getDefaultName() const override
         {
             return "RobotNameService";
         }
@@ -91,6 +83,18 @@ namespace armarx
          * @see PropertyUser::createPropertyDefinitions()
          */
         armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-    };
-}
 
+
+        // RobotNameServiceInterface interface
+    public:
+        bool registerRobotInfo(const robot_name_service::dto::RobotInfo& robot,
+                               const Ice::Current& current) override;
+        void unregisterRobotInfo(const std::string& name, const Ice::Current& current) override;
+        IceUtil::Optional<robot_name_service::dto::RobotInfo>
+        getRobotInfo(const std::string& name, const Ice::Current& current) override;
+
+    private:
+        mutable std::mutex robotsMutex;
+        std::map<std::string, robot_name_service::core::RobotInfo> robots;
+    };
+} // namespace armarx

source/RobotAPI/components/RobotState/RobotStateComponent.cpp

@@ -37,6 +37,8 @@
 #include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
+#include <RobotAPI/components/units/RobotUnit/util/NonRtTiming.h>
 
 
 using namespace Eigen;
@@ -296,10 +298,10 @@ namespace armarx
     {
         if (timestamp <= 0)
         {
-            timestamp = IceUtil::Time::now().toMicroSeconds();
+            timestamp = armarx::rtNow().toMicroSeconds();
         }
 
-        IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
+        IceUtil::Time time = mapRtTimestampToNonRtTimestamp(IceUtil::Time::microSeconds(timestamp));
 
         ARMARX_DEBUG << deactivateSpam(1) << "Got new jointangles: " << jointAngles
                      << " from timestamp " << time.toDateTime()

source/RobotAPI/components/armem/client/CMakeLists.txt

 add_subdirectory(ExampleMemoryClient)
 add_subdirectory(GraspProviderExample)
+add_subdirectory(MemoryToDebugObserver)
 add_subdirectory(ObjectInstanceToIndex)
 add_subdirectory(RobotStatePredictionClientExample)
 add_subdirectory(SimpleVirtualRobot)

source/RobotAPI/components/armem/client/MemoryToDebugObserver/CMakeLists.txt

+armarx_component_set_name(MemoryToDebugObserver)
+
+set(COMPONENT_LIBS
+    ArmarXCore
+    ArmarXCoreComponentPlugins
+    ArmarXGuiComponentPlugins
+    RobotAPICore RobotAPIInterfaces
+    armem
+)
+
+set(SOURCES
+    Component.cpp
+)
+
+set(HEADERS
+    Component.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+armarx_generate_and_add_component_executable(
+    COMPONENT_INCLUDE
+        "${CMAKE_CURRENT_LIST_DIR}/Component.h"
+    COMPONENT_NAMESPACE
+        armarx::memory_to_debug_observer
+    COMPONENT_CLASS_NAME
+        Component
+)

source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.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 "Component.h"
+
+#include <SimoxUtility/json.h>
+
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+namespace armarx::memory_to_debug_observer
+{
+
+    armarx::PropertyDefinitionsPtr
+    Component::createPropertyDefinitions()
+    {
+        armarx::PropertyDefinitionsPtr defs =
+            new ComponentPropertyDefinitions(getConfigIdentifier());
+
+        {
+            armem::MemoryID robotEntityId{"RobotState", "Proprioception", "Armar7", "Armar7"};
+
+            std::vector<std::string> jointNames{
+                "Neck_1_Yaw",
+                "Neck_2_Hemisphere_A",
+                "Neck_3_Hemisphere_B",
+            };
+
+            for (const std::string& jointName : jointNames)
+            {
+                std::vector<std::string> attributes{
+                    "position",
+                    "positionTarget",
+                    "relativePosition",
+                    "velocity",
+                    "velocityTarget",
+                    "torque",
+                    "motorCurrent",
+                    "currentTarget",
+                };
+                for (const std::string& attribute : attributes)
+                {
+                    properties.memoryToDebugObserver.plottedValues.push_back({
+                        .entityID = robotEntityId,
+                        .aronPath = {{"joints", attribute, jointName}},
+                    });
+                    properties.memoryToDebugObserver.plottedValues.push_back({
+                        .entityID = robotEntityId,
+                        .aronPath = {{"extraLongs", jointName + ".absoluteEncoderTicks"}},
+                    });
+                }
+            }
+
+            simox::json::json j = properties.memoryToDebugObserver;
+            properties.memoryToDebugObserverJson = j.dump();
+        }
+        defs->optional(properties.memoryToDebugObserverJson,
+                       "p.memoryToDebugObserverJson",
+                       "Configuration of MemoryToDebugObserver in JSON format.");
+
+        defs->optional(properties.pollFrequencyHz, "p.pollFrequencyHz");
+
+        return defs;
+    }
+
+    Component::Component()
+    {
+    }
+
+    std::string
+    Component::getDefaultName() const
+    {
+        return "MemoryToDebugObserver";
+    }
+
+    void
+    Component::onInitComponent()
+    {
+        DebugObserverComponentPluginUser::setDebugObserverBatchModeEnabled(true);
+
+        {
+            simox::json::json j = simox::json::json::parse(properties.memoryToDebugObserverJson);
+            j.get_to(properties.memoryToDebugObserver);
+        }
+    }
+
+    void
+    Component::onConnectComponent()
+    {
+        createRemoteGuiTab();
+        RemoteGui_startRunningTask();
+
+        task = new RunningTask<Component>(this, &Component::runLoop);
+        task->start();
+    }
+
+    void
+    Component::onDisconnectComponent()
+    {
+        task->stop();
+        task = nullptr;
+    }
+
+    void
+    Component::onExitComponent()
+    {
+    }
+
+    void
+    Component::runLoop()
+    {
+        armem::client::util::MemoryToDebugObserver::Services services{
+            .memoryNameSystem = memoryNameSystem(),
+            .debugObserver = getDebugObserverComponentPlugin(),
+        };
+        armem::client::util::MemoryToDebugObserver memoryToDebugObserver{
+            properties.memoryToDebugObserver, services};
+
+        Frequency frequency = Frequency::Hertz(properties.pollFrequencyHz);
+        Metronome metronome(frequency);
+        while (task and not task->isStopped())
+        {
+            memoryToDebugObserver.pollOnce();
+            metronome.waitForNextTick();
+        }
+    }
+
+    void
+    Component::createRemoteGuiTab()
+    {
+        using namespace armarx::RemoteGui::Client;
+
+        tab.group = GroupBox();
+        tab.group.setLabel("Todo ...");
+
+        VBoxLayout root = {tab.group, VSpacer()};
+        RemoteGui_createTab(getName(), root, &tab);
+    }
+
+    void
+    Component::RemoteGui_update()
+    {
+        if (tab.rebuild.exchange(false))
+        {
+            createRemoteGuiTab();
+        }
+    }
+
+} // namespace armarx::memory_to_debug_observer

source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.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::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
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <ArmarXCore/util/tasks.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h>
+
+namespace armarx::memory_to_debug_observer
+{
+
+    /**
+     * @defgroup Component-MemoryToDebugObserver MemoryToDebugObserver
+     * @ingroup RobotAPI-Components
+     *
+     * Transfers data from the memory system to the \ref Component-DebugObserver "Debug Observer",
+     * allowing to visualize them in the \ref ArmarXGui-GuiPlugins-PlotterPlugin "Live Plotter".
+     *
+     * @see armarx::armem::client::util::MemoryToDebugObserver for the business logic
+     * implementation.
+     *
+     * @class Component
+     * @ingroup Component-MemoryToDebugObserver
+     * @brief Implementation of \ref Component-MemoryToDebugObserver.
+     *
+     * @see armarx::armem::client::util::MemoryToDebugObserver for the business logic
+     * implementation.
+     */
+    class Component :
+        virtual public armarx::Component,
+        virtual public armarx::armem::ClientPluginUser,
+        virtual public armarx::DebugObserverComponentPluginUser,
+        virtual public armarx::LightweightRemoteGuiComponentPluginUser
+    {
+    public:
+        Component();
+
+        /// @see armarx::ManagedIceObject::getDefaultName()
+        std::string getDefaultName() const override;
+
+        // LightweightRemoteGuiComponentPluginUser interface
+    public:
+        void createRemoteGuiTab();
+        void RemoteGui_update() override;
+
+
+    protected:
+        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+        void onInitComponent() override;
+        void onConnectComponent() override;
+        void onDisconnectComponent() override;
+        void onExitComponent() override;
+
+        void runLoop();
+
+
+    private:
+        struct Properties
+        {
+            armem::client::util::MemoryToDebugObserver::Properties memoryToDebugObserver;
+            std::string memoryToDebugObserverJson;
+
+            float pollFrequencyHz = 30;
+        };
+
+        Properties properties;
+
+        armarx::RunningTask<Component>::pointer_type task;
+
+        struct RemoteGuiTab : RemoteGui::Client::Tab
+        {
+            std::atomic_bool rebuild = false;
+
+            RemoteGui::Client::GroupBox group;
+        };
+
+        RemoteGuiTab tab;
+    };
+} // namespace armarx::memory_to_debug_observer

source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp

@@ -29,7 +29,8 @@ namespace armarx::skills::provider
                                 .rootProfileDefaults = root_profile_params.toAron(),
                                 .timeout = armarx::core::time::Duration::MilliSeconds(1000),
                                 .parametersType =
-                                    armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
+                                    armarx::skills::Example::HelloWorldAcceptedType::ToAronType(),
+                                .resultType = armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
     }
 
     Skill::MainResult
@@ -42,6 +43,6 @@ namespace armarx::skills::provider
                                 .dump(2)
                          << "\n"
                          << "(executed at: " << IceUtil::Time::now() << ")";
-        return {TerminatedSkillStatus::Succeeded, nullptr};
+        return {TerminatedSkillStatus::Succeeded, in.parameters.toAron()};
     }
 } // namespace armarx::skills::provider

source/RobotAPI/components/units/RobotUnit/CMakeLists.txt

@@ -78,6 +78,7 @@ set(LIB_HEADERS
     util/introspection/ClassMemberInfo.h
     util/RtLogging.h
     util/RtTiming.h
+    util/NonRtTiming.h
     util/CtrlUtil.h
 
     #robot unit modules need to be added to the list below (but not here)

source/RobotAPI/components/units/RobotUnit/README.md

-# RobotUnit
+# RobotUnit  {#RobotUnit}
+
 The RobotUnit can be used for real-time control.
 The central principle is that all controllers are executed synchronously.
 The controllers are arranged in a 2-layer architecture.

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp

@@ -853,7 +853,7 @@ namespace armarx::RobotUnitModule
         }
 
         SensorAndControl& sc = _module<ControlThreadDataBuffer>().rtGetSensorAndControlBuffer();
-        sc.writeTimestamp = IceUtil::Time::now(); // this has to be in real time
+        sc.writeTimestamp = armarx::rtNow(); // this has to be in real time
         sc.sensorValuesTimestamp = sensorValuesTimestamp;
         sc.timeSinceLastIteration = timeSinceLastIteration;
         ARMARX_CHECK_EQUAL(rtGetSensorDevices().size(), sc.sensors.size());

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp

@@ -95,8 +95,11 @@ namespace armarx::RobotUnitModule
             std::stringstream ss;
             ss << "Requested controller class '" << className
                << "' unknown! Known classes:" << NJointControllerRegistry::getKeys()
-               << " (If this class exists in a different lib then load it via "
-                  "loadLibFromPath(path) or loadLibFromPackage(package, lib))";
+               << " (If this class exists in a different lib then load it in the property "
+                  "definitions of the RT-unit. DO NOT load it via "
+                  "loadLibFromPath(path) or loadLibFromPackage(package, lib)) (see "
+                  "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/documentation/-/"
+                  "issues/85)";
             ARMARX_ERROR << ss.str();
             throw InvalidArgumentException{ss.str()};
         }
@@ -303,6 +306,9 @@ namespace armarx::RobotUnitModule
     ControllerManagement::loadLibFromPath(const std::string& path, const Ice::Current&)
     {
         throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+        ARMARX_WARNING << "Do not use this function as it has implications on the RT thread (see "
+                        "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/"
+                        "documentation/-/issues/85)";
         const bool result = getArmarXManager()->loadLibFromPath(path);
         ARMARX_INFO << "loadLibFromPath('" << path << "') -> " << result;
         return result;
@@ -314,6 +320,9 @@ namespace armarx::RobotUnitModule
                                              const Ice::Current&)
     {
         throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+        ARMARX_WARNING << "Do not use this function as it has implications on the RT thread (see "
+                        "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/"
+                        "documentation/-/issues/85)";
         const bool result = getArmarXManager()->loadLibFromPackage(package, lib);
         ARMARX_INFO << "loadLibFromPackage('" << package << "', '" << lib << "') -> " << result;
         return result;

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp

@@ -35,6 +35,7 @@
 #include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
 #include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
 #include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/util/NonRtTiming.h>
 
 namespace armarx::RobotUnitModule
 {
@@ -255,7 +256,7 @@ namespace armarx::RobotUnitModule
     {
         ARMARX_TRACE;
         throwIfInControlThread(BOOST_CURRENT_FUNCTION);
-        const auto beg = TimeUtil::GetTime(true);
+        const auto beg = armarx::rtNow();
 
 
         StringVariantBaseMap ctrlDevMap;
@@ -314,7 +315,7 @@ namespace armarx::RobotUnitModule
             }
         }
 
-        const auto end = TimeUtil::GetTime(true);
+        const auto end = armarx::rtNow();
         return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
     }
 
@@ -476,6 +477,7 @@ namespace armarx::RobotUnitModule
             getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
         debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();
 
+        observerEnablePublishing = getProperty<bool>("ObserverEnablePublishing").getValue();
         observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
         observerPublishControlTargets =
             getProperty<bool>("ObserverPublishControlTargets").getValue();
@@ -575,7 +577,10 @@ namespace armarx::RobotUnitModule
             [&] { publish({}); }, publishPeriodMs, false, getName() + "_PublisherTask");
         ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
         publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
-        publisherTask->start();
+        if (observerEnablePublishing)
+        {
+            publisherTask->start();
+        }
     }
 
     void
@@ -628,7 +633,10 @@ namespace armarx::RobotUnitModule
         const auto requestedJointControllers =
             _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
 
-        lastControlThreadTimestamp = controlThreadOutputBuffer.sensorValuesTimestamp;
+        // controlThreadOutputBuffer.sensorValuesTimestamp is in MONOTONIC_RAW (not relative to epoch).
+        // We have to map it to be relative to epoch (REAL_TIME).
+        lastControlThreadTimestamp =
+            armarx::mapRtTimestampToNonRtTimestamp(controlThreadOutputBuffer.sensorValuesTimestamp);
 
         const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
         //publish publishing meta state

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h

@@ -58,6 +58,10 @@ namespace armarx::RobotUnitModule
             defineOptionalProperty<std::size_t>(
                 "PublishPeriodMs", 10, "Milliseconds between each publish");
 
+            defineOptionalProperty<bool>("ObserverEnablePublishing",
+                                         true,
+                                         "Whether the publishing thread is started or not",
+                                         PropertyDefinitionBase::eModifiable);
             defineOptionalProperty<bool>("ObserverPublishSensorValues",
                                          true,
                                          "Whether sensor values are send to the observer",
@@ -271,6 +275,8 @@ namespace armarx::RobotUnitModule
 
         /// @brief Whether \ref SensorValueBase "SensorValues" should be published to the observers
         std::atomic_bool observerPublishSensorValues;
+        /// @brief Whether the publishing thread should be started or not
+        std::atomic_bool observerEnablePublishing;
         /// @brief Whether \ref ControlTargetBase "ControlTargets" should be published to the observers
         std::atomic_bool observerPublishControlTargets;
         /// @brief Whether \ref Timing information should be published to the observers

source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp

@@ -362,7 +362,7 @@ namespace armarx
             const auto numExcessEntries =
                 std::max(requiredAdditionalEntries, numEntries - entries.size());
             const auto requiredSize = entries.size() + numExcessEntries;
-            ARMARX_WARNING << "Iteration " << iterationCount << " required "
+            ARMARX_VERBOSE << "Iteration " << iterationCount << " required "
                            << requiredAdditionalEntries << " | " << numExcessEntries
                            << " additional message entries. \n"
                            << "The requested total number of entries is " << requiredSize << ". \n"
@@ -371,7 +371,7 @@ namespace armarx
                            << getMaximalNumberOfBufferEntries();
             if (requiredSize > getMaximalNumberOfBufferEntries())
             {
-                ARMARX_WARNING << deactivateSpam(1, to_string(requiredSize)) << "Iteration "
+                ARMARX_VERBOSE << deactivateSpam(1, to_string(requiredSize)) << "Iteration "
                                << iterationCount << " would require " << requiredSize
                                << " message entries, but the maximal number of entries is "
                                << getMaximalNumberOfBufferEntries();