scenarios/memory_to_debug_observer/memory_to_debug_observer.scx

+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="memory_to_debug_observer" creation="1970-01-01.01:00:00" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+	<application name="MemoryToDebugObserver" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+

source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp

-#include <regex>
-#include <fstream>
-
 #include "VisualizationRobot.h"
 
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <fstream>
+#include <regex>
+
 #include <VirtualRobot/SceneObject.h>
-#include <VirtualRobot/XML/RobotIO.h>
 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/XML/RobotIO.h>
 
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
 namespace armarx::viz::coin
 {
     namespace
     {
-        VirtualRobot::RobotPtr loadRobot(std::string const& project, std::string const& filename)
+        VirtualRobot::RobotPtr
+        loadRobot(std::string const& project, std::string const& filename)
         {
-            VirtualRobot::RobotPtr  result;
+            VirtualRobot::RobotPtr result;
 
             if (filename.empty())
             {
@@ -32,7 +33,8 @@ namespace armarx::viz::coin
                 ARMARX_INFO << deactivateSpam()
                             << "You specified the absolute path to the robot file:"
                             << "\n\t'" << filename << "'"
-                            << "\nConsider specifying the containing ArmarX package and relative data path instead to "
+                            << "\nConsider specifying the containing ArmarX package and relative "
+                               "data path instead to "
                             << "improve portability to other systems.";
             }
             // We need to always check that the file is readable otherwise, VirtualRobot::RobotIO::loadRobot crashes
@@ -59,9 +61,7 @@ namespace armarx::viz::coin
                 if (result)
                 {
                     result->setThreadsafe(false);
-                    // Do we want to propagate joint values? Probably not...
-                    // Closing the hand on the real robot could be implemented on another level
-                    result->setPropagatingJointValuesEnabled(false);
+                    result->setPropagatingJointValuesEnabled(true);
                 }
                 else
                 {
@@ -88,7 +88,8 @@ namespace armarx::viz::coin
 
         static std::vector<RobotInstancePool> robotCache;
 
-        LoadedRobot getRobotFromCache(std::string const& project, std::string const& filename)
+        LoadedRobot
+        getRobotFromCache(std::string const& project, std::string const& filename)
         {
             // We can use a global variable, since this code is only executed in the GUI thread
 
@@ -104,7 +105,8 @@ namespace armarx::viz::coin
                     if (instancePool.usedInstances < instancePool.robots.size())
                     {
                         // 1) We have still unused instances in the pool ==> Just return one
-                        ARMARX_DEBUG << "Reusing robot instance from cache " << VAROUT(project) << ", " << VAROUT(filename);
+                        ARMARX_DEBUG << "Reusing robot instance from cache " << VAROUT(project)
+                                     << ", " << VAROUT(filename);
                         result.robot = instancePool.robots[instancePool.usedInstances];
                         instancePool.usedInstances += 1;
                     }
@@ -112,14 +114,17 @@ namespace armarx::viz::coin
                     else
                     {
                         // 2) We do not have unused instances in the pool ==> Clone one
-                        ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", " << VAROUT(filename);
+                        ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", "
+                                     << VAROUT(filename);
 
                         if (instancePool.robots.size() > 0)
                         {
-                            VirtualRobot::RobotPtr const& robotToClone = instancePool.robots.front();
+                            VirtualRobot::RobotPtr const& robotToClone =
+                                instancePool.robots.front();
                             float scaling = 1.0f;
                             bool preventCloningMeshesIfScalingIs1 = true;
-                            result.robot = robotToClone->clone(nullptr, scaling, preventCloningMeshesIfScalingIs1);
+                            result.robot = robotToClone->clone(
+                                nullptr, scaling, preventCloningMeshesIfScalingIs1);
 
                             // Insert the cloned robot into the instance pool
                             instancePool.robots.push_back(result.robot);
@@ -127,8 +132,10 @@ namespace armarx::viz::coin
                         }
                         else
                         {
-                            ARMARX_WARNING << "Encountered empty robot instance pool while trying to clone new instance"
-                                           << "\nRobot: " << VAROUT(project) << ", " << VAROUT(filename)
+                            ARMARX_WARNING << "Encountered empty robot instance pool while trying "
+                                              "to clone new instance"
+                                           << "\nRobot: " << VAROUT(project) << ", "
+                                           << VAROUT(filename)
                                            << "\nUsed instances: " << instancePool.usedInstances
                                            << "\nRobots: " << instancePool.robots.size();
                         }
@@ -137,7 +144,8 @@ namespace armarx::viz::coin
                 }
             }
 
-            ARMARX_DEBUG << "Loading robot from file  " << VAROUT(project) << ", " << VAROUT(filename);
+            ARMARX_DEBUG << "Loading robot from file  " << VAROUT(project) << ", "
+                         << VAROUT(filename);
             result.robot = loadRobot(project, filename);
             if (result.robot)
             {
@@ -146,13 +154,15 @@ namespace armarx::viz::coin
                 instancePool.filename = filename;
                 instancePool.robots.push_back(result.robot);
                 instancePool.usedInstances = 1;
-            } else
+            }
+            else
             {
-                ARMARX_WARNING << deactivateSpam(5) << "Robot " << VAROUT(project) << ", " << VAROUT(filename) << "could not be loaded!";
+                ARMARX_WARNING << deactivateSpam(5) << "Robot " << VAROUT(project) << ", "
+                               << VAROUT(filename) << "could not be loaded!";
             }
             return result;
         }
-    }
+    } // namespace
 
     VisualizationRobot::~VisualizationRobot()
     {
@@ -160,7 +170,8 @@ namespace armarx::viz::coin
         {
             if (instancePool.project == loaded.project && instancePool.filename == loaded.filename)
             {
-                ARMARX_DEBUG << "Removing robot from chace " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename);
+                ARMARX_DEBUG << "Removing robot from chace " << VAROUT(loaded.project) << ", "
+                             << VAROUT(loaded.filename);
                 std::vector<VirtualRobot::RobotPtr>& robots = instancePool.robots;
                 auto robotIter = std::find(robots.begin(), robots.end(), loaded.robot);
                 if (robotIter != robots.end())
@@ -176,7 +187,8 @@ namespace armarx::viz::coin
                     {
                         ARMARX_WARNING << "Expected there to be at least one used instance "
                                        << "while trying to put robot instance back into the pool"
-                                       << "\nRobot: " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename)
+                                       << "\nRobot: " << VAROUT(loaded.project) << ", "
+                                       << VAROUT(loaded.filename)
                                        << "\nUsed instances: " << instancePool.usedInstances;
                     }
                 }
