ManagerNode.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    RobotComponents
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#include <numeric>
#include <algorithm>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include "../../util/Samplers.h"
 
#include "WorkerNode.h"
#include "ManagerNode.h"
#include "util.h"
 
namespace armarx::addirrtstar
{
    //from managedIceObject
    void ManagerNode::onInitComponent()
    {
        setTag("ManagerNode");
        ARMARX_VERBOSE_S << "start init";
        //set other variables
        killRequest = false;
 
        ARMARX_CHECK_EXPRESSION(!workersPerRemoteObjectNode.size());
        workersPerRemoteObjectNode.resize(remoteObjectNodes.size(), 0);
 
        ARMARX_CHECK_EXPRESSION(!maxWorkersPerRemoteObjectNode.size());
        maxWorkersPerRemoteObjectNode.reserve(remoteObjectNodes.size());
        std::transform(
            remoteObjectNodes.begin(), remoteObjectNodes.end(),
            std::back_inserter(maxWorkersPerRemoteObjectNode),
            [](const RemoteObjectNodeInterfacePrx & prx)
        {
            const auto count = static_cast<std::size_t>(prx->getNumberOfCores());
            ARMARX_VERBOSE_S << "ROI " << prx.get() << " has " << count << " cores";
            return count;
        }
        );
        //there cant be more worker than remote objects
        const Ice::Long maxROCount = std::accumulate(maxWorkersPerRemoteObjectNode.begin(), maxWorkersPerRemoteObjectNode.end(), 0);
        maximalWorkerCount = std::min(maximalWorkerCount, maxROCount);
        initialWorkerCount = std::min(initialWorkerCount, maxROCount);
        activeWorkerCount = 0;
 
        //init tree with start node
        tree.init(
            FullIceTree
        {
            FullNodeDataListList{
                FullNodeDataList{
                    FullNodeData{
                        startNode, //config
                        NodeId{0, 0}, //parent
                        std::numeric_limits<Ice::Float>::infinity(), //radius;
                        0.f //fromParentCost
                    }
                }
            },
            Ice::LongSeq{ -1}
        },
        addParams,
        maximalWorkerCount //since this tree does no updates the id is not important
        );
        tree.increaseWorkerCountTo(maximalWorkerCount);
 
        //init prefix
        updateTopicPrefix = getName() + ':';
 
        //register for updates
        usingTopic(updateTopicPrefix + ADDIRRTSTAR_TREE_UPDATE_TOPIC_NAME);
 
        ARMARX_VERBOSE_S << "done init";
    }
 
    void ManagerNode::onConnectComponent()
    {
        ARMARX_VERBOSE_S << "armarx::addirrtstar::ManagerNode::onConnectComponent()";
        //start manager thread
        managerThread = std::thread {[this]{this->managerTask();}};
    }
 
 
    void ManagerNode::onExitComponent()
    {
        ARMARX_VERBOSE_S << "start onExitComponent()";
        killRequest = true;
        managerThread.join();
        ARMARX_VERBOSE_S << "done onExitComponent()";
    }
 
    //from ManagerNodeBase
    PathWithCost ManagerNode::getBestPath(const Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock {treeMutex};
        auto&& path = tree.getBestPath();
        path.nodes.emplace_back(goalNode);
        return path;
    }
    Ice::Long ManagerNode::getPathCount(const Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock {treeMutex};
        return tree.getPathCount();
    }
    PathWithCost ManagerNode::getNthPathWithCost(Ice::Long n, const Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock {treeMutex};
        auto&& path = tree.getNthPathWithCost(static_cast<std::size_t>(n));
        path.nodes.emplace_back(goalNode);
        return path;
    }
 
    PathWithCostSeq ManagerNode::getAllPathsWithCost(const Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock {treeMutex};
        const auto&& pathCount = tree.getPathCount();
        PathWithCostSeq paths {pathCount};
 
        for (std::size_t i = 0; i < pathCount; ++i)
        {
            paths.at(i) = tree.getNthPathWithCost(i);
            paths.at(i).nodes.emplace_back(goalNode);
        }
 
        return paths;
    }
 
