Updater.h

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
 */
#pragma once
 
#include <functional>
#include <vector>
#include <map>
#include <mutex>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/DataStructures.h>
 
#include "Tree.h"
namespace armarx::rrtconnect
{
    struct UpdateId
    {
        UpdateId(Ice::Long workerId = -1, Ice::Long updateSubId = -1):
            workerId {workerId},
            updateSubId {updateSubId}
        {}
 
        UpdateId(const Update& u):
            workerId {u.workerId},
            updateSubId
        {
            ((workerId >= 0) && (static_cast<std::size_t>(workerId) < u.dependetOnUpdateIds.size())) ?
            u.dependetOnUpdateIds.at(workerId) + 1 :
            -1
        }
        {}
 
        bool operator <(const UpdateId& other) const
        {
            return (workerId < other.workerId) || ((workerId == other.workerId) && (updateSubId < other.updateSubId));
        }
 
        Ice::Long workerId;
        Ice::Long updateSubId;
    };
 
    class Updater
    {
    public:
        Updater() = default;
 
 
        void setWorkerCount(std::size_t count)
        {
            if (appliedUpdateIds.size() < count)
            {
                appliedUpdateIds.resize(count, -1);
            }
        }
 
        void setWorkerId(Ice::Long newId);
 
        void setTrees(const std::vector<std::reference_wrapper<Tree>>& newTrees)
        {
            trees = newTrees;
        }
 
        //pending
        void addPendingUpdate(const Update& u);
 
        void clearPendingUpdates();
 
        bool hasPendingUpdate(const UpdateId& id) const
        {
            return pendingUpdateLookupTable.find(id) != pendingUpdateLookupTable.end();
        }
 
        Update& getPendingUpdate(const UpdateId& id)
        {
            return getPendingUpdate(pendingUpdateLookupTable.at(id));
        }
 
        Update& getPendingUpdate(std::size_t id)
        {
            return pendingUpdates.at(id);
        }
 
        //applying
        void applyUpdate(const Update& u);
 
        template<class LockType, class RemoteUpdateGetter>
        void applyPendingUpdates(LockType&& lock, RemoteUpdateGetter getRemoteUpdate)
        {
            applyPendingUpdates(lock, getRemoteUpdate, [](Update&&) {});
        }
 
        template<class LockType, class RemoteUpdateGetter, class UpdateConsumer>
        void applyPendingUpdates(LockType&& lock, RemoteUpdateGetter getRemoteUpdate, UpdateConsumer updateConsumer)
        {
            ARMARX_CHECK_EXPRESSION(lock);
            //we want i for an assert after the loop
            std::size_t i = 0;
 
            for (; i < pendingUpdates.size(); ++i)
            {
                //apply this pending update
                applyPendingUpdate(getPendingUpdate(i), lock, getRemoteUpdate, updateConsumer);
                ARMARX_CHECK_EXPRESSION(lock);
            }
 
            ARMARX_CHECK_EXPRESSION(lock);
            ARMARX_CHECK_EXPRESSION(i == pendingUpdates.size());
            //clear updates
            clearPendingUpdates();
        }
 
        template<class LockType, class RemoteUpdateGetter, class UpdateConsumer>
        void applyPendingUpdate(Update& u, LockType&& lock, RemoteUpdateGetter getRemoteUpdate, UpdateConsumer updateConsumer)
        {
            ARMARX_CHECK_EXPRESSION(lock);
            ARMARX_CHECK_EXPRESSION(u.workerId >= 0);
 
            if (u.dependetOnUpdateIds.empty())
            {
                //this update was moved from! => it was applied because it was queued out of order)
                return;
            }
 
            ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(u.workerId) < u.dependetOnUpdateIds.size());
 
            //is it an update from this node or was it already applied?
            if (
                //u.workerId == workerId || //self update //now handled by has applied update
                hasAppliedUpdate(u)//update was already applied
            )
            {
                return;
            }
 
            //this update is new => check if it requires prev updates
            //(if we have them: apply them directly, else get them from remote)
            prepareUpdate(u.dependetOnUpdateIds, lock, getRemoteUpdate, updateConsumer);
            ARMARX_CHECK_EXPRESSION(lock);
 
            //now we have all required updates
            ARMARX_CHECK_EXPRESSION(canApplyUpdate(u));
            applyUpdate(u);
            //we dont need the update anymore. move it to the consumer
            updateConsumer(std::move(u));
            //mark the update as moved from
            u.dependetOnUpdateIds.clear();
        }
 
        template<class LockType, class RemoteUpdateGetter, class UpdateConsumer>
        void prepareUpdate(
            Ice::LongSeq dependetOnUpdateIds,
            LockType&& lock,
            RemoteUpdateGetter getRemoteUpdate,
            UpdateConsumer updateConsumer
        )
        {
            ARMARX_CHECK_EXPRESSION(lock);
 
            for (Ice::Long workerNodeId = 0; static_cast<std::size_t>(workerNodeId) < dependetOnUpdateIds.size(); ++workerNodeId)
            {
                const auto updateSubId = dependetOnUpdateIds.at(workerNodeId);
 
                const UpdateId uId
                {
                    workerNodeId, updateSubId
                };
 
                if (hasAppliedUpdate(uId))
                {
                    //we have this update!
                    continue;
                }
                //the update was not applied
 
                //is this update local?
                if (hasPendingUpdate(uId))
                {
                    //local update => recurse
                    applyPendingUpdate(getPendingUpdate(uId), lock, getRemoteUpdate, updateConsumer);
                }
                else
                {
                    ARMARX_CHECK_EXPRESSION(lock);
                    //get the update from remote. this may take a while => unlock
                    lock.unlock();
                    Update update = getRemoteUpdate(workerNodeId, updateSubId);
                    lock.lock();
                    //prepare this update and apply it
                    prepareUpdate(update.dependetOnUpdateIds, lock, getRemoteUpdate, updateConsumer);
                    ARMARX_CHECK_EXPRESSION(lock);
                    ARMARX_CHECK_EXPRESSION(canApplyUpdate(update));
                    applyUpdate(update);
                    //we dont need the update anymore. move it to the consumer
                    updateConsumer(std::move(update));
                    //mark the update as moved from
                    update.dependetOnUpdateIds.clear();
                }
            }
        }
 
        //applied
        bool hasAppliedUpdate(Ice::Long workerId, Ice::Long updateSubId) const
        {
            return (this->workerId == workerId) || ((static_cast<std::size_t>(workerId) < appliedUpdateIds.size()) ?
                                                    updateSubId <= appliedUpdateIds.at(workerId) : false);
        }
 
        bool hasAppliedUpdate(const UpdateId& id) const
        {
            return hasAppliedUpdate(id.workerId, id.updateSubId);
        }
 
        bool canApplyUpdate(const Update& u);
 
        const std::vector<Ice::Long>& getAppliedUpdateIds() const
        {
            return appliedUpdateIds;
        }
 
    private:
        std::vector<std::reference_wrapper<Tree>> trees;
 
        std::deque<Update> pendingUpdates;
        std::map<UpdateId, std::size_t> pendingUpdateLookupTable;
 
        std::vector<Ice::Long> appliedUpdateIds;
        Ice::Long workerId = -1;
    };
}