ManagerNode.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 <atomic>
#include <condition_variable>
#include <mutex>
#include <thread>
 
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
 
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h>
 
#include "../../util/PlanningUtil.h"
#include "Tree.h"
 
namespace armarx
{
    template <class IceBaseClass, class DerivedClass>
    class GenericFactory;
}
 
namespace armarx::addirrtstar
{
    class ManagerNode;
    /**
    * @brief An ice handle for a ManagerNode.
    */
    using ManagerNodePtr = IceInternal::Handle<ManagerNode>;
 
    /**
    * @brief Manages the planning of the addirrt* algorithm.
    * Starts workers and checks whether the planning should end.
    */
    class ManagerNode : virtual public ManagerNodeBase, virtual public ManagedIceObject
    {
        /**
        * @brief The clock used to meassure durations.
        */
        using ClockType = std::chrono::system_clock;
 
    public:
        /**
        * @brief Ctor.
        * @param task The manager node's task. The manager will store its result in it.
        * @param remoteObjectNodes RemoteObjectNodes used for parallelization.
        * @param initialWorkerCount The initial worker count.
        * @param maximalWorkerCount The maximal worker count.
        * @param planningComputingPowerRequestStrategy The used cprs.
        * @param dcdStep The dcd step size.
        * @param maximalPlanningTimeInSeconds The maximal planning time in seconds. Planning will end after this time.
        * @param batchSize The size of a batch.
        * @param nodeCountDeltaForGoalConnectionTries deprecated
        * @param cspace The used planning cspace.
        * @param startCfg The start configuration.
        * @param goalCfg The goal configuration.
        * @param addParams Parameters for adaptive dynamic domain.
        * @param targetCost The target cost. Planning will end after shorter path was found..
        */
        ManagerNode(
            TaskBasePrx task,
            RemoteObjectNodePrxList remoteObjectNodes,
            //management
            Ice::Long initialWorkerCount,
            Ice::Long maximalWorkerCount,
            const cprs::ComputingPowerRequestStrategyBasePtr& planningComputingPowerRequestStrategy,
            //general
            float dcdStep,
            Ice::Long maximalPlanningTimeInSeconds,
            Ice::Long batchSize,
            Ice::Long nodeCountDeltaForGoalConnectionTries,
            //problem
            CSpaceBasePtr cspace,
            VectorXf startCfg,
            VectorXf goalCfg,
            AdaptiveDynamicDomainParameters addParams,
            float targetCost) :
            ManagerNodeBase(task,
                            remoteObjectNodes,
                            initialWorkerCount,
                            maximalWorkerCount,
                            planningComputingPowerRequestStrategy,
                            dcdStep,
                            maximalPlanningTimeInSeconds,
                            cspace,
                            startCfg,
                            goalCfg,
                            addParams,
                            targetCost,
                            batchSize,
                            nodeCountDeltaForGoalConnectionTries)
        {
            //other variables get initialized in onInitComponent
        }
 
        /**
        * @brief dtor.
        * asserts the manager thread was joined.
        */
        ~ManagerNode() override
        {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wterminate"
            ARMARX_CHECK_EXPRESSION(!managerThread.joinable());
#pragma GCC diagnostic pop
        }
 
        //from managedIceObject
        /**
        * @brief Initializes the tree and sampler.
        * Starts the manager thread.
        */
        void onInitComponent() override;
        /**
        * @brief noop. (debug output)
        */
        void onConnectComponent() override;
 
        /**
        * @brief noop. (debug output)
        */
        void
        onDisconnectComponent() override
        {
            ARMARX_VERBOSE_S << "armarx::addirrtstar::ManagerNode::onDisconnectComponent()";
        }
 
        /**
        * @brief Stopps planning and joins the manager thread.
        */
        void onExitComponent() override;
 
        /**
        * @return The components default name.
        */
        std::string
        getDefaultName() const override
        {
            return "ADDIRRTStarManagerNode";
        }
 
        //from ManagerNodeBase
        /**
        * @brief Sets the flag to stop planning
        */
        void
        kill(const Ice::Current& = Ice::emptyCurrent) override
        {
            killRequest = true;
        }
 