@@ -184,12 +196,14 @@ namespace armarx::viz::coin
         }
     }
 
-    bool VisualizationRobot::update(ElementType const& element)
+    bool
+    VisualizationRobot::update(ElementType const& element)
     {
         IceUtil::Time time_start = IceUtil::Time::now();
-        (void) time_start;
+        (void)time_start;
 
-        bool robotChanged = loaded.project != element.project || loaded.filename != element.filename;
+        bool robotChanged =
+            loaded.project != element.project || loaded.filename != element.filename;
         if (robotChanged)
         {
             // The robot file changed, so reload the robot
@@ -199,8 +213,7 @@ namespace armarx::viz::coin
         {
             ARMARX_WARNING << deactivateSpam(10)
                            << "Robot will not visualized since it could not be loaded."
-                           << "\nID: " << element.id
-                           << "\nProject: " << element.project
+                           << "\nID: " << element.id << "\nProject: " << element.project
                            << "\nFilename: " << element.filename;
             return true;
         }
@@ -253,10 +266,8 @@ namespace armarx::viz::coin
 
         if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
         {
-            if (loadedColor.r != element.color.r
-                || loadedColor.g != element.color.g
-                || loadedColor.b != element.color.b
-                || loadedColor.a != element.color.a)
+            if (loadedColor.r != element.color.r || loadedColor.g != element.color.g ||
+                loadedColor.b != element.color.b || loadedColor.a != element.color.a)
             {
                 int numChildren = node->getNumChildren();
                 for (int i = 0; i < numChildren; i++)
@@ -294,7 +305,8 @@ namespace armarx::viz::coin
         return true;
     }
 
-    void VisualizationRobot::recreateVisualizationNodes(int drawStyle)
+    void
+    VisualizationRobot::recreateVisualizationNodes(int drawStyle)
     {
         VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
         if (drawStyle & data::ModelDrawStyle::COLLISION)
@@ -331,8 +343,9 @@ namespace armarx::viz::coin
         }
     }
 
-    void clearRobotCache()
+    void
+    clearRobotCache()
     {
         robotCache.clear();
     }
-}
+} // namespace armarx::viz::coin

source/RobotAPI/components/CMakeLists.txt

@@ -27,6 +27,7 @@ add_subdirectory(NaturalIKTest)
 add_subdirectory(ObjectPoseClientExample)
 add_subdirectory(ObjectPoseProviderExample)
 add_subdirectory(RobotHealth)
+add_subdirectory(RobotDefinition)
 add_subdirectory(RobotNameService)
 add_subdirectory(RobotState)
 add_subdirectory(RobotToArViz)

