WorkerNode.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 <algorithm>
#include <atomic>
#include <memory>
#include <mutex>
#include <thread>
 
#include <ArmarXCore/core/ManagedIceObject.h>
 
#include <RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.h>
 
#include "../../util/CollisionCheckUtil.h"
#include "../../util/Samplers.h"
#include "Tree.h"
#include "Updater.h"
 
namespace armarx
{
    template <class IceBaseClass, class DerivedClass>
    class GenericFactory;
}
 
namespace armarx::rrtconnect
{
    class WorkerNode;
    /**
     * @brief An ice handle for a WorkerNode of the RRTConnect algorithm
     */
    using WorkerNodePtr = IceInternal::Handle<WorkerNode>;
 
    class WorkerNode : virtual public armarx::ManagedIceObject, virtual public WorkerNodeBase
    {
    public:
        using ConfigType = VectorXf;
 
        //inherit base ctors
        using WorkerNodeBase::WorkerNodeBase;
 
        /**
         * @brief dtor. Asserts the worker thread was joined.
         */
        ~WorkerNode() override
        {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wterminate"
            ARMARX_CHECK_EXPRESSION(!workerThread.joinable());
#pragma GCC diagnostic pop
        }
 
        //from managedIceObject
 
        void onInitComponent() override;
        void onConnectComponent() override;
        void onExitComponent() override;
 
        std::string
        getDefaultName() const override
        {
            return "RRTConnectWorkerNode";
        }
 
        //from WorkerNodeBase
        Update getUpdate(Ice::Long updateId,
                         const Ice::Current& = Ice::emptyCurrent) const override;
        void setWorkerNodes(const WorkerNodeBasePrxList& workers,
                            const Ice::Current& = Ice::emptyCurrent) override;
 
        void
        abort(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            abortRequest = true;
        }
 
        //from WorkerUpdateInterface
        void updateTree(const Update& u, const Ice::Current& = Ice::emptyCurrent) override;
 
        void applyUpdates();
 
        //from ice
        void ice_postUnmarshal() override;
 
    protected:
        friend class GenericFactory<WorkerNodeBase, WorkerNode>;
        /**
         * @brief Only used when transmitting through ice.
         */
        WorkerNode() = default;
 
        /**
         * @param workerNodeId The update's worker id.
         * @param updateSubId The update's sub id.
         * @return The update fetched from the manager node.
         */
        Update
        getRemoteUpdate(Ice::Long workerNodeId, Ice::Long updateSubId)
        {
            ARMARX_VERBOSE_S << "[worker " << workerId << "] requesting update " << workerNodeId
                             << " / " << updateSubId;
            ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerNodeId) < workers.size());
            return workers.at(workerNodeId)->getUpdate(updateSubId);
        }
 
        /**
         * @brief The worker task.
         * It performs all planning.
         */
        void workerTask();
 
        void sendUpdate();
 
        NodeId addNode(const ConfigType& cfg, const NodeId& parent, bool addToPrimaryTree);
 
        NodeId
        addNodeToPrimaryTree(const ConfigType& cfg, const NodeId& parent)
        {
            return addNode(cfg, parent, true);
        }
 
        NodeId
        addNodeToSecondaryTree(const ConfigType& cfg, const NodeId& parent)
        {
            return addNode(cfg, parent, false);
        }
 
        /**
         * @return The cspace's dimensionality
         */
        std::size_t
        getDimensionality()
        {
            return cspace->getDimensionality();
        }
 
        Tree&
        getPrimaryTree()
        {
            return fromStartIsPrimaryTree ? treeFromStart : treeFromGoal;
        }
 
        Tree&
        getSecondaryTree()
        {
            return (!fromStartIsPrimaryTree) ? treeFromStart : treeFromGoal;
        }
 
        Ice::Byte
        getPrimaryTreeId() const
        {
            return fromStartIsPrimaryTree ? 0 : 1;
        }
 
        Ice::Byte
        getSecondaryTreeId() const
        {
            return fromStartIsPrimaryTree ? 1 : 0;
        }
 
        void
        swapTrees()
        {
            fromStartIsPrimaryTree ^= true;
        }
 
        Tree treeFromStart;
        Tree treeFromGoal;
        bool fromStartIsPrimaryTree = true;
 
        mutable std::mutex updateMutex;
        Updater updater;
 
        std::atomic_bool doApplyUpdates{false};
        WorkerNodeBasePrxList workers;
 
        std::thread workerThread;
 
        TreeUpdateInterfacePrx globalWorkers;
 
        std::atomic_bool abortRequest;
 
        std::deque<Update> localUpdates;
 
        std::unique_ptr<CuboidSampler> sampler;
        void prepareNextUpdate();
    };
} // namespace armarx::rrtconnect