    Update ManagerNode::getUpdate(Ice::Long workerId, Ice::Long updateId, const Ice::Current&) const
    {
        ARMARX_WARNING_S << "worker requested remote update w/u " << workerId << "/" << updateId;
        std::lock_guard<std::mutex> lock {updateMutex};
        ARMARX_CHECK_EXPRESSION(workerId >= 0);
        ARMARX_CHECK_EXPRESSION(updateId >= 0);
 
        const auto uWorkerId = static_cast<std::size_t>(workerId);
        const auto uUpdateId = static_cast<std::size_t>(updateId);
        ARMARX_CHECK_EXPRESSION(uWorkerId < getWorkerCount());
 
        if (hasLocalUpdateRequiresUpdateMutex(uWorkerId, uUpdateId))
        {
            return getLocalUpdateRequiresUpdateMutex(uWorkerId, uUpdateId);
        }
 
        return getRemoteUpdate(uWorkerId, uUpdateId);
    }
 
    FullIceTree ManagerNode::getTree(const Ice::Current&) const
    {
        //since we a copying the tree and all update ids we need both mutexes
        std::lock(treeMutex, updateMutex);
        std::lock_guard<std::mutex> treeLock(treeMutex, std::adopt_lock);
        std::lock_guard<std::mutex> updateLock(updateMutex, std::adopt_lock);
        return tree.getIceTree();
    }
 
    void ManagerNode::managerTask()
    {
        try
        {
            ARMARX_CHECK_EXPRESSION(planningComputingPowerRequestStrategy);
            //since post unmarshall is of this is called BEFORE post unmarshall of the member cspace is called this assert has to be done here!
            const auto dim = static_cast<std::size_t>(cspace->getDimensionality());
            ARMARX_CHECK_EXPRESSION(startNode.size() == dim);
            ARMARX_CHECK_EXPRESSION(cspace->isValidConfiguration(std::make_pair(&startNode.front(), &startNode.back() + 1)));
            ARMARX_CHECK_EXPRESSION(goalNode.size() == dim);
            ARMARX_CHECK_EXPRESSION(cspace->isValidConfiguration(std::make_pair(&goalNode.front(), &goalNode.back() + 1)));
 
            //calculate rotation matrix
            {
                UniformProlateSpheroidDistribution<float> dist(startNode.begin(), startNode.end(), goalNode.begin());
                rotationMatrix = dist.getRotationMatrix();
            }
 
            //after starting the requred proxies are set.
            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
 
            ARMARX_IMPORTANT << "Planning Task starts planning";
            //start initial worker
            for (Ice::Long i = 0; i < initialWorkerCount; ++i)
            {
                createNewWorker();
            }
 
            timepointStart = ClockType::now();
            TaskStatus::Status status = TaskStatus::ePlanning;
            planningComputingPowerRequestStrategy->updateTaskStatus(TaskStatus::ePlanning);
 
            std::mutex mutexWait;
            std::unique_lock<std::mutex> waitingLock {mutexWait};
 
 
            planningComputingPowerRequestStrategy->setCurrentStateAsInitialState();
 
            while (true)
            {
                managerEvent.wait_for(waitingLock, std::chrono::milliseconds {100});
 
                if (killRequest)
                {
                    //update correct status
                    status = transitionAtKill(status);
                    ARMARX_IMPORTANT << "Planning Task received killRequest";
                    break;
                }
 
                if (hasTimeRunOut())
                {
                    status = transitionAtOutoftime(status);
                    ARMARX_IMPORTANT << "Planning Task encountered timeout (" << maximalPlanningTimeInSeconds << " seconds)";
                    break;
                }
 
                if (isPlanningDone())
                {
                    status = transitionAtDone(status);
                    ARMARX_IMPORTANT << "Planning Task is finished";
                    break;
                }
 
                {
                    std::lock(treeMutex, updateMutex);
                    std::lock_guard<std::mutex> treeLock(treeMutex, std::adopt_lock);
                    std::unique_lock<std::mutex> updateLock(updateMutex, std::adopt_lock);
                    //apply all updates.
                    //this touches the updates and tree => both mutexes are required.
                    //when fetching a remote update the tree is in a consistent state and the updates are not used.  => both mutexes can be released
                    //since there is no lock guard for this only updateMutex is unlocked.
 
                    applyUpdatesNotThreadSafe(updateLock);
 
                    if ((status == TaskStatus::ePlanning) && tree.getPathCount())
                    {
                        status = TaskStatus::eRefining;
                        task->setTaskStatus(status);
                        ARMARX_IMPORTANT << "Planning Task start refining the found solution";
                        planningComputingPowerRequestStrategy->updateTaskStatus(TaskStatus::eRefining);
                    }
 
                    //update planningComputingPowerRequestStrategy
                    planningComputingPowerRequestStrategy->updateNodeCount(tree.size());
                }
 
                // pause worker processes if maximalWorkerCount was reduced
                while (getActiveWorkerCount() > static_cast<std::size_t>(maximalWorkerCount) && getActiveWorkerCount() > 1)
                {
                    workers.at(activeWorkerCount - 1)->pauseWorker(true);
                    activeWorkerCount--;
                }
                // unpause/start new worker, if maximalWorkerCount is greater then the number of active workers and new resources are required
                if ((getActiveWorkerCount() < static_cast<std::size_t>(maximalWorkerCount)) && planningComputingPowerRequestStrategy->shouldAllocateComputingPower())
                {
                    if (activeWorkerCount >= workers.size())
                    {
                        createNewWorker();
                    }
                    else
                    {
                        workers.at(activeWorkerCount - 1)->pauseWorker(false);
                        activeWorkerCount++;
                    }
                }
            }
 
            ARMARX_VERBOSE_S << "finished main loop managerTask()";
 
            //clean up everyting
            cleanupAllWorkers();
 
            //write back data
            ARMARX_VERBOSE_S << "storing paths in task";
            task->setPaths(getAllPathsWithCost());
 
            //the status may have changed! (if aborted / failed it may be now RefinementAborted)
            if ((status == TaskStatus::ePlanningAborted || status == TaskStatus::ePlanningFailed) && tree.getPathCount())
            {
                //first set the task status
                task->setTaskStatus(TaskStatus::eRefining);
                status = TaskStatus::eRefinementAborted;
            }
 
            ARMARX_VERBOSE_S << "setting task status to " << status;
            //update status
            task->setTaskStatus(status);
            ARMARX_VERBOSE_S << "exit managerTask";
        }
 
#define common_exception_output "EXCEPTION!\n"\
            << "\n\ttask name: " << task->getTaskName()\
            << "\n\tice id = " << task->ice_id()\
            << "\n\told status " << TaskStatus::toString(task->getTaskStatus())
        catch (Ice::Exception& e)
        {
            ARMARX_ERROR_S << common_exception_output
                           << "\n\tWHAT:\n" << e.what()
                           << "\n\tSTACK:\n" << e.ice_stackTrace();
            task->setTaskStatus(TaskStatus::eException);
        }
        catch (std::exception& e)
        {
            ARMARX_ERROR_S << common_exception_output
                           << "\n\tWHAT:\n" << e.what();
            task->setTaskStatus(TaskStatus::eException);
        }
        catch (...)
        {
            ARMARX_ERROR_S << common_exception_output
                           << "\n\tsomething not derived from std::exception was thrown";
            task->setTaskStatus(TaskStatus::eException);
        }
#undef common_exception_output
    }
 
 
 