source/RobotAPI/components/RobotDefinition/CMakeLists.txt

+armarx_component_set_name("RobotDefinition")
+
+set(COMPONENT_LIBS
+    ArmarXCore
+    ArmarXCoreInterfaces  # for DebugObserverInterface
+    ArmarXGuiComponentPlugins
+    RobotAPICore
+    RobotAPIInterfaces
+    RobotAPISkills
+    armem
+    robot_name_service_core
+    robot_name_service_client
+)
+
+set(SOURCES
+    RobotDefinition.cpp
+)
+
+set(HEADERS
+    RobotDefinition.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx")

source/RobotAPI/components/RobotDefinition/RobotDefinition.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::RobotNameService
+ * @author     [Author Name] ( [Author Email] )
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "RobotDefinition.h"
+
+namespace armarx
+{
+    void
+    RobotDefinition::onInitComponent()
+    {
+    }
+
+    void
+    RobotDefinition::onConnectComponent()
+    {
+    }
+
+    void
+    RobotDefinition::onDisconnectComponent()
+    {
+    }
+
+    void
+    RobotDefinition::onExitComponent()
+    {
+    }
+
+    armarx::PropertyDefinitionsPtr
+    RobotDefinition::createPropertyDefinitions()
+    {
+        armarx::PropertyDefinitionsPtr defs =
+            new ComponentPropertyDefinitions(getConfigIdentifier());
+
+        return defs;
+    }
+} // namespace armarx

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/GraspProviderExample/GraspProviderExample.cpp

@@ -5,74 +5,73 @@
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/time/Metronome.h>
 
-#include <RobotAPI/libraries/core/Pose.h>
-
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/query/query_fns.h>
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
-
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/core/Pose.h>
 
 namespace armarx
 {
-    GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(std::string prefix) :
+    GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(
+        std::string prefix) :
         armarx::ComponentPropertyDefinitions(prefix)
     {
     }
 
-    armarx::PropertyDefinitionsPtr GraspProviderExample::createPropertyDefinitions()
+    armarx::PropertyDefinitionsPtr
+    GraspProviderExample::createPropertyDefinitions()
     {
         ARMARX_IMPORTANT << "Prperty defs";
-        armarx::PropertyDefinitionsPtr defs = new GraspProviderExamplePropertyDefinitions(getConfigIdentifier());
+        armarx::PropertyDefinitionsPtr defs =
+            new GraspProviderExamplePropertyDefinitions(getConfigIdentifier());
 
         defs->topic(debugObserver);
 
         defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
 
         return defs;
-
     }
 
-    std::string GraspProviderExample::getDefaultName() const
+    std::string
+    GraspProviderExample::getDefaultName() const
     {
         return "GraspProviderExample";
     }
 
-    GraspProviderExample::GraspProviderExample() : writer(memoryNameSystem()), reader(memoryNameSystem())
+    GraspProviderExample::GraspProviderExample()
     {
-
     }
 
-
-    void GraspProviderExample::onInitComponent()
+    void
+    GraspProviderExample::onInitComponent()
     {
         ARMARX_IMPORTANT << "Init";
     }
 
-
-    void GraspProviderExample::onConnectComponent()
+    void
+    GraspProviderExample::onConnectComponent()
     {
-        writer.connect();
-        reader.connect();
+        writer.connect(memoryNameSystem());
+        reader.connect(memoryNameSystem());
         task = new RunningTask<GraspProviderExample>(this, &GraspProviderExample::run);
         task->start();
     }
 
-
-    void GraspProviderExample::onDisconnectComponent()
+    void
+    GraspProviderExample::onDisconnectComponent()
     {
         task->stop();
     }
 
-
-    void GraspProviderExample::onExitComponent()
+    void
+    GraspProviderExample::onExitComponent()
     {
     }
 
-
-    void GraspProviderExample::run()
+    void
+    GraspProviderExample::run()
     {
         ARMARX_IMPORTANT << "Running example.";
 
@@ -90,9 +89,11 @@ namespace armarx
 
             // initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
             grasping::BimanualGraspCandidate bimanualCandidate = makeDummyBimanualGraspCandidate();
-            bimanualCandidate.groupNr = i; //non-necessary field, but used to commit different candidates
+            bimanualCandidate.groupNr =
+                i; //non-necessary field, but used to commit different candidates
 
-            writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::Now(), "bimanualProvider");
+            writer.commitBimanualGraspCandidate(
+                bimanualCandidate, armem::Time::Now(), "bimanualProvider");
 
 
             //test for writing Seqs, candidates from the same object appear as instances of the same snapshot
@@ -102,7 +103,8 @@ namespace armarx
             candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
 
 
-            writer.commitGraspCandidateSeq(candidatesToWrite, armem::Time::Now(), "candidateProvider");
+            writer.commitGraspCandidateSeq(
+                candidatesToWrite, armem::Time::Now(), "candidateProvider");
 
             // test reader and debug by logging the group number of the candidate
 
@@ -112,15 +114,15 @@ namespace armarx
             {
                 candidates = reader.queryLatestGraspCandidates();
             }
-            catch (armem::error::QueryFailed &e)
+            catch (armem::error::QueryFailed& e)
             {
                 ARMARX_ERROR << e.makeMsg(memoryName);
             }
 
 
-            for (auto &[id, ca] : candidates)
+            for (auto& [id, ca] : candidates)
             {
-                ARMARX_INFO << "candidate with ID " << id << " has group number " <<  ca->groupNr ;
+                ARMARX_INFO << "candidate with ID " << id << " has group number " << ca->groupNr;
             }
 
             std::map<std::string, grasping::BimanualGraspCandidatePtr> bimanualCandidates;
@@ -129,21 +131,23 @@ namespace armarx
             {
                 bimanualCandidates = reader.queryLatestBimanualGraspCandidates();
             }
-            catch (armem::error::QueryFailed &e)
+            catch (armem::error::QueryFailed& e)
             {
                 ARMARX_ERROR << e.makeMsg(memoryName);
             }
 
-            for (auto &[id, ca] : bimanualCandidates)
+            for (auto& [id, ca] : bimanualCandidates)
             {
-                ARMARX_INFO << "bimanual candidate with ID " << id << " has group number " <<  ca->groupNr ;
+                ARMARX_INFO << "bimanual candidate with ID " << id << " has group number "
+                            << ca->groupNr;
             }
 
             m.waitForNextTick();
         }
     }
 
