ArmarXObjectScheduler.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, 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    ArmarXCore::core
 * @author    Kai Welke (kai dot welke at kit dot edu)
 * @date      2012
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ArmarXObjectScheduler.h"
 
#include <algorithm> // for min
#include <cstddef> // for NULL
#include <map> // for _Rb_tree_iterator, etc
#include <memory>
#include <ostream> // for operator<<, etc
#include <utility> // for pair
#include <vector> // for vector, vector<>::iterator
 
#include <Ice/BuiltinSequences.h> // for StringSeq
#include <Ice/ObjectAdapter.h> // for ObjectAdapterPtr
#include <IceGrid/Admin.h> // for AdminPrx, Admin
#include <IceGrid/Exception.h> // for DeploymentException, etc
#include <IceStorm/IceStorm.h> // for TopicPrx
#include <IceUtil/Time.h> // for Time
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include "ArmarXCore/core/ArmarXFwd.h" // for ArmarXManagerPtr, etc
#include "ArmarXCore/core/ManagedIceObjectRegistryInterface.h"
#include "ArmarXCore/core/logging/LogSender.h" // for LogSender, flush
#include "ArmarXCore/core/logging/Logging.h" // for ARMARX_VERBOSE, etc
#include "ArmarXCore/interface/core/ManagedIceObjectDefinitions.h"
#include "ArmarXCore/interface/core/ManagedIceObjectDependencyBase.h"
#include "ArmarXCore/interface/core/ThreadingIceBase.h" // for upCast
#include <ArmarXCore/core/ArmarXManager.h> // for ArmarXManagerPtr, etc
#include <ArmarXCore/core/IceManager.h> // for IceManager, ObjectHandles
#include <ArmarXCore/core/ManagedIceObject.h> // for ManagedIceObject
#include <ArmarXCore/core/ManagedIceObjectDependency.h>
#include <ArmarXCore/core/exceptions/Exception.h> // for handleExceptions, etc
#include <ArmarXCore/core/services/tasks/RunningTask.h>
 
#include "IceGridAdmin.h" // for IceGridAdmin
#include "ManagedIceObjectImpl.h"
 
#define WAITMESSAGEINTERVAL (long)5000
 
namespace armarx
{
    // *******************************************************
    // construction
    // *******************************************************
 
 
    ArmarXObjectScheduler::ArmarXObjectScheduler(const ArmarXManagerPtr& armarXManager,
                                                 const IceManagerPtr& iceManager,
                                                 const ManagedIceObjectPtr& object,
                                                 Ice::ObjectAdapterPtr objectAdapterToAddTo,
                                                 bool startSchedulingObject) :
        armarXManager(armarXManager),
        iceManager(iceManager),
        managedObject(object),
        terminateRequested(false),
        objectedInitialized(false),
        tryReconnect(true),
        objectAdapterToAddTo(objectAdapterToAddTo)
    {
        setTag("ObjectScheduler");
        // set object scheduler and armarxManager in managedIceObject
        object->impl->objectScheduler = this;
        object->impl->armarXManager = armarXManager;
 
        interruptCondition = std::make_shared<std::condition_variable>();
        interruptConditionVariable = std::make_shared<bool>();
        if (startSchedulingObject)
        // start management thread
        {
            startScheduling();
        }
    }
 
    armarx::ArmarXObjectScheduler::~ArmarXObjectScheduler()
    {
        wakeupDependencyCheck();
 
        if (scheduleObjectTask)
        {
            scheduleObjectTask->stop(true);
        }
 
        //    ARMARX_VERBOSE << "~ArmarXObjectScheduler()" ;
    }
 
    void
    ArmarXObjectScheduler::startScheduling()
    {
        if (!scheduleObjectTask)
        {
            scheduleObjectTask = new RunningTask<ArmarXObjectScheduler>(
                this,
                &ArmarXObjectScheduler::scheduleObject,
                managedObject->getName() + "ArmarXObjectScheduler");
        }
 
        if (!scheduleObjectTask->isRunning())
        {
            scheduleObjectTask->start();
        }
        else
        {
            ARMARX_INFO << managedObject->getName() << " already scheduled";
        }
    }
 