    void ManagerNode::createNewWorkerOn(std::size_t remoteObjectNodeIndex)
    {
        std::lock_guard<std::mutex> lock {workerMutex};
        ARMARX_VERBOSE_S << "creating worker on " << remoteObjectNodeIndex;
 
        ARMARX_CHECK_EXPRESSION(remoteObjectNodeIndex < remoteObjectNodes.size());
        RemoteObjectNodeInterfacePrx& ron = remoteObjectNodes.at(remoteObjectNodeIndex);
        //prepare worker
        const Ice::Long newWorkerId = getWorkerCount();
 
        ARMARX_CHECK_EXPRESSION(getProxy());
        ManagerNodeBasePrx selfProxy = ManagerNodeBasePrx::uncheckedCast(getProxy());
        ARMARX_CHECK_EXPRESSION(selfProxy);
 
        WorkerNodePtr newWorker {new WorkerNode{
                CSpaceBasePtr::dynamicCast(cspace->clone()),
                startNode, goalNode, dcdStep, addParams,
                selfProxy,
                newWorkerId,
                batchSize,
                nodeCountDeltaForGoalConnectionTries,
                updateTopicPrefix,
                rotationMatrix
            }
        };
        std::stringstream newWorkerName;
        newWorkerName << newWorker->getName() << ":" << newWorkerId << "@[" << getName() << "]";
        //init local structures
        appliedUpdates.emplace_back();
        workers.emplace_back(ron->registerRemoteHandledObject(newWorkerName.str(), newWorker));
        //increment count
        activeWorkerCount++;
        ++workersPerRemoteObjectNode.at(remoteObjectNodeIndex);
 
        ARMARX_VERBOSE_S << "\n created worker"
                         << "\n\t RON index: " << remoteObjectNodeIndex
                         << "\n\t #worker: " << getWorkerCount();
        planningComputingPowerRequestStrategy->allocatedComputingPower();
    }
 
