ArmarXMultipleObjectsScheduler.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarX
 * @author     Mirko Waechter( mirko.waechter at kit dot edu)
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "ArmarXMultipleObjectsScheduler.h"
 
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <memory>
namespace armarx
{
 
    ArmarXMultipleObjectsScheduler::ArmarXMultipleObjectsScheduler()
    {
        interruptCondition = std::make_shared<std::condition_variable>();
        interruptConditionVariable = std::make_shared<bool>();
        *interruptConditionVariable = false;
        scheduleObjectsTask = new RunningTask<ArmarXMultipleObjectsScheduler>(this, &ArmarXMultipleObjectsScheduler::schedule, "ArmarXMultipleObjectsScheduler");
        scheduleObjectsTask->start();
    }
 
    ArmarXMultipleObjectsScheduler::~ArmarXMultipleObjectsScheduler()
    {
        scheduleObjectsTask->stop(false);
        interruptCondition->notify_all();
        scheduleObjectsTask->waitForFinished();
    }
 
    bool ArmarXMultipleObjectsScheduler::addObjectScheduler(const ArmarXObjectSchedulerPtr& scheduler)
    {
        bool result = false;
        bool found = false;
        {
            std::scoped_lock lock(dataMutex);
            for (size_t j = 0; j < schedulers.size(); ++j)   // std::find does not work for IceUtil::Handle?!....
            {
                for (size_t i = j + 1; i < schedulers.size(); ++i)
                {
                    if (schedulers.at(j).get() == schedulers.at(i).get())
                    {
                        found = true;
                        break;
                    }
                }
                if (found)
                {
                    break;
                }
            }
            if (!found)
            {
                schedulers.push_back(scheduler);
                scheduler->setInteruptConditionVariable(interruptCondition, interruptConditionVariable);
                result = true;
            }
        }
        if (!found)
        {
            std::scoped_lock lock(interruptMutex);
            *interruptConditionVariable = true;
            interruptCondition->notify_all();
        }
        return result;
    }
 
    void ArmarXMultipleObjectsScheduler::schedule()
    {
        while (!scheduleObjectsTask->isStopped())
        {
            {
                //                ARMARX_INFO << "Checking states";
                std::vector<ArmarXObjectSchedulerPtr> tempSchedulers;
                {
                    std::scoped_lock lock(dataMutex);
                    tempSchedulers = schedulers;
                }
                std::vector<ArmarXObjectSchedulerPtr> schedulersToErase;
                int i = 0;
                for (ArmarXObjectSchedulerPtr& sched : tempSchedulers)
                {
                    auto state = sched->getObjectState();
                    //                    ARMARX_INFO << "Checking " << sched->getObject()->getName() << " state: " << sched->getObject()->getState();
 
                    switch (state)
                    {
                        case eManagedIceObjectCreated:
                            sched->initObject();
                            if (sched->checkDependenciesResolvement())
                            {
                                sched->startObject();
                            }
 
                            break;
                        case eManagedIceObjectInitializing:
                            break;
                        case eManagedIceObjectInitialized:
                            if (sched->isTerminationRequested())
                            {
                                sched->exitObject();
                                schedulersToErase.push_back(sched);
                            }
                            else if (sched->checkDependenciesResolvement())
                            {
                                sched->startObject();
                            }
                            break;
                        case eManagedIceObjectStarting:
                            break;
                        case eManagedIceObjectStarted:
                            if (sched->isTerminationRequested())
                            {
                                sched->disconnectObject();
                                sched->exitObject();
                                schedulersToErase.push_back(sched);
                            }
                            else if (!sched->checkDependenciesStatus())
                            {
                                sched->disconnectObject();
                            }
                            break;
 
                        case eManagedIceObjectExiting:
                            break;
                        case eManagedIceObjectExited:
                            break;
                        default:
                            break;
                    }
                    i++;
                }
                std::scoped_lock lock(dataMutex);
                std::reverse(schedulersToErase.begin(), schedulersToErase.end());
                for (auto sched : schedulersToErase)
                {
 
                    //                    std::remove_if(schedulers.begin(), schedulers.end(), [](const ArmarXObjectSchedulerPtr& lhs, const ArmarXObjectSchedulerPtr& rhs){ return lhs.get() == rhs.get();});// std::remove does not work
 
                    for (auto it = schedulers.begin(); it != schedulers.end(); it++)
                    {
                        if (sched.get() == it->get())
                        {
                            schedulers.erase(it);
                            break;
                        }
                    }
                }
            }
            std::unique_lock lock(interruptMutex);
 
            *interruptConditionVariable = false;
 
            while (!*interruptConditionVariable && !scheduleObjectsTask->isStopped())
            {
                if (interruptCondition->wait_for(lock, std::chrono::milliseconds(2000)) == std::cv_status::timeout)
                {
                    //                    ARMARX_INFO << "Timed out";
                    break;
                }
            }
 
        }
    }
 
} // namespace armarx