diff --git a/source/armarx/control/client/ControllerBuilder.h b/source/armarx/control/client/ControllerBuilder.h index 63c26b84ef513be045837a5ee6ef76f2ba7d2aa4..0c720ce5912a92ca7f4eb28a5ef54144b6000102 100644 --- a/source/armarx/control/client/ControllerBuilder.h +++ b/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