    void ManagerNode::createNewWorker()
    {
        if (getWorkerCount() >= static_cast<std::size_t>(maximalWorkerCount))
        {
            return;
        }
 
        //select where to create a new worker
        ARMARX_CHECK_EXPRESSION(remoteObjectNodes.size() == workersPerRemoteObjectNode.size());
        ARMARX_CHECK_EXPRESSION(remoteObjectNodes.size() == maxWorkersPerRemoteObjectNode.size());
        std::size_t ronIndex = 0;
 
        for (; (ronIndex < remoteObjectNodes.size()) &&
             !(workersPerRemoteObjectNode.at(ronIndex) < maxWorkersPerRemoteObjectNode.at(ronIndex)); ++ronIndex);//no body
 
        ARMARX_CHECK_EXPRESSION(ronIndex < remoteObjectNodes.size());
        createNewWorkerOn(ronIndex);
    }
 
    //not threadsafe! use only when holding updateMutex
    Update ManagerNode::getRemoteUpdate(std::size_t workerId, std::size_t updateId) const
    {
        std::lock_guard<std::mutex> lock {workerMutex};
        ARMARX_WARNING_S << "manager requested remote update w/u " << workerId << "/" << updateId;
        ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
        return workers.at(workerId)->getUpdate(updateId);
    }
    //not threadsafe! use only when holding updateMutex
    bool ManagerNode::hasLocalUpdateRequiresUpdateMutex(std::size_t workerId, std::size_t updateId) const
    {
        ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
        return (updateId < appliedUpdates.at(workerId).size()) || (tree.hasPendingUpdate(workerId, updateId));
    }
    //not threadsafe! use only when holding updateMutex
    const Update& ManagerNode::getLocalUpdateRequiresUpdateMutex(std::size_t workerId, std::size_t updateId) const
    {
        ARMARX_CHECK_EXPRESSION(hasLocalUpdateRequiresUpdateMutex(workerId, updateId));
 
        if (updateId < appliedUpdates.at(workerId).size())
        {
            return appliedUpdates.at(workerId).at(updateId);
        }
 
        //we have the update. so it has to be pending
        return tree.getPendingUpdate(workerId, updateId);
    }
 