    // *******************************************************
    // termination handling
    // *******************************************************
    void
    ArmarXObjectScheduler::terminate()
    {
        terminateRequested = true;
 
        if (managedObject)
        {
            managedObject->impl->stateCondition.notify_all();
        }
 
        wakeupDependencyCheck();
        {
            std::scoped_lock lock(interruptMutex);
            *interruptConditionVariable = true;
        }
        interruptCondition->notify_all();
 
        if (scheduleObjectTask)
        {
            scheduleObjectTask->stop(false);
        }
    }
 
    void
    ArmarXObjectScheduler::waitForTermination()
    {
        while (scheduleObjectTask && !scheduleObjectTask->waitForFinished(5000))
        {
            ARMARX_INFO << managedObject->getName()
                        << " is blocking the removal - continuing to wait.";
        }
        std::unique_lock lock(managedObject->impl->objectStateMutex);
 
        while (managedObject->impl->objectState != eManagedIceObjectExited)
        {
            managedObject->impl->stateCondition.wait_for(lock, std::chrono::milliseconds(100));
            ARMARX_INFO << deactivateSpam(5) << managedObject->getName()
                        << " is blocking the removal - continuing to wait.";
        }
    }
 
    void
    ArmarXObjectScheduler::waitForInterrupt()
    {
        std::unique_lock lock(interruptMutex);
 
        if (terminateRequested || (scheduleObjectTask && !scheduleObjectTask->isRunning()))
        {
            return;
        }
 
        *interruptConditionVariable = false;
 
        while (!*interruptConditionVariable)
        {
            interruptCondition->wait(lock);
        }
    }
 
    bool
    ArmarXObjectScheduler::waitForObjectState(ManagedIceObjectState stateToWaitFor,
                                              const long timeoutMs) const
    {
        if (!managedObject)
        {
            return false;
        }
 
        if (timeoutMs == -1)
        {
            while (!terminateRequested)
            {
                std::unique_lock lock(managedObject->impl->objectStateMutex);
 
                if (managedObject->impl->objectState != stateToWaitFor)
                {
                    managedObject->impl->stateCondition.wait_for(
                        lock, std::chrono::milliseconds(WAITMESSAGEINTERVAL));
                }
                else
                {
                    return true;
                }
 
                ARMARX_VERBOSE /*<< deactivateSpam(4)*/ << "Waiting for '"
                                                        << managedObject->getName()
                                                        << "' to reach state "
                                                        << ManagedIceObject::GetObjectStateAsString(
                                                               stateToWaitFor);
            }
        }
        else
        {
            IceUtil::Time startTime = IceUtil::Time::now();
            IceUtil::Time waitTime = startTime;
            long waitTimeLeft = timeoutMs;
 
            while (waitTimeLeft > 0 && !terminateRequested)
            {
                waitTime = IceUtil::Time::now() - startTime;
                waitTimeLeft = timeoutMs - waitTime.toMilliSeconds();
 
                if (waitTime.toMilliSeconds() > 2000)
                {
                    ARMARX_VERBOSE << "Waiting for " << waitTimeLeft << "ms for '"
                                   << managedObject->getName() << "' to reach state "
                                   << ManagedIceObject::GetObjectStateAsString(stateToWaitFor);
                }
 
                std::unique_lock lock(managedObject->impl->objectStateMutex);
 
                if (managedObject->impl->objectState != stateToWaitFor)
                {
                    managedObject->impl->stateCondition.wait_for(
                        lock,
                        std::chrono::milliseconds(std::min(WAITMESSAGEINTERVAL, waitTimeLeft)));
                }
                else
                {
                    return true;
                }
            }
        }
 
        return false;
    }
 
