WorkerNode.cpp

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
 */
 
#include <algorithm>
#include <chrono>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
#include "../../util/CollisionCheckUtil.h"
 
#include "WorkerNode.h"
#include "util.h"
 
namespace armarx::addirrtstar
{
    void WorkerNode::onInitComponent()
    {
        ARMARX_CHECK_EXPRESSION(manager);
        //init values
        killRequest = false;
        workerPaused = false;
        updateTopicName = topicPrefix + ADDIRRTSTAR_TREE_UPDATE_TOPIC_NAME;
        //init collision test
        cspace->initCollisionTest();
        //register for armarx message passing
        usingTopic(updateTopicName);
        offeringTopic(updateTopicName);
 
        //start worker
        workerThread = std::thread {[this]{workerTask();}};
    }
 
    void WorkerNode::onConnectComponent()
    {
        globalWorkers = getTopic<TreeUpdateInterfacePrx>(updateTopicName);
    }
 
    void WorkerNode::onExitComponent()
    {
        //kill worker and join it
        killWorker();
        workerThread.join();
    }
 
    void WorkerNode::updateTree(const Update& u, const Ice::Current&)
    {
        std::lock_guard<std::mutex> lock {updateMutex};
        tree.addPendingUpdate(u);
    }
 
    void WorkerNode::workerTask()
    {
        //init sampler
        std::vector<std::pair<float, float>> dimensionBounds {};
        dimensionBounds.reserve(getDimensionality());
 
        for (const auto& dim : cspace->getDimensionsBounds())
        {
            dimensionBounds.emplace_back(dim.min, dim.max);
        }
 
        ARMARX_CHECK_EXPRESSION(getDimensionality() == goalCfg.size());
        ARMARX_CHECK_EXPRESSION(getDimensionality() == startCfg.size());
 
        sampler.reset(
            new InformedSampler
        {
            typename InformedSampler::DistributionType{
                dimensionBounds.begin(), dimensionBounds.end(),
                startCfg.begin(), startCfg.end(), goalCfg.begin(),
                rotationMatrix
            },
            std::mt19937{std::random_device{}()}
        }
        );
        rotationMatrix.clear();
 
        //after starting the requred proxies are set.
        getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
 
        //some assertions
        ARMARX_CHECK_EXPRESSION(sampler);
        //init tree
        //it is initialized after we subscribed to the topic
        initTree();
 
        while (true)
        {
            //check for kill
            if (killRequest)
            {
                break;
            }
 
            if (workerPaused)
            {
                std::this_thread::sleep_for(std::chrono::seconds(2));
                continue;
            }
 
            doBatch();
        }
 
        auto finalUpdateId = tree.getPreviousUpdateId();
        ARMARX_CHECK_EXPRESSION(tree.getAppliedUpdateIds().at(workerId) == finalUpdateId);
        //send the the final update id. Important: DON'T send size-1 since the last element is an empty update
        manager->setWorkersFinalUpdateId(workerId, finalUpdateId);
    }
 
    void WorkerNode::doBatch()
    {
        //update max cost for sampler
        sampler->getDistribution().setMaximalCost(tree.getBestCost());
 
        //iterate batch
        for (Ice::Long currentBatchIteration = 0; currentBatchIteration < batchSize; ++currentBatchIteration)
        {
            // //break on kill => sends last update and ends calculation
            if (killRequest)
            {
                break;
            }
 
            doBatchIteration();
        }//for batch
 
        //was the goal reached?
        //try connect the nearest node of all newly (since the last check) added nodes to the goal
        const auto& workersNodes = tree.getNodes().at(workerId);
 
        if (onePastLastGoalConnect + nodeCountDeltaForGoalConnectionTries <= workersNodes.size())
        {
            NodeId goalConnectNodeNearestId;
            goalConnectNodeNearestId.workerId = workerId;
            float goalConnectNodeNearestDistance = std::numeric_limits<float>::infinity();
 
            //        const Update& u = tree.getCurrentUpdate();
            for (std::size_t numberOfNode = onePastLastGoalConnect; numberOfNode < workersNodes.size(); ++numberOfNode)
            {
                const Tree::NodeType& currNode = workersNodes.at(numberOfNode);
                const float currNodeDistance = distance(goalCfg, currNode.config);
 
                if (currNodeDistance < goalConnectNodeNearestDistance)
                {
                    goalConnectNodeNearestDistance = currNodeDistance;
                    goalConnectNodeNearestId.numberOfNode = numberOfNode;
                }
            }
 
            if (goalConnectNodeNearestDistance < std::numeric_limits<float>::infinity())
            {
                const auto cfgReached = steer(tree.getNode(goalConnectNodeNearestId).config, goalCfg);
 
                if (cfgReached == tree.getNode(goalConnectNodeNearestId).config)
                {
                    //this node is a boundary node!
                    tree.decreaseRadius(goalConnectNodeNearestId);
                }
                else if (cfgReached == goalCfg)
                {
                    //node can reach goal node
                    //since this node was added to the tree it is rewired
                    //new rewiring could improve other paths but no path leading to the goal
                    //wiht a lower cost can include this node => rewiring would be a waste of time
                    tree.addGoalReached(goalConnectNodeNearestId, goalConnectNodeNearestDistance);
                }
                else
                {
                    //reached some config => add it to the tree and rewire it
                    addAndRewireConfig(cfgReached, goalConnectNodeNearestId);
                }
            }
            onePastLastGoalConnect = workersNodes.size();
        }
 
        //send update
        {
            std::lock_guard<std::mutex> lock {updateMutex};
            tree.sendCurrentUpdate(globalWorkers);
        }
    }
 