    void ManagerNode::setWorkersFinalUpdateId(Ice::Long workerId, Ice::Long finalUpdateId, const Ice::Current&)
    {
        {
            std::lock_guard<std::mutex> lock {updateMutex};
            ARMARX_CHECK_EXPRESSION(workerId >= 0);
            ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerId) < workersFinalUpdateId.size());
            ARMARX_CHECK_EXPRESSION(workersFinalUpdateId.at(workerId) == -1);
            workersFinalUpdateId.at(workerId) = finalUpdateId;
        }
        ARMARX_VERBOSE_S << "final update id for worker " << workerId << " is " << finalUpdateId;
        managerEvent.notify_all();
    }
 
    void ManagerNode::updateTree(const Update& u, const Ice::Current&)
    {
        std::lock_guard<std::mutex> lock {updateMutex};
        tree.addPendingUpdate(u);
        managerEvent.notify_all();
    }
 
    void ManagerNode::cleanupAllWorkers()
    {
        workersFinalUpdateId.resize(getWorkerCount(), -1);
        //kill all
        {
            std::lock_guard<std::mutex> lock {workerMutex};
 
            for (auto& worker : workers)
            {
                worker->killWorker();
            }
        }
 
        {
            std::lock(treeMutex, updateMutex);
            std::unique_lock<std::mutex> treeLock(treeMutex, std::adopt_lock);
            std::unique_lock<std::mutex> updateLock(updateMutex, std::adopt_lock);
 
            //sync for update counts
            do
            {
                managerEvent.wait_for(updateLock, std::chrono::milliseconds {100});
            }
            while (std::any_of(
                       workersFinalUpdateId.begin(), workersFinalUpdateId.end(),
                       [](const Ice::Long & l)
        {
            return l == -1;
        }
               ));
 
            //get all updates
            tree.prepareUpdate(workersFinalUpdateId, updateLock,
                               [this](std::size_t workerId, Ice::Long updateId)//remote update getter
            {
                ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
                return getRemoteUpdate(workerId, updateId);
            },
            [this](Update && u) //update consumer
            {
                cacheAppliedUpdateRequiresUpdateMutex(std::move(u));
            }
                              );
 
            ARMARX_CHECK_EXPRESSION(workersFinalUpdateId.size() == appliedUpdates.size());
 
            for (std::size_t i = 0; i < workersFinalUpdateId.size(); ++i)
            {
                const auto workerAppliedUpdateCount = appliedUpdates.at(i).size();
                const auto workerUpdateCount = static_cast<std::size_t>(workersFinalUpdateId.at(i) + 1);
                const auto workersLastUpdateId = tree.getAppliedUpdateIds().at(i);
                ARMARX_CHECK_EXPRESSION(workersLastUpdateId == static_cast<Ice::Long>(workerAppliedUpdateCount) - 1);
                ARMARX_CHECK_EXPRESSION(workerUpdateCount == workerAppliedUpdateCount);
            }
        }
 
        //now all required data is stored here
        //destroy worker
        std::lock_guard<std::mutex> lock {workerMutex};
        workers.clear();
    }
 
    void ManagerNode::applyUpdatesNotThreadSafe(std::unique_lock<std::mutex>& updateLock)
    {
        ARMARX_CHECK_EXPRESSION(updateLock);
        tree.applyPendingUpdates(updateLock,
                                 [this](std::size_t workerId, Ice::Long updateId)//remote update getter
        {
            ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
            return getRemoteUpdate(workerId, updateId);
        },
        [this](Update && u) //update consumer
        {
            planningComputingPowerRequestStrategy->updateNodeCreations(u.nodes.size(), batchSize);
 
            cacheAppliedUpdateRequiresUpdateMutex(std::move(u));
        }
                                );
        ARMARX_CHECK_EXPRESSION(updateLock);
    }
 
    void ManagerNode::cacheAppliedUpdateRequiresUpdateMutex(Update&& u)
    {
        //correct worker id
        const auto workerId = getUpdatesWorkerId(u);
        ARMARX_CHECK_EXPRESSION(workerId < appliedUpdates.size());
        //assert the last update from the same worker is the worker's update prior to this update
        ARMARX_CHECK_EXPRESSION(tree.hasAppliedUpdate(workerId, getUpdatesPreviousUpdateSubId(u)));
        //the update was applied to the tree
        ARMARX_CHECK_EXPRESSION(tree.hasAppliedUpdate(workerId, getUpdatesSubId(u)));
 
        //    ARMARX_DEBUG_S << "caching update (workerId/updateID): " << workerId << " / " << getUpdatesSubId(u);
        //add the update to the cache
        appliedUpdates.at(workerId).emplace_back(std::move(u));
    }
 
    Ice::Long ManagerNode::getNodeCount(const Ice::Current&) const
    {
        std::lock_guard<std::mutex> treeLock {treeMutex};
        return static_cast<Ice::Long>(tree.size());
    }
 
    bool ManagerNode::isPlanningDone()
    {
        std::lock_guard<std::mutex> treeLock(treeMutex, std::adopt_lock);
        return tree.getBestCost() <= targetCost;
    }
 
    bool ManagerNode::hasTimeRunOut()
    {
        std::lock_guard<std::mutex> treeLock(treeMutex, std::adopt_lock);
        return ((ClockType::now() - timepointStart) >= std::chrono::seconds {maximalPlanningTimeInSeconds});
    }
 
    void ManagerNode::setMaxCpus(Ice::Int maxCpus, const Ice::Current&)
    {
        maximalWorkerCount = maxCpus;
    }
 
    Ice::Int ManagerNode::getMaxCpus(const Ice::Current&) const
    {
        return maximalWorkerCount;
    }
}