diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp index bdc02ee10edc9f06b049f4c395427bc4c2fef56d..45d8f734ddc393c387cfdfb2700390ec30c64612 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp @@ -23,6 +23,7 @@ #include "NJointController.h" #include <ArmarXCore/core/util/algorithm.h> +#include <ArmarXCore/core/application/Application.h> #include "../RobotUnit.h" @@ -262,6 +263,13 @@ namespace armarx rtRobotNodes = rtRobot->getRobotNodes(); } + ThreadPoolPtr NJointController::getThreadPool() const + { + ARMARX_CHECK_EXPRESSION(Application::getInstance()); + return Application::getInstance()->getThreadPool(); + } + + const SensorValueBase* NJointController::useSensorValue(const std::string& deviceName) const { ARMARX_CHECK_EXPRESSION_W_HINT( @@ -284,6 +292,49 @@ namespace armarx controlDeviceUsedIndices.reserve(robotUnit.getNumberOfControlDevices()); } + NJointController::~NJointController() + { + // make sure the destructor of the handles does not throw an exception and triggers a termination of the process + try + { + ARMARX_DEBUG << "Deleting thread handles"; + ScopedLock lock(threadHandlesMutex); + for (auto& pair : threadHandles) + { + auto& name = pair.first; + auto& handle = pair.second; + if (!handle) + { + ARMARX_VERBOSE << "Thread Handle is NULL - skipping"; + continue; + } + std::future_status status; + do + { + status = handle->getFuture().wait_for(std::chrono::seconds(3)); + if (status == std::future_status::timeout) + { + ARMARX_INFO << "Still waiting for " << name << " thread handle!"; + } + else if (status == std::future_status::ready || status == std::future_status::deferred) + { + ARMARX_VERBOSE << "Joining " << name << " task"; + handle->join(); + handle.reset(); + } + } + while (status != std::future_status::ready); + } + + threadHandles.clear(); + + } + catch (...) + { + handleExceptions(); + } + } + ControlTargetBase* NJointController::useControlTarget(const std::string& deviceName, const std::string& controlMode) { ARMARX_CHECK_EXPRESSION_W_HINT( diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h index 53d3decd5c8122e0c2b244b06bffa228e960eb95..7abfb6981d1c1c38f42186996c8b12c3a654f315 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h @@ -34,6 +34,7 @@ #include <ArmarXCore/core/util/Registrar.h> #include <ArmarXCore/core/util/TripleBuffer.h> #include <ArmarXCore/core/exceptions/local/ExpressionException.h> +#include <ArmarXCore/core/services/tasks/ThreadPool.h> #include <ArmarXGui/interface/WidgetDescription.h> @@ -61,6 +62,8 @@ namespace VirtualRobot namespace armarx { + using ThreadPoolPtr = std::shared_ptr<class ThreadPool>; + namespace RobotUnitModule { class NJointControllerAttorneyForPublisher; @@ -652,6 +655,55 @@ namespace armarx void onInitComponent() override {} /// @see Component::onConnectComponent void onConnectComponent() override {} + + // //////////////////////////////////////////////////////////////////////////////////////// // + // ////////////////////////////////// ThreadPool functionality///////////////////////////// // + // //////////////////////////////////////////////////////////////////////////////////////// // + protected: + ThreadPoolPtr getThreadPool() const; + + /** + * @brief Executes a given task in a separate thread from the Application ThreadPool. + * @param taskName Descriptive name of this task to identify it on errors. + * @param task std::function object (or lambda) that is to be executed. + * @note This task will be joined in the destructor of the NJointController. So make sure it terminates, when the + * controller is deactivated or removed! + * + * @code{.cpp} + * runTask("PlotterTask", [&] + { + CycleUtil c(30); + getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted); + while (getState() == eManagedIceObjectStarted) + { + if (isControllerActive()) + { + StringVariantBaseMap errors; + for (size_t i = 0; i < sensors.size(); i++) + { + errors[cfg->jointNames.at(i)] = new Variant(currentError(i)); + } + dbgObs->setDebugChannel("TrajectoryController", errors); + } + c.waitForCycleDuration(); + } + }); + * @endcode + */ + template < typename Task > + void runTask(const std::string& taskName, Task&& task) + { + ScopedLock lock(threadHandlesMutex); + ARMARX_CHECK_EXPRESSION(!taskName.empty()); + ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName)); + ARMARX_VERBOSE << "Adding NJointController task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount(); + auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task)); + ARMARX_CHECK_EXPRESSION_W_HINT(handlePtr->isValid(), "Could not add task (" << taskName << " - " << typeid(task).name() << " ) - available threads: " << getThreadPool()->getAvailableTaskCount()); + threadHandles[taskName] = handlePtr; + } + std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles; + Mutex threadHandlesMutex; + // //////////////////////////////////////////////////////////////////////////////////////// // // ///////////////////////////////////// ice interface //////////////////////////////////// // // //////////////////////////////////////////////////////////////////////////////////////// // @@ -783,7 +835,7 @@ namespace armarx NJointController(); - ~NJointController() override = default; + ~NJointController() override; //ice interface (must not be called in the rt thread) //c++ interface (local calls) (must be called in the rt thread)