Merge branch 'feature/robot-unit-component-plugin-waiting' into 'master'

Feature/robot unit component plugin waiting until robot unit is running

See merge request ArmarX/RobotAPI!158

source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp

@@ -130,6 +130,10 @@ namespace armarx::armem::server::robot_state
 
     void RobotStateMemory::onConnectComponent()
     {
+        waitUntilRobotUnitIsRunning();
+
+        ARMARX_CHECK_NOT_NULL(getRobotUnit()->getKinematicUnit());
+
         descriptionSegment.connect(getArvizClient(), getRobotUnit());
 
         proprioceptionSegment.connect(getArvizClient(), getRobotUnit());

source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp

 #include "RobotUnitComponentPlugin.h"
+#include "ArmarXCore/util/CPPUtility/Pointer.h"
+#include <thread>
 
 namespace armarx::plugins
 {
@@ -112,7 +114,11 @@ namespace armarx::plugins
     {
         _deactivated = true;
     }
-}
+
+
+
+
+}  // namespace armarx::plugins
 
 namespace armarx
 {
@@ -130,6 +136,18 @@ namespace armarx
     {
         return *plugin;
     }
+
+    void RobotUnitComponentPluginUser::waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond) const
+    {
+        ARMARX_INFO << "Waiting until robot unit is running ...";
+
+        while ((not termCond()) and ((isNullptr(getRobotUnit()) or (not getRobotUnit()->isRunning()))))
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+
+        ARMARX_INFO << "Robot unit is up and running.";
+    }
 }
 
 

source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h

@@ -78,7 +78,20 @@ namespace armarx
     {
     public:
         RobotUnitComponentPluginUser();
+
         RobotUnitInterfacePrx getRobotUnit() const;
+
+        /**
+         * @brief Waits until the robot unit is running.
+         * 
+         * Although the robot unit proxy might be initialized (\see getRobotUnit()), the robot unit might
+         * not be fully initialized. 
+         * 
+         * @param termCond Termination condition. If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting 
+        *                  for the robot unit to become available.
+         */
+        void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;}) const;
+
         plugins::RobotUnitComponentPlugin& getRobotUnitComponentPlugin();
     private:
         armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;

source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h

@@ -47,7 +47,6 @@ namespace armarx::armem::client::robot_state::localization
     */
     class TransformWriter :
         virtual public TransformWriterInterface
-    // virtual public ::armarx::armem::MemoryConnector
     {
     public:
         TransformWriter(armem::ClientWriterComponentPluginUser& memoryClient);

source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp

@@ -99,25 +99,8 @@ namespace armarx::armem::server::robot_state::description
     void Segment::updateRobotDescription()
     {
         ARMARX_CHECK_NOT_NULL(robotUnit);
+        auto kinematicUnit = robotUnit->getKinematicUnit();
 
-        const auto waitForKinematicUnit = [&]()
-        {
-            while (true)
-            {
-                auto kinematicUnit = robotUnit->getKinematicUnit();
-
-                if (kinematicUnit)
-                {
-                    ARMARX_INFO << "Kinematic unit is now available.";
-                    return kinematicUnit;
-                }
-
-                ARMARX_INFO << "Waiting for kinematic unit ...";
-                std::this_thread::sleep_for(std::chrono::seconds(1));
-            }
-        };
-
-        auto kinematicUnit = waitForKinematicUnit();
         ARMARX_CHECK_NOT_NULL(kinematicUnit);
 
         const auto robotName = kinematicUnit->getRobotName();