    void WorkerNode::applyPendingUpdates()
    {
        {
            //add updates
            tree.applyPendingUpdates(
                std::unique_lock<std::mutex> {updateMutex},
                [this](Ice::Long workerNodeId, Ice::Long updateSubId)
            {
                return getRemoteUpdate(workerNodeId, updateSubId);
            }
            ); //this methode is thread save if the given mutex is correct
        }
    }
 
    void WorkerNode::doBatchIteration()
    {
        applyPendingUpdates();
 
        // //sample
        ConfigType cfgRnd(getDimensionality());
 
        NodeId nodeNearestId;
        float nodeNearestDistance;
        //sample until the sample is in the dynamic domain of nearest
 
        {
            u_int64_t samplingTries {0}; //should never overflow
            do
            {
                //dont get stuck in this loop when killed
                if (!(samplingTries % 1000))
                {
                    //check every 1000th loop
                    if (killRequest)
                    {
                        return;
                    }
 
                    //prevent getting stuck with a few nodes with min radius
                    //+1000 => worker will not try to apply updates at sampling try 0 (it just applied them!)
                    if (!((samplingTries + 1000) % 10000))
                    {
                        applyPendingUpdates();
                    }
                }
                (*sampler)(cfgRnd.data());
                std::tie(nodeNearestId, nodeNearestDistance) = tree.getNearestNeighbourAndDistance(cfgRnd);
                ++samplingTries;
            }
            while (nodeNearestDistance > tree.getNode(nodeNearestId).radius);
        }
 
        const auto cfgReached = steer(tree.getNode(nodeNearestId).config, cfgRnd);
 
        //if the dcd parameter is correct the path xNear -> xNew
        //is collision free (per invariant), but may be the same as xNearest
        //we do no continuous CD
        if (tree.getNode(nodeNearestId).config == cfgReached)
        {
            //this node is a boundary node!
            tree.decreaseRadius(nodeNearestId);
            return;
        }
 
        //no boundary node! find parent, add it to the tree, and rewire it
        addAndRewireConfig(cfgReached, nodeNearestId);
    }
 
    void WorkerNode::addAndRewireConfig(
        const ConfigType& cfgReached,
        const NodeId& nodeNearestId)
    {
        NodeId nodeBestId;
        float costReachedToNodeBest;
        std::vector<std::pair<NodeId, float>> kNearestIdsAndDistances;
        std::map<NodeId, bool> isCollisionFreeCache;
 
 
        findBestParent(
            nodeNearestId,
            cfgReached,
            //out
            nodeBestId,
            costReachedToNodeBest,
            kNearestIdsAndDistances,
            isCollisionFreeCache
        );
 
        //add node to the tree
        const auto nodeReachedId = tree.addNode(
                                       cfgReached, //reached point
                                       nodeBestId, //parent id
                                       costReachedToNodeBest//cost parent->node
                                   );
 
        rewire(nodeReachedId, kNearestIdsAndDistances, isCollisionFreeCache);
    }
 