    bool
    ArmarXObjectScheduler::waitForObjectStateMinimum(ManagedIceObjectState minimumStateToWaitFor,
                                                     const long timeoutMs) const
    {
        if (!managedObject)
        {
            return false;
        }
 
        if (timeoutMs == -1)
        {
 
            while (!terminateRequested)
            {
                std::unique_lock lock(managedObject->impl->objectStateMutex);
 
                if (managedObject->impl->objectState < minimumStateToWaitFor)
                {
                    bool timeout = managedObject->impl->stateCondition.wait_for(
                                       lock, std::chrono::milliseconds(WAITMESSAGEINTERVAL)) ==
                                   std::cv_status::timeout;
                    if (timeout)
                    {
                        ARMARX_IMPORTANT
                            << /*deactivateSpam(4) <<*/ "Waiting for '" << managedObject->getName()
                            << "' to reach minimum state "
                            << ManagedIceObject::GetObjectStateAsString(minimumStateToWaitFor)
                            << "\nWaiting for: " << ARMARX_STREAM_PRINTER
                        {
                            if (managedObject->getUnresolvedDependencies().empty())
                            {
                                out << "nothing";
                            }
                            else
                            {
                                out << simox::alg::join(managedObject->getUnresolvedDependencies(),
                                                        ", ");
                            }
                        };
                    }
                }
                else
                {
                    return true;
                }
            }
        }
        else
        {
 
            IceUtil::Time startTime = IceUtil::Time::now();
            IceUtil::Time waitTime = startTime;
            long waitTimeLeft = timeoutMs;
 
            while (waitTimeLeft > 0 && !terminateRequested)
            {
                waitTime = IceUtil::Time::now() - startTime;
                waitTimeLeft = timeoutMs - waitTime.toMilliSeconds();
 
                if (waitTime.toMilliSeconds() > 2000)
                {
                    ARMARX_IMPORTANT
                        << "Waiting for " << waitTimeLeft << "ms for '" << managedObject->getName()
                        << "' to reach minimum state "
                        << ManagedIceObject::GetObjectStateAsString(minimumStateToWaitFor);
                }
 
                std::unique_lock lock(managedObject->impl->objectStateMutex);
 
                if (managedObject->impl->objectState < minimumStateToWaitFor)
                {
                    managedObject->impl->stateCondition.wait_for(
                        lock,
                        std::chrono::milliseconds(std::min(WAITMESSAGEINTERVAL, waitTimeLeft)));
                }
                else
                {
                    return true;
                }
            }
        }
 
        return false;
    }
 
    bool
    ArmarXObjectScheduler::isTerminated() const
    {
        return (scheduleObjectTask && scheduleObjectTask->isFinished()) ||
               getObjectState() == eManagedIceObjectExited;
    }
 
    bool
    ArmarXObjectScheduler::isTerminationRequested() const
    {
        return terminateRequested;
    }
 
    const ManagedIceObjectPtr&
    ArmarXObjectScheduler::getObject() const
    {
        return managedObject;
    }
 
    ManagedIceObjectState
    ArmarXObjectScheduler::getObjectState() const
    {
        return managedObject ? (ManagedIceObjectState)managedObject->getState()
                             : eManagedIceObjectExited;
    }
 
    // *******************************************************
    // dependency resolution
    // *******************************************************
    void
    ArmarXObjectScheduler::waitForDependencies(int timeoutMs)
    {
        IceUtil::Time startTime = IceUtil::Time::now();
 
        bool dependenciesResolved = false;
 
        while (!dependenciesResolved && !terminateRequested)
        {
            dependenciesResolved = checkDependenciesResolvement();
 
 
            if (timeoutMs != -1 && (IceUtil::Time::now() - startTime).toMilliSeconds() >= timeoutMs)
            {
                throw LocalException("Could not resolve dependencies in ") << timeoutMs << " ms";
            }
 
            if (!dependenciesResolved) // only wait when dependencies are not resolved yet
            {
                std::unique_lock lock(dependencyWaitMutex);
                dependencyWaitConditionVariable = false;
                bool timeout = false;
 
                while (!dependencyWaitConditionVariable && !timeout)
                {
                    timeout =
                        (dependencyWaitCondition.wait_for(lock, std::chrono::milliseconds(1000)) ==
                         std::cv_status::timeout);
                }
            }
        }
 
        if (!terminateRequested)
        {
            ARMARX_VERBOSE << "All " << managedObject->getName() << " dependencies resolved";
        }
    }
 
