armem/dev => master

source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp

@@ -19,6 +19,7 @@
 #include <ArmarXCore/core/time/CycleUtil.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/query/query_fns.h>
@@ -39,6 +40,9 @@ namespace armarx::robot_state
 
         defs->topic(debugObserver);
 
+        defs->optional(p.robotName, "robotName");
+        defs->optional(p.updateFrequency, "updateFrequency");
+
         virtualRobotReader.registerPropertyDefinitions(defs);
 
         return defs;
@@ -55,7 +59,9 @@ namespace armarx::robot_state
     {
         virtualRobotReader.connect();
 
-        task = new RunningTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run);
+        ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
+
+        task = new PeriodicTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run, 1000 / p.updateFrequency);
         task->start();
     }
 
@@ -68,44 +74,32 @@ namespace armarx::robot_state
 
     void VirtualRobotReaderExampleClient::run()
     {
-        ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
-
-        std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr};
 
-        CycleUtil cycle(IceUtil::Time::milliSeconds(100));
-        IceUtil::Time start = TimeUtil::GetTime();
+        // initialize if needed
+        if (virtualRobot == nullptr)
+        {
+            TIMING_START(getRobot);
 
-        CycleUtil c(100);
+            virtualRobot = virtualRobotReader.getRobot(p.robotName, IceUtil::Time::now());
 
-        while (not task->isStopped())
-        {
-            // initialize
-            if(virtualRobot == nullptr)
+            if (virtualRobot == nullptr)
             {
-                TIMING_START(getRobot);
-
-                virtualRobot = virtualRobotReader.getRobot("Armar6", IceUtil::Time::now());
-
-                if(virtualRobot == nullptr)
-                {
-                    ARMARX_WARNING << "Could not create virtual robot.";
-                    c.waitForCycleDuration();
-                    continue;
-                }
-                // only print timing once the robot is loadable & loaded
-                TIMING_END_STREAM(getRobot, ARMARX_INFO);
+                ARMARX_WARNING << deactivateSpam(1) << "Could not create virtual robot.";
+                return;
             }
+            // only print timing once the robot is loadable & loaded
+            TIMING_END_STREAM(getRobot, ARMARX_INFO);
+        }
 
-            ARMARX_INFO << "Synchronizing robot";
+        ARMARX_INFO << deactivateSpam(10) << "Synchronizing robot";
 
-            const IceUtil::Time now = TimeUtil::GetTime();
+        const IceUtil::Time now = TimeUtil::GetTime();
+
+        TIMING_START(synchronizeRobot);
+        virtualRobotReader.synchronizeRobot(*virtualRobot, now);
+        TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
 
-            TIMING_START(synchronizeRobot);
-            virtualRobotReader.synchronizeRobot(*virtualRobot, now);
-            TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
 
-            c.waitForCycleDuration();
-        }
     }
 
 }  // namespace armarx::robot_state