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 <atomic>
#include <mutex>
#include <thread>
#include <unordered_map>
#include <utility>
#include <vector>
#include <deque>
 
#include <ArmarXCore/core/ManagedIceObject.h>
 
#include "../../util/Samplers.h"
#include "../../util/Metrics.h"
#include "../../util/PlanningUtil.h"
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h>
 
#include "Tree.h"
 
namespace armarx
{
    template <class IceBaseClass, class DerivedClass> class GenericFactory;
}
namespace armarx::addirrtstar
{
    class WorkerNode;
    /**
    * @brief An ice handle for a WorkerNode of the addirrt* algorithm
    */
    using WorkerNodePtr = IceInternal::Handle<WorkerNode>;
 
    /**
    * @brief Implements the worker side of the batch distributed adaptive dynamic domain informed rrt* planner.
    *
    * This algorithm has two variants for selecting nodes for rewiring.
    * (see doi> 10.1177/0278364911406761)
    * One using k-Nearest-Neighbours with:
    *      - k(#nodes)= kRRT * log(#nodes)
    *      - kRRT > 2^(dimensions+1) * e (1 + 1/dimensions)
    *
    * One using Nearest-Neighbours with a radius:
    *      - r(#nodes)= min{γRRT( log(#nodes) / #nodes )^(1/dimensions) , η}
    *      - γRRT =(2 ( 1 + 1/dimensions))^(1/d) *(μ(Xfree) / ζd)^(1/dimensions)
    *      - η = maximal step size during 1 extend (+inf in this implementation)
    *      - μ(Xfree) = Lebesgue measure of the obstacle-free space
    *      - ζd = volume of the d-dimensional unit ball in Euclidean space
    *
    * since μ(Xfree) can only be estimated the k-NN version is used.
    */
    class WorkerNode:
        virtual public ManagedIceObject,
        virtual public WorkerNodeBase
    {
    public:
        /**
        * @brief Type of a configuration.
        */
        using ConfigType = Tree::ConfigType;
 
        static_assert(std::numeric_limits<Ice::Float>::has_infinity, "requires inf in current implementation");
 
        /**
        * @brief ctor
        * @param cspace The palnning cspace.
        * @param startCfg The start configuration.
        * @param goalCfg The goal configuration.
        * @param DCDStepSize The dcd step size.
        * @param addParams The adaptive dynamic domain parameters.
        * @param manager A proxy to the manager node.
        * @param workerId The worker's id.
        * @param batchSize The batch size.
        * @param nodeCountDeltaForGoalConnectionTries Number of nodes created (by a worker) before a connect to the goal node is tried (by this worker).
        * @param topicPrefix The used topic prefix. (for the topic name to distribute updates)
        * @param rotationMatrix The rotation matrix for the informed ellipsoid.
        */
        WorkerNode(
            const CSpaceBasePtr& cspace,
            const VectorXf& startCfg,
            const VectorXf& goalCfg,
            Ice::Float DCDStepSize,
            AdaptiveDynamicDomainParameters addParams,
            const ManagerNodeBasePrx& manager,
            Ice::Long workerId,
            Ice::Long batchSize,
            Ice::Long nodeCountDeltaForGoalConnectionTries,
            const std::string& topicPrefix,
            const Ice::FloatSeq& rotationMatrix
 
        ):
            WorkerNodeBase(
                cspace,
                startCfg, goalCfg,
                DCDStepSize,
                addParams,
                manager, workerId, batchSize, nodeCountDeltaForGoalConnectionTries, topicPrefix,
                rotationMatrix
            ),
            onePastLastGoalConnect {0}
        {
        }
 
        /**
        * @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
        /**
        * @brief Initializes the worker and starts the worker thread.
        */
        void onInitComponent() override;
        /**
        * @brief Gets a proxy to the topic.
        */
        void onConnectComponent() override;
        /**
        * @brief noop
        */
        void onDisconnectComponent() override {}
        /**
        * @brief Kills the worker thread (if it is still running) and joins it.
        */
        void onExitComponent() override;
        /**
        * @return The component's default name.
        */
        std::string getDefaultName() const override
        {
            return "ADDIRRTStarWorkerNode";
        }
 
        //from WorkerNodeBase
        /**
        * @brief Signals the worker thread to exit.
        */
        void killWorker(const Ice::Current& = Ice::emptyCurrent) override
        {
            killRequest = true;
        }
 
        void pauseWorker(bool pauseFlag, const Ice::Current& = Ice::emptyCurrent) override
        {
            workerPaused = pauseFlag;
        }
 
        /**
        * @param updateId The updates id.
        * @return This workers update with the given id.
        */
        Update getUpdate(Ice::Long updateId, const Ice::Current& = Ice::emptyCurrent) const override;
 
 
        //from WorkerUpdateInterface
        /**
        * @brief Receives an update from other workers and adds it to the queue of pending updates.
        * @param u the update.
        */
        void updateTree(const Update& u, const Ice::Current& = Ice::emptyCurrent)override;
 
        //from ice
        /**
        * @brief noop
        */
        void ice_postUnmarshal()override
        {
        }
 
        /**
        * @brief Calculates the euclidian distance between from and to.
        * @param from First vector.
        * @param to Second vector.
        * @return The euclidian distance between from and to.
        */
        static float distance(const ConfigType& from, const ConfigType& to)
        {
            ARMARX_CHECK_EXPRESSION(from.size() == to.size());
            return euclideanDistance(from.data(), from.data() + from.size(), to.data());
        }
 
    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);
 
