allow reuse of controllers

source/armarx/control/client/ControllerBuilder.h

@@ -27,6 +27,8 @@
 #include <type_traits>
 #include <SimoxUtility/json/json.hpp>
 
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/time/Clock.h"
 #include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h"
 #include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 
@@ -191,12 +193,20 @@ namespace armarx::control::client
             {
                 if (not controller)
                 {
+                    ARMARX_INFO << "Controller does not exist yet.";
+                    return false;
+                }
+
+                if(not allowReuse_)
+                {
+                    ARMARX_INFO << "Not allowed to reuse controller.";
                     return false;
                 }
 
                 // check if controller has correct type
                 if (controller->getClassName() != controllerClassName)
                 {
+                    ARMARX_INFO << "Controller class does not match.";
                     return false;
                 }
 
@@ -209,11 +219,36 @@ namespace armarx::control::client
                 return true;
             }();
 
+            if((not allowReuse_) and controller)
+            {
+                ARMARX_TRACE;
+
+                ARMARX_INFO << "Deleting existing controller `" << controllerName << "`"; 
+                try
+                {
+                    ARMARX_TRACE;
+                    controllerCreator->deactivateAndDeleteNJointController(controllerName);
+                }
+                catch(...)
+                {
+                    ARMARX_WARNING << GetHandledExceptionString();
+                }
+
+                ARMARX_INFO << "Done deleting"; 
+
+                Clock::WaitFor(Duration::MilliSeconds(150));
+            }
+
             // create controller if necessary
             if (not canReuseController)
             {
+                ARMARX_TRACE;
+
+                ARMARX_INFO << "Creating controller `" << controllerName << "`";
+
                 controller = controllerCreator->createNJointController(
                     controllerClassName, controllerName, config);
+                ARMARX_CHECK_NOT_NULL(controller);
             }
 
             if (not controller)
@@ -236,6 +271,11 @@ namespace armarx::control::client
             return std::move(ctrlWrapper);
         }
 
+        ControllerBuilder&  allowReuse(const bool allowReuse)
+        {
+            this->allowReuse_ = allowReuse;
+        }
+
     private:
         bool
         initDefaultConfig()
@@ -272,5 +312,7 @@ namespace armarx::control::client
         std::optional<std::string> nodeSetName;
 
         const std::string controllerNamePrefix;
+
+        bool allowReuse_ = false;
     };
 } // namespace armarx::control::client