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 <map>
#include <mutex>
#include <vector>
 
#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;
    };
} // namespace armarx::rrtconnect