    void WorkerNode::rewire(
        const NodeId& nodeId,
        const std::vector<std::pair<NodeId, float>>& kNearestIdsAndDistances,
        const std::map<NodeId, bool>& isCollisionFreeCache
    )
    {
        //find root node
        const auto& node = tree.getNode(nodeId);
        const ConfigType& cfgNode = node.config;
        const float  nodeFromStartCost = node.fromStartCost;
 
        for (const auto& nearIdAndDist : kNearestIdsAndDistances)
        {
            const auto& nodeNearId = nearIdAndDist.first;
            auto& nodeNear = tree.getNode(nodeNearId);
            const auto& cfgNodeNear = nodeNear.config;
 
            const auto costPathToNearOld = nodeNear.fromStartCost;
            const auto costNodeToNear = nearIdAndDist.second;
            const auto costPathToNearOverNode = nodeFromStartCost + costNodeToNear;
 
            //is this a better path
            if (costPathToNearOverNode < costPathToNearOld)
            {
                //check collision
                const auto it = isCollisionFreeCache.find(nodeNearId);
 
                if ((it != isCollisionFreeCache.end() && (*it).second) || isPathCollisionFree(cfgNode, cfgNodeNear))
                {
                    //rewire node
                    tree.setNodeParent(nodeNearId, nodeId, costNodeToNear);
                }
 
            }
        }
    }
 
    void WorkerNode::findBestParent(
        const NodeId& nodeNearestId,
        const ConfigType& cfgReached,
        NodeId& outNodeBestId,
        float& outCostReachedToNodeBest,
        std::vector<std::pair<NodeId, float>>& outKNearestIdsAndDistances,
        std::map<NodeId, bool>& outIsCollisionFreeCache
    )
    {
        // //find parent
        outNodeBestId = nodeNearestId;
        outCostReachedToNodeBest = distance(tree.getNode(outNodeBestId).config, cfgReached);
        auto costPathToReachedOverNodeBest = tree.getNode(nodeNearestId).fromStartCost + outCostReachedToNodeBest;
        outKNearestIdsAndDistances = tree.getKNearestNeighboursAndDistances(cfgReached, getK());
 
        for (const auto& nearIdAndDist : outKNearestIdsAndDistances)
        {
 
            auto& nodeNearId = nearIdAndDist.first;
            auto& nodeNear = tree.getNode(nodeNearId);
            const auto& cfgNodeNear = nodeNear.config;
 
            const auto costReachedToNodeNear = nearIdAndDist.second;
            const auto costPathToReachedOverNodeNear = nodeNear.fromStartCost +  costReachedToNodeNear;
 
            if (costPathToReachedOverNodeNear < costPathToReachedOverNodeBest)
            {
                //check collision
                if (isPathCollisionFree(cfgNodeNear, cfgReached))
                {
                    outNodeBestId = nodeNearId;
                    costPathToReachedOverNodeBest = costPathToReachedOverNodeNear;
                    outCostReachedToNodeBest = costReachedToNodeNear;
                    outIsCollisionFreeCache[nodeNearId] = true;
                }
                else
                {
                    outIsCollisionFreeCache[nodeNearId] = false;
                }
            }
        }
    }
 
    WorkerNode::ConfigType WorkerNode::steer(const ConfigType& from, const ConfigType& to)
    {
        ARMARX_CHECK_EXPRESSION(getDimensionality() == from.size());
        ARMARX_CHECK_EXPRESSION(getDimensionality() == to.size());
 
        return dcdSteer(from, to, DCDStepSize, [this](const ConfigType & cfg)
        {
            return isCollisionFree(cfg);
        });
    }
 
    bool WorkerNode::isPathCollisionFree(const ConfigType& from, const ConfigType& to)
    {
        bool tmp = dcdIsPathCollisionFree(from, to, DCDStepSize, [this](const ConfigType & cfg)
        {
            return isCollisionFree(cfg);
        });//steer(from, to) == to;
        return tmp;
    }
 
    bool WorkerNode::isCollisionFree(const ConfigType& cfg)
    {
        bufferCDRange.first = cfg.data();
        bufferCDRange.second = bufferCDRange.first + cfg.size();
        return cspace->isCollisionFree(bufferCDRange);
    }
 
    std::size_t WorkerNode::getK()
    {
        const auto dim = static_cast<float>(getDimensionality());
        const auto kRRT = std::pow(2.f, dim + 1.f) * M_E * (1.f + 1.f / dim);
        const auto k = kRRT * std::log(static_cast<float>(tree.size()));
        return static_cast<std::size_t>(std::ceil(k));
    }
 
    Update WorkerNode::getUpdate(Ice::Long updateId, const Ice::Current&) const
    {
        std::lock_guard<std::mutex> lock {updateMutex};
        ARMARX_CHECK_EXPRESSION(updateId >= 0);
        ARMARX_CHECK_EXPRESSION(updateId <= tree.getPreviousUpdateId());
        return tree.getLocalUpdates().at(static_cast<std::size_t>(updateId));
    }
 
    Update WorkerNode::getRemoteUpdate(Ice::Long workerNodeId, Ice::Long updateSubId)
    {
        return manager->getUpdate(workerNodeId, updateSubId);
    }
}