    bool
    ArmarXObjectScheduler::checkDependenciesResolvement()
    {
        bool dependenciesResolved = true;
        bool stateChanged = false;
        std::string unresolvedNames;
 
        // retrieve dependencies
        DependencyMap dependencies = managedObject->getConnectivity().dependencies;
        DependencyMap::iterator iter = dependencies.begin();
 
        while (iter != dependencies.end())
        {
            ManagedIceObjectDependencyPtr dependency =
                ManagedIceObjectDependencyPtr::dynamicCast(iter->second);
 
            // check dependency
            dependency->check();
 
            // check whether dependency has been resolves
            if (!dependency->getResolved())
            {
                dependenciesResolved = false;
                unresolvedNames += "\t" + dependency->getName() + "\n";
            }
 
            // check if dependecy state has changed
            if (dependency->getStateChanged())
            {
                stateChanged = true;
            }
 
            iter++;
        }
 
        // output list of objects we still need
        if (stateChanged)
        {
            if (unresolvedNames.length() > 0)
            {
                ARMARX_INFO << "ManagedIceObject '" << managedObject->getName()
                            << "' still waiting for: \n " << unresolvedNames;
            }
            else
            {
                ARMARX_INFO << "All dependencies of '" << managedObject->getName() << "' resolved!";
            }
        }
        return dependenciesResolved;
    }
 
    void
    ArmarXObjectScheduler::setInteruptConditionVariable(
        std::shared_ptr<std::condition_variable> interruptCondition,
        std::shared_ptr<bool> interruptConditionVariable)
    {
        this->interruptCondition = interruptCondition;
        this->interruptConditionVariable = interruptConditionVariable;
    }
 
    void
    ArmarXObjectScheduler::wakeupDependencyCheck()
    {
        //    ARMARX_DEBUG << managedObject->getName() << " scheduler was woken up";
        {
            std::scoped_lock lock(dependencyWaitMutex);
            dependencyWaitConditionVariable = true;
        }
        dependencyWaitCondition.notify_all();
    }
 
    bool
    ArmarXObjectScheduler::checkDependenciesStatus() const
    {
        bool dependencyLost = false;
        // retrieve dependencies
        DependencyMap dependencies = managedObject->getConnectivity().dependencies;
        DependencyMap::iterator iter = dependencies.begin();
 
        while (iter != dependencies.end())
        {
            ManagedIceObjectDependencyPtr dependency =
                ManagedIceObjectDependencyPtr::dynamicCast(iter->second);
 
            // check dependency
            dependency->check();
 
            // check whether dependency has been resolved
            if (!dependency->getResolved())
            {
                dependencyLost = true;
                iceManager->removeProxyFromCache(dependency->getName(), dependency->getType());
            }
 
            iter++;
        }
 
        return !dependencyLost;
    }
 
    bool
    ArmarXObjectScheduler::dependsOn(const std::string& objectName)
    {
        DependencyMap dependencies = managedObject->getConnectivity().dependencies;
        DependencyMap::iterator iter = dependencies.begin();
 
        while (iter != dependencies.end())
        {
            ManagedIceObjectDependencyPtr dependency =
                ManagedIceObjectDependencyPtr::dynamicCast(iter->second);
 
            if (dependency->getName() == objectName)
            {
                return true;
            }
 
            iter++;
        }
 
        return false;
    }
 
    void
    ArmarXObjectScheduler::disconnected(bool reconnect)
    {
        tryReconnect = reconnect;
        {
            std::scoped_lock lock(interruptMutex);
            *interruptConditionVariable = true;
        }
 
        interruptCondition->notify_all();
    }
 
    // *******************************************************
    // main scheduling thread
    // *******************************************************
    void
    ArmarXObjectScheduler::scheduleObject()
    {
        // first let Managed ice objects initialize
        if (!objectedInitialized)
        {
            initObject();
        }
 
        while (!terminateRequested && (scheduleObjectTask && !scheduleObjectTask->isStopped()))
        {
            // try to resolve dependencies
            waitForDependencies();
 
 
            // register component with ice
            if (!terminateRequested)
            {
                startObject();
            }
 
            // wait for disconnect or shutdown
            waitForInterrupt();
 
            //        checkDependencyStatusTask->stop();
 
            disconnectObject();
 
            if (!tryReconnect)
            {
                break;
            }
        }
 
        scheduleObjectTask->waitForStop();
 
        // exit managed object
        if (terminateRequested)
        {
            exitObject();
        }
    }
 
    // *******************************************************
    // ManagedIceObject phases
    // *******************************************************
    void
    ArmarXObjectScheduler::initObject()
    {
        try
        {
            objectedInitialized = true;
            managedObject->init(iceManager);
        }
        catch (...) // dispatch and handle exception
        {
            managedObject->setObjectState(eManagedIceObjectInitializationFailed);
            handleExceptions();
            terminate();
        }
    }
 