-    grasping::GraspCandidate GraspProviderExample::makeDummyGraspCandidate()
+    grasping::GraspCandidate
+    GraspProviderExample::makeDummyGraspCandidate()
     {
         armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate();
 
@@ -168,9 +172,11 @@ namespace armarx
         return candidate;
     }
 
-    grasping::BimanualGraspCandidate GraspProviderExample::makeDummyBimanualGraspCandidate()
+    grasping::BimanualGraspCandidate
+    GraspProviderExample::makeDummyBimanualGraspCandidate()
     {
-        armarx::grasping::BimanualGraspCandidate bimanualCandidate = armarx::grasping::BimanualGraspCandidate();
+        armarx::grasping::BimanualGraspCandidate bimanualCandidate =
+            armarx::grasping::BimanualGraspCandidate();
 
         bimanualCandidate.approachVectorLeft = Vector3BasePtr(toIce(zeroVector));
         bimanualCandidate.approachVectorRight = Vector3BasePtr(toIce(zeroVector));
@@ -185,4 +191,4 @@ namespace armarx
         return bimanualCandidate;
     }
 
-}
+} // namespace armarx

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/armem/client/RobotStatePredictionClientExample/Component.cpp

@@ -21,33 +21,31 @@
  */
 
 
-#include "RobotStatePredictionClientExample.h"
-#include "Impl.h"
-
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
+#include "Impl.h"
+#include "RobotStatePredictionClientExample.h"
 
 namespace armarx::robot_state_prediction_client_example
 {
 
-    Component::Component() :
-        pimpl(std::make_unique<Impl>(memoryNameSystem()))
+    Component::Component() : pimpl(std::make_unique<Impl>())
     {
     }
 
-
     RobotStatePredictionClientExample::~RobotStatePredictionClientExample() = default;
 
-
-    std::string Component::getDefaultName() const
+    std::string
+    Component::getDefaultName() const
     {
         return "RobotStatePredictionClientExample";
     }
 
-
-    armarx::PropertyDefinitionsPtr Component::createPropertyDefinitions()
+    armarx::PropertyDefinitionsPtr
+    Component::createPropertyDefinitions()
     {
-        armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+        armarx::PropertyDefinitionsPtr defs =
+            new ComponentPropertyDefinitions(getConfigIdentifier());
 
         ARMARX_CHECK_NOT_NULL(pimpl);
         pimpl->defineProperties(defs, "p.");
@@ -55,13 +53,13 @@ namespace armarx::robot_state_prediction_client_example
         return defs;
     }
 
-
-    void Component::onInitComponent()
+    void
+    Component::onInitComponent()
     {
     }
 
-
-    void Component::onConnectComponent()
+    void
+    Component::onConnectComponent()
     {
         pimpl->connect(memoryNameSystem(), arviz);
         pimpl->start();
@@ -70,19 +68,19 @@ namespace armarx::robot_state_prediction_client_example
         RemoteGui_startRunningTask();
     }
 
-
-    void Component::onDisconnectComponent()
+    void
+    Component::onDisconnectComponent()
     {
         pimpl->stop();
     }
 
-
-    void Component::onExitComponent()
+    void
+    Component::onExitComponent()
     {
     }
 
-
-    void Component::createRemoteGuiTab()
+    void
+    Component::createRemoteGuiTab()
     {
         using namespace armarx::RemoteGui::Client;
 
@@ -90,8 +88,9 @@ namespace armarx::robot_state_prediction_client_example
         RemoteGui_createTab(getName(), root, &tab);
     }
 
-    void Component::RemoteGui_update()
+    void
+    Component::RemoteGui_update()
     {
     }
 
-}
+} // namespace armarx::robot_state_prediction_client_example

