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

@@ -17,11 +17,10 @@ namespace armarx::skills::provider
     IncompleteSkill::GetSkillDescription()
     {
         auto d = HelloWorldSkill::GetSkillDescription();
-        return SkillDescription{{"IncompleteSkill"},
-                                d.description,
-                                {},
-                                d.timeout + armarx::core::time::Duration::Seconds(2),
-                                d.parametersType};
+        return SkillDescription{.skillId = {.skillName = "IncompleteSkill"},
+                                .description = d.description,
+                                .timeout = d.timeout + armarx::core::time::Duration::Seconds(2),
+                                .parametersType = d.parametersType};
     }
 
     Skill::PrepareResult
@@ -49,7 +48,6 @@ namespace armarx::skills::provider
     IncompleteSkill::main(const MainInput& in)
     {
         auto s = HelloWorldSkill();
-        s.setParameters(in.parameters);
         return s.mainOfSkill();
     }
 } // namespace armarx::skills::provider

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

@@ -30,11 +30,10 @@ namespace armarx::skills::provider
 
         // Add another lambda example skill
         {
-            skills::SkillDescription fooDesc({"Foo"},
-                                             "This skill does exactly nothing.",
-                                             nullptr,
-                                             armarx::core::time::Duration::MilliSeconds(1000),
-                                             nullptr);
+            skills::SkillDescription fooDesc{.skillId = SkillID{.skillName = "Foo"},
+                                             .description = "This skill does exactly nothing.",
+                                             .timeout =
+                                                 armarx::core::time::Duration::MilliSeconds(1000)};
             addSkillFactory(fooDesc,
                             []()
                             {

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

@@ -12,11 +12,9 @@ namespace armarx::skills::provider
     SkillDescription
     TimeoutSkill::GetSkillDescription()
     {
-        return SkillDescription{{"Timeout"},
-                                "This fails with timeout reached",
-                                {},
-                                armarx::core::time::Duration::MilliSeconds(2000),
-                                nullptr};
+        return SkillDescription{.skillId = SkillID{.skillName = "Timeout"},
+                                .description = "This fails with timeout reached",
+                                .timeout = armarx::core::time::Duration::MilliSeconds(2000)};
     }
 
     PeriodicSkill::StepResult

source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml

@@ -22,6 +22,9 @@
             <ObjectChild key='some_text'>
                 <String />
             </ObjectChild>
+            <ObjectChild key='some_optional_text'>
+                <String optional="True" />
+            </ObjectChild>
 
             <ObjectChild key='some_list_of_matrices'>
                 <List>

source/RobotAPI/components/units/GraspCandidateObserver.cpp

@@ -24,45 +24,51 @@
 #include "GraspCandidateObserver.h"
 
 //#include <ArmarXCore/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
 #include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
 #include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
 #include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+
 #include <RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
 #include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 #define TCP_POSE_CHANNEL "TCPPose"
 #define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
 using namespace armarx;
 using namespace armarx::grasping;
 
-GraspCandidateObserver::GraspCandidateObserver() : graspCandidateWriter(memoryNameSystem())
+GraspCandidateObserver::GraspCandidateObserver()
 {
 }
 
-void GraspCandidateObserver::onInitObserver()
+void
+GraspCandidateObserver::onInitObserver()
 {
     usingTopic(getProperty<std::string>("GraspCandidatesTopicName").getValue());
     offeringTopic(getProperty<std::string>("ConfigTopicName").getValue());
-
-
 }
 
-void GraspCandidateObserver::onConnectObserver()
+void
+GraspCandidateObserver::onConnectObserver()
 {
-    configTopic = getTopic<GraspCandidateProviderInterfacePrx>(getProperty<std::string>("ConfigTopicName").getValue());
-    graspCandidateWriter.connect();
+    configTopic = getTopic<GraspCandidateProviderInterfacePrx>(
+        getProperty<std::string>("ConfigTopicName").getValue());
+    graspCandidateWriter.connect(memoryNameSystem());
 }
 
-PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
+PropertyDefinitionsPtr
+GraspCandidateObserver::createPropertyDefinitions()
 {
-    return PropertyDefinitionsPtr(new GraspCandidateObserverPropertyDefinitions(
-                                      getConfigIdentifier()));
+    return PropertyDefinitionsPtr(
+        new GraspCandidateObserverPropertyDefinitions(getConfigIdentifier()));
 }
 
-bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter, const std::string& providerName, const GraspCandidatePtr& candidate)
+bool
+GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter,
+                                      const std::string& providerName,
+                                      const GraspCandidatePtr& candidate)
 {
     if (filter->providerName != "*" && filter->providerName != providerName)
     {
@@ -72,11 +78,13 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
     {
         return false;
     }
-    if (filter->approach != AnyApproach && (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
+    if (filter->approach != AnyApproach &&
+        (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
     {
         return false;
     }
-    if (filter->preshape != AnyAperture && (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
+    if (filter->preshape != AnyAperture &&
+        (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
     {
         return false;
     }
@@ -87,7 +95,8 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
     return true;
 }
 
-std::string GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
+std::string
+GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
 {
     switch (type)
     {
@@ -102,7 +111,8 @@ std::string GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
     }
 }
 
-void GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
+void
+GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
 {
     if (updateCounters.count(providerName) == 0)
     {
@@ -118,26 +128,40 @@ void GraspCandidateObserver::handleProviderUpdate(const std::string& providerNam
     {
         offerChannel(providerName, "Channel of " + providerName);
     }
-    offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters[providerName]), "Counter that increases for each update");
-    offerOrUpdateDataField(providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
+    offerOrUpdateDataField(providerName,
+                           "updateCounter",
+                           Variant(updateCounters[providerName]),
+                           "Counter that increases for each update");
+    offerOrUpdateDataField(
+        providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
 }
 
-void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::reportGraspCandidates(const std::string& providerName,
+                                              const GraspCandidateSeq& candidates,
+                                              const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     this->candidates[providerName] = candidates;
-    graspCandidateWriter.commitGraspCandidateSeq(candidates, armarx::armem::Time::Now(), providerName);
+    graspCandidateWriter.commitGraspCandidateSeq(
+        candidates, armarx::armem::Time::Now(), providerName);
     handleProviderUpdate(providerName, candidates.size());
 }
 
-void GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName, const BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName,
+                                                      const BimanualGraspCandidateSeq& candidates,
+                                                      const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     this->bimanualCandidates[providerName] = candidates;
     handleProviderUpdate(providerName, candidates.size());
 }
 
-void GraspCandidateObserver::reportProviderInfo(const std::string& providerName, const ProviderInfoPtr& info, const Ice::Current&)
+void
+GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
+                                           const ProviderInfoPtr& info,
+                                           const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     providers[providerName] = info;
@@ -154,36 +178,37 @@ void GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
     offerOrUpdateDataField(providerName, "objectType", ObjectTypeToString(info->objectType), "");
 }
 
-InfoMap GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
+InfoMap
+GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     return providers;
 }
 
-StringSeq GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
+StringSeq
+GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     return getAvailableProviderNames();
 }
 
-
-
-ProviderInfoPtr GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
+ProviderInfoPtr
+GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     checkHasProvider(providerName);
     return providers[providerName];
 }
 
-bool GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
+bool
+GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
 {
     std::unique_lock lock(dataMutex);
     return hasProvider(providerName);
 }
 
-
-
-GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getAllCandidates(const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     GraspCandidateSeq all;
@@ -194,11 +219,16 @@ GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
     return all;
 }
 
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName, const Ice::Current& c)
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName,
+                                                const Ice::Current& c)
 {
     return getCandidatesByProviders(Ice::StringSeq{providerName});
 }
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByProviders(const Ice::StringSeq& providerNames, const Ice::Current& c)
+
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByProviders(const Ice::StringSeq& providerNames,
+                                                 const Ice::Current& c)
 {
     std::unique_lock lock(dataMutex);
     GraspCandidateSeq all;
@@ -213,7 +243,9 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByProviders(const Ice::St
     return all;
 }
 
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter, const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter,
+                                              const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     GraspCandidateSeq matching;
@@ -230,20 +262,26 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateF
     return matching;
 }
 
-Ice::Int GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
+Ice::Int
+GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName,
+                                                   const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     checkHasProvider(providerName);
     return updateCounters[providerName];
 }
 
-IntMap GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
+IntMap
+GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
 {
     std::unique_lock lock(dataMutex);
     return updateCounters;
 }
 
-bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&)
+bool
+GraspCandidateObserver::setProviderConfig(const std::string& providerName,
+                                          const StringVariantBaseMap& config,
+                                          const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     if (providers.count(providerName) == 0)
@@ -254,19 +292,23 @@ bool GraspCandidateObserver::setProviderConfig(const std::string& providerName,
     return true;
 }
 
-void GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates,
+                                              const Ice::Current&)
 {
     std::unique_lock lock(selectedCandidatesMutex);
     selectedCandidates = candidates;
 }
 
-GraspCandidateSeq GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
 {
     std::unique_lock lock(selectedCandidatesMutex);
     return selectedCandidates;
 }
 
-BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
+BimanualGraspCandidateSeq
+GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
     BimanualGraspCandidateSeq all;
@@ -277,19 +319,25 @@ BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const
     return all;
 }
 
-void GraspCandidateObserver::setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::setSelectedBimanualCandidates(
+    const grasping::BimanualGraspCandidateSeq& candidates,
+    const Ice::Current&)
 {
     std::unique_lock lock(selectedCandidatesMutex);
     selectedBimanualCandidates = candidates;
 }
 
-BimanualGraspCandidateSeq GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
+BimanualGraspCandidateSeq
+GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
 {
     std::unique_lock lock(selectedCandidatesMutex);
     return selectedBimanualCandidates;
 }
 
-void GraspCandidateObserver::clearCandidatesByProvider(const std::string& providerName, const Ice::Current&)
+void
+GraspCandidateObserver::clearCandidatesByProvider(const std::string& providerName,
+                                                  const Ice::Current&)
 {
     std::unique_lock lock(dataMutex);
 
@@ -299,18 +347,24 @@ void GraspCandidateObserver::clearCandidatesByProvider(const std::string& provid
     }
 }
 
-bool GraspCandidateObserver::hasProvider(const std::string& providerName)
+bool
+GraspCandidateObserver::hasProvider(const std::string& providerName)
 {
     return providers.count(providerName) > 0;
 }
-void GraspCandidateObserver::checkHasProvider(const std::string& providerName)
+
+void
+GraspCandidateObserver::checkHasProvider(const std::string& providerName)
 {
     if (!hasProvider(providerName))
     {
-        throw LocalException("Unknown provider name '") << providerName << "'. Available providers: " << getAvailableProviderNames();
+        throw LocalException("Unknown provider name '")
+            << providerName << "'. Available providers: " << getAvailableProviderNames();
     }
 }
-StringSeq GraspCandidateObserver::getAvailableProviderNames()
+
+StringSeq
+GraspCandidateObserver::getAvailableProviderNames()
 {
     StringSeq names;
     for (const auto& pair : providers)

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();

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

@@ -34,6 +34,7 @@
 #include "../Devices/SensorDevice.h"
 #include "../SensorValues/SensorValueBase.h"
 #include "HeterogenousContinuousContainer.h"
+#include "RtTiming.h"
 
 namespace armarx
 {
@@ -50,7 +51,7 @@ namespace armarx::detail
 {
     struct RtMessageLogEntryBase
     {
-        RtMessageLogEntryBase() : time{IceUtil::Time::now()}
+        RtMessageLogEntryBase() : time{armarx::rtNow()}
         {
         }
 

source/RobotAPI/components/units/RobotUnit/util/NonRtTiming.h

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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    ArmarX
+ * @author     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+#include <time.h>
+
+#include <IceUtil/Time.h>
+
+#include "RtTiming.h"
+
+namespace armarx
+{
+    inline IceUtil::Time
+    nonRtNow()
+    {
+        using namespace rt_timing::constants;
+
+        struct timespec ts;
+        clock_gettime(CLOCK_REALTIME, &ts);
+        return IceUtil::Time::microSeconds(ts.tv_sec * seconds2MicroSeconds +
+                                           ts.tv_nsec / nanoSeconds2MicroSeconds);
+    }
+
+    inline IceUtil::Time
+    mapRtTimestampToNonRtTimestamp(const IceUtil::Time& time_monotic_raw)
+    {
+        // This is the "real time" clock, i.e. NTP-synchronized and relative to epoch.
+        IceUtil::Time now_real_time = armarx::nonRtNow();
+        // This is not relative to epoch and not NTP-synchronized.
+        IceUtil::Time now_monotonic_raw = armarx::rtNow();
+
+        /*
+             * Assumption for small very small time deltas (i.e. "time" is close to "now"):
+             *
+             * time_real_time - now_real_time == time_monotic_raw - now_monotonic_raw
+             * =>
+             * time_real_time = time_monotic_raw - now_monotonic_raw + now_real_time
+             */
+        //
+        IceUtil::Time time_real_time = time_monotic_raw - now_monotonic_raw + now_real_time;
+        ARMARX_DEBUG << VAROUT(time_monotic_raw) << " vs. " << VAROUT(time_real_time);
+        return time_real_time;
+    }
+
+} // namespace armarx

source/RobotAPI/components/units/RobotUnit/util/RtTiming.h

@@ -23,19 +23,27 @@
  */
 #pragma once
 
-#include <chrono>
-//#include <ArmarXCore/core/time/TimeUtil.h>
-#include "ControlThreadOutputBuffer.h"
+#include <time.h>
+
+#include <IceUtil/Time.h>
 
 namespace armarx
 {
+    namespace rt_timing::constants
+    {
+        inline constexpr const std::int64_t seconds2MicroSeconds = 1e6;
+
+        static constexpr const std::int64_t nanoSeconds2MicroSeconds = 1000;
+    } // namespace rt_timing::constants
 
     inline IceUtil::Time
     rtNow()
     {
+        using namespace rt_timing::constants;
         struct timespec ts;
-        clock_gettime(CLOCK_MONOTONIC, &ts);
-        return IceUtil::Time::microSeconds(ts.tv_sec * 1e6 + ts.tv_nsec / 1000);
+        clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
+        return IceUtil::Time::microSeconds(ts.tv_sec * seconds2MicroSeconds +
+                                           ts.tv_nsec / nanoSeconds2MicroSeconds);
     }
 } // namespace armarx
 
@@ -43,16 +51,14 @@ namespace armarx
 //! \ingroup VirtualTime
 //! Prints duration with comment in front of it, yet only once per second.
 #define RT_TIMING_END_COMMENT(name, comment)                                                       \
-    ARMARX_RT_LOGF_INFO(                                                                           \
-        "%s - duration: %.3f ms", comment, (armarx::rtNow() - name).toMilliSecondsDouble())        \
-        .deactivateSpam(1);
+    printf("%s - duration: %.3f ms \n", comment, (armarx::rtNow() - name).toMilliSecondsDouble());
 //! \ingroup VirtualTime
 //! Prints duration
 #define RT_TIMING_END(name) RT_TIMING_END_COMMENT(name, #name)
 //! \ingroup VirtualTime
 //! Prints duration with comment in front of it if it took longer than threshold
 #define RT_TIMING_CEND_COMMENT(name, comment, thresholdMs)                                         \
-    if ((armarx::rtNow() - name).toMilliSeconds() >= thresholdMs)                                  \
+    if ((armarx::rtNow() - name).toMilliSecondsDouble() >= thresholdMs)                            \
     RT_TIMING_END_COMMENT(name, comment)
 //! \ingroup VirtualTime
 //! Prints duration if it took longer than thresholdMs

source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp

@@ -22,6 +22,7 @@
 
 #include "GamepadUnit.h"
 
+#include "ArmarXCore/core/logging/Logging.h"
 #include <ArmarXCore/util/CPPUtility/trace.h>
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
 #include <linux/joystick.h>
@@ -53,7 +54,7 @@ void GamepadUnit::onConnectComponent()
         ARMARX_TRACE;
         if (!dataTimestamp)
         {
-            ARMARX_INFO << "dataTimestamp is null, waiting for value";
+            ARMARX_INFO << deactivateSpam(1) << "dataTimestamp is null, waiting for value";
             return;
         }
         ARMARX_CHECK_NOT_NULL(dataTimestamp);

source/RobotAPI/drivers/GamepadUnit/Joystick.h

@@ -220,7 +220,7 @@ namespace armarx
             stop.type = EV_FF;
             stop.code = e.id;
             stop.value = 0;
-            const int stopStatus = write(fdEvent, static_cast<const void*>(&stop), sizeof(stop));
+            [[maybe_unused]] const int stopStatus = write(fdEvent, static_cast<const void*>(&stop), sizeof(stop));
 
 
             ret = ioctl(fdEvent, EVIOCRMFF, e.id);

source/RobotAPI/drivers/GamepadUnit/README.md

-# GamepadUnit
+# GamepadUnit  {#GamepadUnit}
 
 This component is used to control the robot with a gamepad.