ArViz: Add robot instance pool

- This prevents expensive cloning if elements do not have stable names
- This is the case, e.g. for grasp candidates

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

@@ -78,37 +78,108 @@ namespace armarx::viz::coin
             return result;
         }
 
-        static std::vector<LoadedRobot> robotcache;
+        struct RobotInstancePool
+        {
+            std::string project;
+            std::string filename;
+            std::size_t usedInstances = 0;
+            std::vector<VirtualRobot::RobotPtr> robots;
+        };
+
+        static std::vector<RobotInstancePool> robotCache;
 
         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
 
             LoadedRobot result;
+            result.project = project;
+            result.filename = filename;
 
-            for (LoadedRobot const& loaded : robotcache)
+            for (RobotInstancePool& instancePool : robotCache)
             {
-                if (loaded.project == project && loaded.filename == filename)
+                if (instancePool.project == project && instancePool.filename == filename)
                 {
-                    ARMARX_DEBUG << "loading robot from chace " << VAROUT(project) << ", " << VAROUT(filename);
-                    result = loaded;
-                    //do not scale the robot and do not clone meshes if scaling = 1
-                    result.robot = loaded.robot->clone(nullptr, 1, true);
+                    // There are two possibilities here:
+                    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);
+                        result.robot = instancePool.robots[instancePool.usedInstances];
+                        instancePool.usedInstances += 1;
+                    }
+
+                    else
+                    {
+                        // 2) We do not have unused instances in the pool ==> Clone one
+                        ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", " << VAROUT(filename);
+
+                        if (instancePool.robots.size() > 0)
+                        {
+                            VirtualRobot::RobotPtr const& robotToClone = instancePool.robots.front();
+                            float scaling = 1.0f;
+                            bool preventCloningMeshesIfScalingIs1 = true;
+                            result.robot = robotToClone->clone(nullptr, scaling, preventCloningMeshesIfScalingIs1);
+
+                            // Insert the cloned robot into the instance pool
+                            instancePool.robots.push_back(result.robot);
+                            instancePool.usedInstances += 1;
+                        }
+                        else
+                        {
+                            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();
+                        }
+                    }
                     return result;
                 }
             }
 
-            ARMARX_DEBUG << "loading robot from file  " << VAROUT(project) << ", " << VAROUT(filename);
-            result.project = project;
-            result.filename = filename;
+            ARMARX_DEBUG << "Loading robot from file  " << VAROUT(project) << ", " << VAROUT(filename);
             result.robot = loadRobot(project, filename);
 
-            robotcache.push_back(result);
+            RobotInstancePool& instancePool = robotCache.emplace_back();
+            instancePool.project = project;
+            instancePool.filename = filename;
+            instancePool.robots.push_back(result.robot);
+            instancePool.usedInstances = 1;
 
             return result;
         }
     }
 
+    VisualizationRobot::~VisualizationRobot()
+    {
+        for (RobotInstancePool& instancePool : robotCache)
+        {
+            if (instancePool.project == loaded.project && instancePool.filename == 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())
+                {
+                    // Do not erase the robot, but rather move it to the back
+                    // We can later reuse the unused instances at the back of the vector
+                    std::swap(*robotIter, robots.back());
+                    if (instancePool.usedInstances > 0)
+                    {
+                        instancePool.usedInstances -= 1;
+                    }
+                    else
+                    {
+                        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)
+                                       << "\nUsed instances: " << instancePool.usedInstances;
+                    }
+                }
+            }
+        }
+    }
+
     bool VisualizationRobot::update(ElementType const& element)
     {
         IceUtil::Time time_start = IceUtil::Time::now();
@@ -235,6 +306,6 @@ namespace armarx::viz::coin
 
     void clearRobotCache()
     {
-        robotcache.clear();
+        robotCache.clear();
     }
 }

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

@@ -19,6 +19,8 @@ namespace armarx::viz::coin
     {
         using ElementType = data::ElementRobot;
 
+        ~VisualizationRobot();
+
         bool update(ElementType const& element);
 
         void recreateVisualizationNodes(int drawStyle);

source/RobotAPI/components/ArViz/Example/ArVizExample.cpp

@@ -141,6 +141,27 @@ namespace armarx
 
     }
 
+    void fillRobotHandsLayer(viz::Layer& layer)
+    {
+        Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+
+        for (int i = 0; i < 10; ++i)
+        {
+            // Always generate a new name, so the robot needs to be cached through the instance pool
+            int randomIndex = std::rand();
+            std::string name = "Hand_" + std::to_string(randomIndex);
+
+            pos.x() = 1500.0f;
+            pos.y() = i * 200.0f;
+            viz::Robot robot = viz::Robot(name)
+                               .position(pos)
+                               .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml")
+                               .overrideColor(simox::Color::green(64 + i * 8));
+
+            layer.add(robot);
+        }
+    }
+
 
     void fillExampleLayer(viz::Layer& layer, double timeInSeconds)
     {
@@ -449,6 +470,7 @@ namespace armarx
         viz::Layer pointsLayer = arviz.layer("Points");
         viz::Layer objectsLayer = arviz.layer("Objects");
         viz::Layer disAppearingLayer = arviz.layer("DisAppearing");
+        viz::Layer robotHandsLayer = arviz.layer("RobotHands");
 
 
         // These layers are not updated in the loop.
@@ -457,7 +479,8 @@ namespace armarx
             fillPermanentLayer(permanentLayer);
             arviz.commit(permanentLayer);
         }
-        if (getProperty<bool>("layers.ManyElements"))
+        bool manyElements = getProperty<bool>("layers.ManyElements");
+        if (manyElements)
         {
             viz::Layer manyElementsLayer = arviz.layer("ManyElements");
             fillManyElementsLayer(manyElementsLayer, 0);
@@ -486,7 +509,13 @@ namespace armarx
             disAppearingLayer.clear();
             fillDisAppearingLayer(disAppearingLayer, timeInSeconds);
 
-            arviz.commit({testLayer, exampleLayer, pointsLayer, objectsLayer, disAppearingLayer});
+            if (manyElements)
+            {
+                robotHandsLayer.clear();
+                fillRobotHandsLayer(robotHandsLayer);
+            }
+
+            arviz.commit({testLayer, exampleLayer, pointsLayer, objectsLayer, disAppearingLayer, robotHandsLayer});
 
             c.waitForCycleDuration();
         }

source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp

@@ -21,6 +21,8 @@
  */
 
 #include "ArVizWidgetController.h"
+#include <RobotAPI/components/ArViz/Coin/VisualizationObject.h>
+#include <RobotAPI/components/ArViz/Coin/VisualizationRobot.h>
 
 #include <string>
 
@@ -143,6 +145,8 @@ namespace armarx
 
     void ArVizWidgetController::onExitComponent()
     {
+        armarx::viz::coin::clearObjectCache();
+        armarx::viz::coin::clearRobotCache();
     }
 
     void ArVizWidgetController::onConnectComponent()