        /**
        * @brief The worker task.
        * It performs all planning.
        */
        void workerTask();
 
        /**
        * @brief initializes the tree by getting the current tree from the manager
        */
        void initTree()
        {
            tree.init(
                manager->getTree(),
                addParams,
                workerId
            );
        }
 
        /**
        * @brief Does a linear interpolation from from to to and checks the path per DCD.
        * @param from starting point for the interpolation. (assumed to be collision free)
        * @param to end point for the interpolation.
        * @return The point furthest away from from (but collision free reachable from from)
        * on the line from->to.
        * If from->to is collision free the return value is identical to to (no floating point error).
        * If the first step on the path from->to has a collision the return value is identical to from (no floating point error).
        */
        ConfigType steer(const ConfigType& from, const ConfigType& to);
        /**
        * @param cfg The configuration.
        * @return Whether the given configuration is collision free.
        */
        bool isCollisionFree(const ConfigType& cfg);
        /**
        * @param from The start configuration.
        * @param to The goal configuration.
        * @return Whether the path from from to to is colliswion free.
        */
        bool isPathCollisionFree(const ConfigType& from, const ConfigType& to);
 
        /**
        * @return The cspace's dimensionality
        */
        std::size_t getDimensionality()
        {
            return cspace->getDimensionality();
        }
 
        /**
        * @return Number of neighbours currently considered as parent.
        */
        std::size_t getK();
 
        /**
        * @brief The RRT tree
        */
        Tree tree;
 
        /**
        * @brief Wheether the node should shut down.
        */
        std::atomic_bool killRequest;
 
        /**
         * @brief workerPaused is true if the worker is currently on hold and does not perform any calculations.
         */
        std::atomic_bool workerPaused;
 
        /**
        * @brief Mutex protecting the update structures.
        */
        mutable std::mutex updateMutex;
 
        /**
        * @brief Thread executing the worker task.
        */
        std::thread workerThread;
 
        /**
        * @brief The update topic's name.
        */
        std::string updateTopicName;
        /**
        * @brief Proxy used to write to the update topic.
        */
        TreeUpdateInterfacePrx globalWorkers;
        /**
        * @brief A string representation of the worker id.
        * Used for output.
        */
        std::string workerIdSring;
 
        /**
        * @brief The index one past the last node that was considered to be connected to the goal configuration.
        */
        std::size_t onePastLastGoalConnect;
 
        /**
        * @brief Stores the sampler.
        *
        * A pointer is used, since the sampler ctor requires space information and in case of default construction
        * the information is not available. (the default ctor is used when transmitting this object per ice.)
        * The pointer gets set during construction or post unmarshalling.
        */
        std::unique_ptr<InformedSampler> sampler;
 
        /**
        *@brief required for static factory methode create
        */
        friend class ManagedIceObject;
 
        /**
        * @brief Searches the optimal parent for a configuration.
        * Used by the rrt* component.
        * @param nodeNearestId The id of the nearest node.
        * @param cfgReached The reached configuration.
        * @param outNodeBestId Output parameter: The id of the optimal parent.
        * @param outCostReachedToNodeBest Output parameter: The distance of reached and the optimal parent.
        * @param outKNearestIdsAndDistances Output parameter: The k nearest nodes and their distance to reached.
        * @param outIsCollisionFreeCache Output parameter: Cache storing collision checks for paths between nodes from knearest and reached.
        */
        void findBestParent(const NodeId& nodeNearestId,
                            const ConfigType& cfgReached,
                            NodeId& outNodeBestId,
                            float& outCostReachedToNodeBest,
                            std::vector<std::pair<NodeId, float>>& outKNearestIdsAndDistances,
                            std::map<NodeId, bool>& outIsCollisionFreeCache
                           );
 
        /**
        * @brief The rewire operation of the rrt* algorithm.
        * It optimizes path lengths.
        * @param nodeId The root node's id
        * @param kNearestIdsAndDistances The k nearest nodes and their distance to the root node.
        * @param isCollisionFreeCache Cache storing collision checks for paths between nodes from knearest and the root node..
        */
        void rewire(
            const NodeId& nodeId,
            const std::vector<std::pair<NodeId, float>>& kNearestIdsAndDistances,
            const std::map<NodeId, bool>& isCollisionFreeCache
        );
 
        /**
        * @brief Adds a configuration to the tree.
        * The configurations best parent is searched and rewiring is performed fot it.
        * @param cfgReached The configuration.
        * @param nodeNearestId The nearest node's id.
        */
        void addAndRewireConfig(const ConfigType& cfgReached, const NodeId& nodeNearestId);
 
        /**
        * @brief Executes one iteration of a batch.
        */
        void doBatchIteration();
 
        /**
        * @brief Executes a batch.
        */
        void doBatch();
 
        /**
        * @brief Applies all pending updates to the tree.
        */
        void applyPendingUpdates();
 
    private:
        /**
        * @brief Buffer for the range used by the collision check.
        */
        std::pair<const float*, const float*> bufferCDRange;
    };
}