diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp index 3343f82337d87ba3b52833d3e97c96bcdb3398d7..ea7f0c92860b1f87b331d06fe11809817832e054 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp @@ -138,6 +138,10 @@ namespace armarx { robotUnit.deleteNJointController(this); } + void NJointController::deactivateAndDeleteController(const Ice::Current&) + { + robotUnit.deactivateAndDeleteNJointController(this); + } void NJointController::publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) { diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h index f754ae66b062cae19024922122508dd26f03671d..51df48c403b6b4879646bb391aeb6628d76aeb31 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h @@ -249,6 +249,7 @@ namespace armarx void activateController(const Ice::Current& = GlobalIceCurrent) final override; void deactivateController(const Ice::Current& = GlobalIceCurrent) final override; void deleteController(const Ice::Current& = GlobalIceCurrent) final override; + void deactivateAndDeleteController(const Ice::Current& = GlobalIceCurrent) final override; WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = GlobalIceCurrent) const override; void callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current& = GlobalIceCurrent) override; diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp index 2f7a57cd91b42ea769cc60d0258f4ed1d600b3c8..593baca54861e53f41175cccb8fdc014479bcf51 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp @@ -440,9 +440,46 @@ namespace armarx } } + void ControllerManagement::deactivateAndDeleteNJointController(const std::string &name, const Ice::Current &) + { + deactivateAndDeleteNJointControllers(getNJointControllersNotNull({name})); + } + void ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq &names, const Ice::Current &) + { + deactivateAndDeleteNJointControllers(getNJointControllersNotNull(names)); + } + void ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerPtr &ctrl) + { + deactivateAndDeleteNJointControllers({ctrl}); + } + void ControllerManagement::deactivateAndDeleteNJointControllers(const std::vector<NJointControllerPtr>& ctrlsToDelVec) + { + auto guard = getGuard(); + throwIfDevicesNotReady(__FUNCTION__); + if (ctrlsToDelVec.empty()) + { + return; + } + deactivateNJointControllers(ctrlsToDelVec); + while( + std::any_of( + ctrlsToDelVec.begin(), + ctrlsToDelVec.end(), + [](const NJointControllerPtr& ctrl) {return ctrl->isControllerActive();} + ) + ) + { + if(isShuttingDown()) + { + return; + } + std::this_thread::sleep_for(std::chrono::microseconds{100}); + } + deleteNJointControllers(ctrlsToDelVec); + } NJointControllerClassDescription ControllerManagement::getNJointControllerClassDescription( - const std::string& className, const Ice::Current&) const + const std::string& className, const Ice::Current&) const { while (getRobotUnitState() == RobotUnitState::InitializingComponent || getRobotUnitState() == RobotUnitState::InitializingDevices) { diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h index c0b3cdfe5785d62c4bcf2e463cce427feec64ffa..91300315ab9586e675a1535a1c79fa6ac8fe0ca0 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h @@ -209,6 +209,26 @@ namespace armarx * @see deleteNJointController */ void deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&) override; + /** + * @brief Queues the given \ref NJointController for deletion and deactivates it if necessary. + * @param name The \ref NJointController to delete. + * @see removeNJointControllersToBeDeleted + * @see nJointControllersToBeDeleted + * @see deleteNJointController + * @see deleteNJointControllers + * @see deactivateAnddeleteNJointControllers + */ + void deactivateAndDeleteNJointController(const std::string& name, const Ice::Current&) override; + /** + * @brief Queues the given \ref NJointController "NJointControllers" for deletion and deactivates them if necessary. + * @param names The \ref NJointController "NJointControllers" to delete. + * @see removeNJointControllersToBeDeleted + * @see nJointControllersToBeDeleted + * @see deleteNJointController + * @see deleteNJointControllers + * @see deactivateAnddeleteNJointController + */ + void deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&) override; /** * @brief Requests activation for the given \ref NJointController. @@ -317,6 +337,27 @@ namespace armarx * @see deleteNJointController */ void deleteNJointControllers(const std::vector<NJointControllerPtr>& ctrls); + /** + * @brief Queues the given \ref NJointController for deletion and deactivates it if necessary. + * @param ctrl The \ref NJointController to delete. + * @see removeNJointControllersToBeDeleted + * @see nJointControllersToBeDeleted + * @see deleteNJointController + * @see deleteNJointControllers + * @see deactivateAnddeleteNJointControllers + */ + void deactivateAndDeleteNJointController(const NJointControllerPtr& ctrl); + /** + * @brief Queues the given \ref NJointController "NJointControllers" for deletion and deactivates them if necessary. + * @param ctrls The \ref NJointController "NJointControllers" to delete. + * @see removeNJointControllersToBeDeleted + * @see nJointControllersToBeDeleted + * @see deleteNJointController + * @see deleteNJointControllers + * @see deactivateAnddeleteNJointController + */ + void deactivateAndDeleteNJointControllers(const std::vector<NJointControllerPtr>& ctrls); + /** * @brief Requests activation for the given \ref NJointController. diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointController.ice index 0921d4d42ad95c62968a5672b50c42adffba4244..55cc87a1d51320c5058a0b3e282579ff837578a9 100644 --- a/source/RobotAPI/interface/units/RobotUnit/NJointController.ice +++ b/source/RobotAPI/interface/units/RobotUnit/NJointController.ice @@ -88,6 +88,7 @@ module armarx void activateController(); void deactivateController(); void deleteController() throws LogicError; + void deactivateAndDeleteController() throws LogicError; ["cpp:const"] idempotent WidgetDescription::StringWidgetDictionary getFunctionDescriptions(); void callDescribedFunction(string fuinctionName, StringVariantBaseMap values) throws InvalidArgumentException; diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice index f5906a1fc6a4ec9cd55d8a2819aab032782373a4..406d8121ba4368907eea319ed951bf81e1bf4bfc 100644 --- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice +++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice @@ -200,6 +200,9 @@ module armarx void deleteNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError; void deleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError; + void deactivateAndDeleteNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError; + void deactivateAndDeleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError; + //loading libs bool loadLibFromPath(string path); bool loadLibFromPackage(string package, string libname);