xsens_threadpool.cpp

Go to the documentation of this file.

 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
#include "xsens_threadpool.h"
#ifndef _MSC_VER
        #include <unistd.h>
        #include <cxxabi.h>
#endif
#ifdef XSENS_DEBUG
        #include <typeinfo>
#endif
 
#include <vector>
#include <xstypes/xsexception.h>
#include "threading.h"
#include <atomic>
 
// Enable this if you want to disable multithreading
//#define ADD_TASK_EXECUTE_NOW
 
namespace xsens
{
 
typedef std::map<unsigned int, std::shared_ptr<PooledTask>> TaskSet;
typedef std::set<PooledThread*> ThreadSet;
typedef std::vector<std::shared_ptr<PooledTask>> TaskList;
 
int processorCount()
{
#if defined(_WIN32)
        SYSTEM_INFO sysinfo;
        ::GetSystemInfo(&sysinfo);
        return (int) sysinfo.dwNumberOfProcessors;
#else
        return sysconf(_SC_NPROCESSORS_ONLN);
        // see http://stackoverflow.com/questions/150355/programmatically-find-the-number-of-cores-on-a-machine
        // for OS-specific code
#endif
}
 
 
unsigned int ThreadPoolTask::needToWaitFor()
{
        return 0;
}
 
 
class PooledTask
{
public:
        ThreadPoolTask* m_task;         
        unsigned int m_id;                      
        std::vector<std::shared_ptr<PooledTask>> m_dependentTasks;      
        XsThreadId m_threadId;
        volatile std::atomic<bool> m_canceling;
 
        PooledTask()
                : m_task(nullptr)
                , m_id(0)
                , m_threadId(0)
                , m_canceling(false)
                , m_completed(false)
                , m_completedMutex()
                , m_completedCondition(m_completedMutex)
        {
        }
 
        ~PooledTask()
        {
                signalCompleted();
                delete m_task;
        }
 
        bool waitForCompletion(uint32_t timeout = UINT32_MAX)
        {
                Lock locker(&m_completedMutex);
                if (!m_completed)
                {
                        // This may look like a dead-lock situation with signalCompleted(), but it isn't
                        if (timeout == UINT32_MAX)
                                m_completedCondition.wait();
                        else
                                m_completedCondition.wait(timeout);
                }
                return m_completed;
        }
 
        void signalCompleted() noexcept
        {
                Lock locker(&m_completedMutex);
                if (!m_completed)
                {
                        m_completed = true;
                        locker.unlock();
                        m_completedCondition.broadcast();
                }
        }
 
private:
        volatile std::atomic_bool m_completed;
        Mutex m_completedMutex;
        WaitCondition m_completedCondition;
};
 
bool ThreadPoolTask::isCanceling() const
{
        if (m_container)
                return m_container->m_canceling;
        return false;
}
 
unsigned int ThreadPoolTask::taskId() const
{
        if (m_container)
                return m_container->m_id;
        return 0;
}
 
 
class PooledThread : public StandardThread
{
public:
        PooledThread(ThreadPool* pool);
        ~PooledThread();
        bool isIdle() const;
        bool isBusy() const;
        unsigned int executedCount() const;
        unsigned int completedCount() const;
        unsigned int failedCount() const;
 
protected:
        ThreadPool* m_pool;                     
        std::shared_ptr<PooledTask> m_task;                     
        unsigned int m_executed;        
        unsigned int m_completed;       
        unsigned int m_failed;          
 