source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp

@@ -37,7 +37,6 @@
 #include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
 #include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
 
-
 namespace simox::alg
 {
     template <class... Args>
@@ -49,7 +48,6 @@ namespace simox::alg
         return conc;
     }
 
-
     template <class KeyT, class ValueT>
     std::map<KeyT, ValueT>
     map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
@@ -64,7 +62,6 @@ namespace simox::alg
         return map;
     }
 
-
     template <class KeyT, class ValueT>
     std::vector<ValueT>
     multi_at(const std::map<KeyT, ValueT>& map,
@@ -111,15 +108,13 @@ namespace simox::alg
 namespace armarx::robot_state_prediction_client_example
 {
 
-    Impl::Impl(armem::client::MemoryNameSystem& memoryNameSystem)
+    Impl::Impl()
     {
-        client.remote.robotReader.emplace(memoryNameSystem);
+        client.remote.robotReader.emplace();
     }
 
-
     Impl::~Impl() = default;
 
-
     void
     Impl::defineProperties(IceUtil::Handle<PropertyDefinitionContainer>& defs,
                            const std::string& prefix)
@@ -133,7 +128,6 @@ namespace armarx::robot_state_prediction_client_example
         client.remote.robotReader->registerPropertyDefinitions(defs);
     }
 
-
     void
     Impl::connect(armem::client::MemoryNameSystem& mns, viz::Client arviz)
     {
@@ -148,12 +142,11 @@ namespace armarx::robot_state_prediction_client_example
             ARMARX_WARNING << e.what();
         }
 
-        client.remote.robotReader->connect();
+        client.remote.robotReader->connect(mns);
 
         this->remote.arviz = arviz;
     }
 
-
     void
     Impl::start()
     {
@@ -161,14 +154,12 @@ namespace armarx::robot_state_prediction_client_example
         task->start();
     }
 
-
     void
     Impl::stop()
     {
         task->stop();
     }
 
-
     void
     Impl::run()
     {
@@ -182,7 +173,6 @@ namespace armarx::robot_state_prediction_client_example
         }
     }
 
-
     void
     Impl::runOnce()
     {

source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h

@@ -27,27 +27,25 @@
 
 #include <ArmarXCore/util/tasks.h>
 
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-#include <RobotAPI/libraries/armem/client/forward_declarations.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-
 #include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
 
 #include "RobotStatePredictionClient.h"
 
-
 namespace armarx::robot_state_prediction_client_example
 {
 
     class Impl
     {
     public:
-
-        Impl(armem::client::MemoryNameSystem& memoryNameSystem);
+        Impl();
         ~Impl();
 
 
-        void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs, const std::string& prefix);
+        void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs,
+                              const std::string& prefix);
         void connect(armem::client::MemoryNameSystem& mns, viz::Client arviz);
 
         void start();
@@ -58,7 +56,6 @@ namespace armarx::robot_state_prediction_client_example
 
 
     public:
-
         struct Properties
         {
             float updateFrequencyHz = 10;
@@ -66,12 +63,14 @@ namespace armarx::robot_state_prediction_client_example
             std::string robotName = "Armar6";
             float predictAheadSeconds = 1.0;
         };
+
         Properties properties;
 
         struct Remote
         {
             viz::Client arviz;
         };
+
         Remote remote;
 
         armarx::SimpleRunningTask<>::pointer_type task;
@@ -82,6 +81,5 @@ namespace armarx::robot_state_prediction_client_example
         std::optional<std::vector<armem::MemoryID>> localizationEntityIDs;
         std::optional<std::vector<armem::MemoryID>> propioceptionEntityIDs;
         std::optional<viz::Robot> robotViz;
-
     };
-}
+} // namespace armarx::robot_state_prediction_client_example