    void
    ArmarXObjectScheduler::startObject()
    {
        // register to iceManager
        ObjectHandles objectHandles = iceManager->registerObject(
            managedObject, managedObject->getName(), objectAdapterToAddTo);
 
        // call hook
        try
        {
            managedObject->start(objectHandles.first,
                                 objectAdapterToAddTo ? objectAdapterToAddTo
                                                      : objectHandles.second);
        }
        catch (...) // dispatch and handle exception
        {
            managedObject->setObjectState(eManagedIceObjectStartingFailed);
            handleExceptions();
        }
 
 
        // offer topics
        Ice::StringSeq offeredTopics = managedObject->getConnectivity().offeredTopics;
        Ice::StringSeq::iterator iterOT = offeredTopics.begin();
 
        while (iterOT != offeredTopics.end())
        {
            iceManager->getTopic<IceStorm::TopicPrx>(*iterOT);
            iterOT++;
        }
 
        // subscribe to topics
        Ice::StringSeq usedTopics = managedObject->getConnectivity().usedTopics;
        Ice::StringSeq::iterator iterUT = usedTopics.begin();
 
        while (iterUT != usedTopics.end())
        {
            iceManager->subscribeTopic(
                objectHandles.first, *iterUT, managedObject->impl->orderedTopicPublishing[*iterUT]);
            iterUT++;
        }
 
        // retrieve (precache) proxies
        DependencyMap dependencies = managedObject->getConnectivity().dependencies;
        // TODO: precaching cannot work with current iceManager since it only provides
        // a template getProxy which uses the typeid for caching
 
 
        // register to admin
        // from now on object is pingable throuhg registry and can be found by
        // waiting ManagedIceObjects
        IceGrid::AdminPrx admin = iceManager->getIceGridSession()->getAdmin();
 
        try
        {
            admin->addObject(objectHandles.first);
        }
        catch (const IceGrid::ObjectExistsException& e)
        {
            admin->updateObject(objectHandles.first);
        }
        catch (const IceGrid::DeploymentException& e)
        {
            ARMARX_ERROR << "*** IceGrid::Admin >> adding " << managedObject->getName()
                         << " raised a DeploymentException(" << e.reason << ")" << flush;
        }
 
        ARMARX_VERBOSE << "Object '" << managedObject->getName() << "'  started";
    }
 
    void
    ArmarXObjectScheduler::disconnectObject()
    {
        ARMARX_INFO << "disconnecting object " << managedObject->getName();
 
        try
        {
            managedObject->disconnect();
        }
 
        catch (...) // dispatch and handle exception
        {
            handleExceptions();
        }
 
        try
        {
            if (iceManager && managedObject)
            {
                iceManager->removeObject(managedObject->getName());
            }
 
            //        IceGrid::AdminPrx admin = iceManager->getIceGridSession()->getAdmin();
            //        if(getObject()->getObjectAdapter())
            //            getObject()->getObjectAdapter()->deactivate();
            //        if(managedObject->getProxy())
            //            admin->removeObject(managedObject->getProxy()->ice_getIdentity());
        }
        catch (IceGrid::ObjectNotRegisteredException& notRegisteredException)
        {
            //        // removing an unregistered object
            //         //!!!
            //        ARMARX_WARNING << "removing "
            //                  << getObject()->getName()
            //                  << " object failed due to ObjectNotRegisteredException"
            //                  << flush;
        }
        // unsubscribe from topics
        Ice::StringSeq usedTopics = managedObject->getConnectivity().usedTopics;
        Ice::StringSeq::iterator iterUT = usedTopics.begin();
 
        try
        {
            while (iterUT != usedTopics.end())
            {
                iceManager->unsubscribeTopic(managedObject->getProxy(), *iterUT);
                iterUT++;
            }
        }
        catch (...)
        {
        }
    }
 
    void
    ArmarXObjectScheduler::exitObject()
    {
        //    ARMARX_INFO << "Exiting object " << managedObject->getName();
 
        managedObject->exit();
 
 
        iceManager = nullptr;
        armarXManager = nullptr;
    }
} // namespace armarx