        int32_t innerFunction(void) override;
};
 
PooledThread::PooledThread(ThreadPool* pool)
        : StandardThread()
        , m_pool(pool)
        , m_task(nullptr)
        , m_executed(0)
        , m_completed(0)
        , m_failed(0)
{
}
 
PooledThread::~PooledThread()
{
        stopThread();
        m_pool = nullptr;
}
 
int32_t PooledThread::innerFunction(void)
{
        if (!m_task)
                m_task = m_pool->getNextTask();
 
        while (m_task && !isTerminating())
        {
                m_task->m_threadId = getThreadId();
                bool complete = false;
                try
                {
                        if (m_task->m_task->exec())
                        {
                                ++m_completed;
                                complete = true;
                        }
                        else if (m_task->m_canceling)
                        {
                                ++m_failed;
                                complete = true;
                        }
                }
#ifdef XSENS_DEBUG
                catch (std::exception& e)
                {
                        const std::type_info& et = typeid(e);
#ifdef __GNUC__
                        int status;
                        char* realname = abi::__cxa_demangle(et.name(), 0, 0, &status);
                        fprintf(stderr, "ThreadPool: Caught an unhandled %s(\"%s\")\n", realname, e.what());
                        free(realname);
#else
                        fprintf(stderr, "ThreadPool: Caught an unhandled %s: %s\n", et.name(), e.what());
#endif
                        complete = true;
                }
#endif
                catch (...)
                {
#ifdef XSENS_DEBUG
                        fprintf(stderr, "ThreadPool: Caught an unhandled unknown exception\n");
#endif
                        m_task->m_task->onError();
                        ++m_failed;
                        complete = true;
                }
 
                ++m_executed;
                if (complete)
                        m_pool->reportTaskComplete(m_task);
                else
                {
                        m_task->m_threadId = 0;
                        m_pool->reportTaskPaused(m_task);
                }
 
                m_task = m_pool->getNextTask();
        }
        return 1;
}
 
bool PooledThread::isIdle() const
{
        return (m_task == nullptr);
}
 
bool PooledThread::isBusy() const
{
        return !isIdle();
}
 
unsigned int PooledThread::executedCount() const
{
        return m_executed;
}
 
unsigned int PooledThread::completedCount() const
{
        return m_completed;
}
 
unsigned int PooledThread::failedCount() const
{
        return m_failed;
}
 
 
ThreadPool::ThreadPool()
        : m_nextId(1)
        , m_suspended(false)
        , m_terminating(false)
{
        setPoolSize(0);
}
 
ThreadPool::~ThreadPool()
{
        m_terminating = true;
        suspend(true);
 
        // all threads should now be idle
        m_tasks.clear();
        m_tasksSearch.clear();
        m_executing.clear();
        m_delaying.clear();
 
        try
        {
                for (ThreadSet::iterator it = m_threads.begin(); it != m_threads.end(); ++it)
                        delete *it;
        }
        catch (...)
        {
                // nothing much we can do about this...
        }
}
 
ThreadPool::TaskId ThreadPool::addTask(ThreadPoolTask* task, ThreadPool::TaskId afterId)
{
        assert(!m_terminating);
        std::shared_ptr<PooledTask> tmp = std::make_shared<PooledTask>();
        tmp->m_task = task;
        Lock safety(&m_safe);
        tmp->m_id = m_nextId;
        task->m_container = tmp.get();
 
        // determine next ID
        if (!++m_nextId)
                m_nextId = 1;
 
#ifndef ADD_TASK_EXECUTE_NOW
        if (afterId)
        {
                std::shared_ptr<PooledTask> after = findTask(afterId);
                if (after)
                {
                        after->m_dependentTasks.push_back(tmp);
                        m_delaying[tmp->m_id] = tmp;
                        return tmp->m_id;
                }
        }
 
        m_tasks.push_back(tmp);
        m_tasksSearch[tmp->m_id] = tmp;
#else
        task->exec();
        task->signalCompleted();
#endif
 
        return tmp->m_id;
}
 
unsigned int ThreadPool::count()
{
        Lock safety(&m_safe);
        return (unsigned int)(m_tasks.size() + m_delaying.size() + m_executing.size());
}
 
void ThreadPool::setPoolSize(unsigned int poolsize)
{
        if (poolsize == 0)
        {
                //int pc = processorCount();
                //poolsize = ::std::max(4, pc*2);
                poolsize = 12;  // fixed count to prevent thread starvation on low-core PCs and overthreading on high-core PCs
        }
 
        bool wasSuspended = m_suspended;
        suspend(poolsize < m_threads.size());
        Lock safety(&m_safe);
 
        // reduce size if pool is too large
        if (poolsize < m_threads.size())
        {
                while (poolsize < m_threads.size())
                {
                        auto it = m_threads.begin();
                        auto thread = *it;
                        safety.unlock();
                        thread->stopThread();
                        safety.lock();
                        delete thread;
                        m_threads.erase(it);
                }
        }
 
        // increase size if pool is too small
        for (unsigned int i = (unsigned int) m_threads.size(); i < poolsize; ++i)
        {
                PooledThread* t = new PooledThread(this);
                m_threads.insert(t);
#ifdef XSENS_DEBUG
                char bufje[64];
                sprintf(bufje, "Pooled Thread %p", t);
#else
                const char bufje[] = "Pooled Thread";
#endif
                if (!t->startThread(bufje))
                {
                        m_threads.erase(m_threads.find(t));
                        delete t;
                        throw XsException(XRV_ERROR, "Could not start thread for ThreadPool");
                }
        }
 
        if (!wasSuspended)
                resume();
}
 
unsigned int ThreadPool::poolSize() const
{
        return (unsigned int) m_threads.size();
}
 
std::shared_ptr<PooledTask> ThreadPool::findTask(ThreadPool::TaskId id)
{
        Lock safety(&m_safe);
        TaskSet::iterator it = m_executing.find(id);
        if (it != m_executing.end())
                return it->second;
 
        it = m_delaying.find(id);
        if (it != m_delaying.end())
                return it->second;
 
        it = m_tasksSearch.find(id);
        if (it != m_tasksSearch.end())
                return it->second;
 
        for (auto const& tsk : m_tasks)
                if (tsk->m_id == id)
                        return tsk;
 
        return std::shared_ptr<PooledTask>();
}
 
XsThreadId ThreadPool::taskThreadId(TaskId id)
{
        Lock safety(&m_safe);
        TaskSet::iterator it = m_executing.find(id);
        if (it != m_executing.end())
                return it->second->m_threadId;
        return 0;
}
 
bool ThreadPool::doesTaskExist(ThreadPool::TaskId id)
{
        return findTask(id) != nullptr;
}
 
void ThreadPool::cancelTask(ThreadPool::TaskId id, bool wait) noexcept
{
        Lock safety(&m_safe);
        TaskSet::iterator it = m_executing.find(id);
        if (it != m_executing.end())
        {
                it->second->m_canceling = true;
                safety.unlock();
                if (wait)
                        waitForCompletion(id);
                return;
        }
 
        it = m_delaying.find(id);
        if (it != m_delaying.end())
        {
                reportTaskComplete(it->second);
                m_delaying.erase(it);
        }
 
        it = m_tasksSearch.find(id);
        if (it != m_tasksSearch.end())
        {
                reportTaskComplete(it->second);
                m_tasksSearch.erase(it);
        }
 
        for (auto tskIt = m_tasks.begin(); tskIt != m_tasks.end(); ++tskIt)
        {
                if ((*tskIt)->m_id == id)
                {
                        reportTaskComplete(*tskIt);
                        m_tasks.erase(tskIt);
                        return;
                }
        }
}
 
void ThreadPool::waitForCompletion(ThreadPool::TaskId id)
{
        std::shared_ptr<PooledTask> task = findTask(id);
        if (task != nullptr)
                task->waitForCompletion();
}
 
void ThreadPool::reportTaskComplete(std::shared_ptr<PooledTask> task)
{
        Lock safety(&m_safe);
 
        // remove task from 'executing' list
        TaskSet::iterator it = m_executing.find(task->m_id);
        if (it != m_executing.end())
                m_executing.erase(it);
 
        // notify dependent tasks that their dependency has been fulfilled
        for (TaskList::iterator dep = task->m_dependentTasks.begin(); dep != task->m_dependentTasks.end(); ++dep)
        {
                it = m_delaying.find((*dep)->m_id);
                if (it != m_delaying.end())
                {
                        m_tasks.push_back(it->second);
                        m_tasksSearch[it->second->m_id] = it->second;
                        m_delaying.erase(it);
                }
        }
        task->signalCompleted();
}
 
std::shared_ptr<PooledTask> ThreadPool::getNextTask()
{
        Lock safety(&m_safe);
        if (!m_suspended && !m_tasks.empty())
        {
                std::shared_ptr<PooledTask> tmp = m_tasks.front();
                m_tasks.pop_front();
                m_tasksSearch.erase(tmp->m_id);
                m_executing[tmp->m_id] = tmp;
                return tmp;
        }
 
        return std::shared_ptr<PooledTask>();
}
 
void ThreadPool::reportTaskPaused(std::shared_ptr<PooledTask> task)
{
        Lock safety(&m_safe);
 
        // remove task from 'executing' list
        TaskSet::iterator it = m_executing.find(task->m_id);
        if (it != m_executing.end())
                m_executing.erase(it);
 
        // add task back into either the delaying list if it should wait for something
        unsigned int waitForId = task->m_task->needToWaitFor();
        if (waitForId)
        {
                std::shared_ptr<PooledTask> after = findTask(waitForId);
                if (after)
                {
                        after->m_dependentTasks.push_back(task);
                        m_delaying[task->m_id] = task;
                        return;
                }
        }
 
        // add task into ready queue
        m_tasks.push_back(task);
        m_tasksSearch[task->m_id] = task;
}
 
void ThreadPool::suspend(bool wait) noexcept
{
        Lock safety(&m_safe);
        m_suspended = true;
 
        if (wait)
        {
                safety.unlock();
 
                for (ThreadSet::const_iterator it = m_threads.begin(); it != m_threads.end(); ++it)
                        while ((*it)->isBusy())
                                xsYield();
        }
}
 
void ThreadPool::resume()
{
        Lock safety(&m_safe);
        m_suspended = false;
}
 
unsigned int ThreadPool::executedCount(unsigned int thread) const
{
        unsigned int i = 0;
        for (ThreadSet::const_iterator it = m_threads.begin(); it != m_threads.end(); ++it, ++i)
                if (i == thread)
                        return (*it)->executedCount();
        return 0;
}
 
unsigned int ThreadPool::completedCount(unsigned int thread) const
{
        unsigned int i = 0;
        for (ThreadSet::const_iterator it = m_threads.begin(); it != m_threads.end(); ++it, ++i)
                if (i == thread)
                        return (*it)->completedCount();
        return 0;
}
 
unsigned int ThreadPool::failedCount(unsigned int thread) const
{
        unsigned int i = 0;
        for (ThreadSet::const_iterator it = m_threads.begin(); it != m_threads.end(); ++it, ++i)
                if (i == thread)
                        return (*it)->failedCount();
        return 0;
}
 
ThreadPool* gPool = NULL;
bool gManagePool = true;
 
ThreadPool* ThreadPool::instance() noexcept
{
        if (gPool)
                return gPool;
        try
        {
                gPool = new ThreadPool;
                gManagePool = true;
        }
        catch (...)
        {
                gPool = nullptr;
                gManagePool = false;
        }
        return gPool;
}
 
void ThreadPool::destroy()
{
        if (gPool && gManagePool)
                delete gPool;
        gPool = NULL;
        gManagePool = true;
}
 
void ThreadPool::setPool(ThreadPool* pool)
{
        if (pool != gPool)
        {
                destroy();
                if (pool)
                {
                        gPool = pool;
                        gManagePool = false;
                }
        }
}
 
 
TaskCompletionWaiter::TaskCompletionWaiter(ThreadPool* pool)
        : ThreadPoolTask()
        , m_pool(pool)
{}
 
TaskCompletionWaiter::TaskCompletionWaiter(const std::list<unsigned int>& waitlist, ThreadPool* pool)
        : ThreadPoolTask()
        , m_pool(pool)
        , m_waitList(waitlist)
{}
 
TaskCompletionWaiter::~TaskCompletionWaiter()
{}
 
bool TaskCompletionWaiter::exec()
{
        while (m_waitList.size())
        {
                // if the task exists, reschedule this task to recheck after the task completes
                if (m_pool->doesTaskExist(*m_waitList.begin()))
                        return false;
 
                m_waitList.erase(m_waitList.begin());
        }
        return true;
}
 
unsigned int TaskCompletionWaiter::needToWaitFor()
{
        if (m_waitList.size())
                return *m_waitList.begin();
        return 0;
}
 
void TaskCompletionWaiter::addWaitId(unsigned int id)
{
        m_waitList.push_back(id);
}
} // namespace xsens