Merge branch 'master' of https://gitlab.com/ArmarX/RobotAPI

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,55 @@ 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
+        ARMARX_DEBUG << "Deleting thread handles";
+        ScopedLock lock(threadHandlesMutex);
+        for (auto& pair : threadHandles)
+        {
+            try
+            {
+
+                auto& name = pair.first;
+                auto& handle = pair.second;
+                if (!handle || !handle->isValid())
+                {
+                    ARMARX_VERBOSE << "Thread Handle is NULL or invalid - skipping";
+                    continue;
+                }
+                if (handle->isDetached())
+                {
+                    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);
+            }
+            catch (...)
+            {
+                handleExceptions();
+            }
+        }
+
+        threadHandles.clear();
+
+
+    }
+
     ControlTargetBase* NJointController::useControlTarget(const std::string& deviceName, const std::string& controlMode)
     {
         ARMARX_CHECK_EXPRESSION_W_HINT(

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)