        /**
        * @return The shortest found path. (with its cost)
        */
        PathWithCost getBestPath(const Ice::Current& = Ice::emptyCurrent) const override;
        /**
        * @return The number of found paths.
        */
        Ice::Long getPathCount(const Ice::Current& = Ice::emptyCurrent) const override;
        /**
        * @param index The index.
        * @return The path at the given index.
        */
        PathWithCost getNthPathWithCost(Ice::Long n,
                                        const Ice::Current& = Ice::emptyCurrent) const override;
        /**
        * @return All found paths.
        */
        PathWithCostSeq getAllPathsWithCost(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
        * @param workerId The updates worker id.
        * @param updateId The updates sub id.
        * @return The requested update. If the update is not cached it will be fetched from the corresponding worker.
        */
        Update getUpdate(Ice::Long workerId,
                         Ice::Long updateId,
                         const Ice::Current& = Ice::emptyCurrent) const override;
        /**
        * @return The current tree with all updates applied.
        */
        FullIceTree getTree(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
        * @brief Used by workers to inform the manager about their number of updates before exiting.
        * Used by the manager to fetch all remaining updates before exiting.
        * @param workerId The worker.
        * @param finalUpdateId Its final update id.
        */
        void setWorkersFinalUpdateId(Ice::Long workerId,
                                     Ice::Long finalUpdateId,
                                     const Ice::Current&) override;
 
        //from TreeUpdateInterface
        /**
        * @brief Adds the given update to the queue of pending updates.
        * @param u The update.
        */
        void updateTree(const Update& u, const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
        * @brief Sents the manager's collected test data to the task.
        */
        void sendManagerNodeData() const;
        /**
        * @return The RRT's node count.
        */
        Ice::Long getNodeCount(const Ice::Current& = Ice::emptyCurrent) const override;
 
        // ResourceManagementInterface interface
        void setMaxCpus(Ice::Int maxCpus, const Ice::Current& = Ice::emptyCurrent) override;
 
        Ice::Int getMaxCpus(const Ice::Current& = Ice::emptyCurrent) const override;
 
    protected:
        friend class GenericFactory<ManagerNodeBase, ManagerNode>;
        /**
        * @brief Ctor used by ice factories.
        */
        ManagerNode() = default;
 
        /**
        * @brief Creates a new worker on the given remote object node.
        * @param remoteObjectNodeIndex The remote object node's index.
        */
        void createNewWorkerOn(std::size_t remoteObjectNodeIndex);
 
        /**
        * @brief The managet task.checkedCastIt checks whether new workers are required and starts them if this is the case.
        * Ith checks whether planing has finished.
        */
        void managerTask();
 
        //not threadsafe! use only when holding updateMutex
        /**
        * @brief Returns whether the given update is cached. (requires the caller to hold the updateMutex)
        * @param workerId The updates worker id.
        * @param updateId The updates sub id.
        * @return Whether the given update is cached.
        */
        bool hasLocalUpdateRequiresUpdateMutex(std::size_t workerId, std::size_t updateId) const;
        //not threadsafe! use only when holding updateMutex
        /**
        * @brief Returns the requested update from the cache. (requires the caller to hold the updateMutex)
        * @param workerId The updates worker id.
        * @param updateId The updates sub id.
        * @return The requested update from the cache.
        */
        const Update& getLocalUpdateRequiresUpdateMutex(std::size_t workerId,
                                                        std::size_t updateId) const;
        //not threadsafe! use only when holding updateMutex
        /**
        * @brief Returns the requested update fetched from the corresponding worker. (requires the caller to hold the updateMutex)
        * @param workerId The updates worker id.
        * @param updateId The updates sub id.
        * @return The requested update fetched from the corresponding worker.
        */
        Update getRemoteUpdate(std::size_t workerId, std::size_t updateId) const;
 
        /**
        * @brief Applies all pending updates. (requires the user to hold update and tree mutex)
        * @param updateLock Lock for the update mutex. it will be unlocked when getting a remote update.
        */
        void applyUpdatesNotThreadSafe(std::unique_lock<std::mutex>& updateLock);
        /**
        * @brief Creates a new worker. (the remote object node is selected automatically)
        */
        void createNewWorker();
        /**
        * @brief Shuts down and removes all workers. (Their newest data is fetched before destruction.)
        */
        void cleanupAllWorkers();
 
        /**
        * @brief Stores the given applied update to the cache. (requires the caller to hold the updateMutex)
        * @param u The update.
        */
        void cacheAppliedUpdateRequiresUpdateMutex(Update&& u);
 
        /**
        * @return Whether planning is done.
        */
        bool isPlanningDone();
 
        /**
        * @return Whether planning has failed.
        */
        bool hasTimeRunOut();
 
        /**
         * @brief getActiveWorkerCount returns the number of currently active workers.
         * @return
         */
        std::size_t
        getActiveWorkerCount() const
        {
            return activeWorkerCount;
        }
 
        /**
        * @brief Returns the number of currently available workers (both active and paused).
        * @return
        */
        std::size_t
        getWorkerCount() const
        {
            return workers.size();
        }
 
        //management
        /**
        * @brief Flag to signal the manager thread to exit.
        */
        std::atomic_bool killRequest;
        /**
        * @brief The tread executing \ref managerTask.
        */
        std::thread managerThread;
        /**
         * @brief currentlyActiveWorkers the index of the newest planning process in the workers vector (is <= workers.size()).
         */
        std::atomic_size_t activeWorkerCount;
        /**
        * @brief Worker proxies.
        * worker[i][j] is the j-th worker on the remote object node remoteObjectNodes[i]
        */
        std::vector<RemoteHandle<WorkerNodeBasePrx>> workers;
        /**
        * @brief How many workers are started on each remote object node.
        */
        std::vector<std::size_t> workersPerRemoteObjectNode;
        /**
        * @brief How many workers are maximal allowed on each remote object node.
        */
        std::vector<std::size_t> maxWorkersPerRemoteObjectNode;
        /**
        * @brief used to lock access to the vector workers
        */
        mutable std::mutex workerMutex;
 
        //updates
        /**
        * @brief Protects the update section of the tree and the update cache of the manager
        */
        mutable std::mutex updateMutex;
        /**
        * @brief The update topic's prefix. (to ensure unique names.)
        */
        std::string updateTopicPrefix;
        /**
        * @brief All applied updates. (per worker)
        */
        std::vector<std::deque<Update>> appliedUpdates;
 
        /**
        * @brief Used when shutting down to ensure all updates were applied before destroying the nodes remote object.
        */
        std::vector<Ice::Long> workersFinalUpdateId;
        //rrt
        /**
        * @brief protects the tree data
        */
        mutable std::mutex treeMutex;
        /**
        * @brief CV used by the manager thread to wait for new updates.
        */
        std::condition_variable managerEvent;
        /**
        * @brief The RRT
        */
        Tree tree;
 
        /**
        * @brief The rotation matrix used by the informed samplers.
        */
        Ice::FloatSeq rotationMatrix;
 
        /**
        * @brief Timepoint when the manager node started planning.
        * Used to check whether the maximal planning time was exceeded.
        */
        ClockType::time_point timepointStart;
 
        /**
        *@brief required for static factory methode create
        */
        friend class ManagedIceObject;
    };
} // namespace armarx::addirrtstar