Task.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 <ArmarXCore/core/system/FactoryCollectionBase.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/Task.h>
 
#include "../../ResourceRequestStrategies/ComputingPowerRequestStrategy.h"
#include "../../util/PlanningUtil.h"
#include "../CPRSAwareMotionPlanningTask.h"
#include "util.h"
 
namespace armarx::addirrtstar
{
    class Task;
    /**
    * @brief An ice handle for an addirrt* \ref Task
    */
    using TaskPtr = IceInternal::Handle<Task>;
 
    /**
    * @brief An addirrt* task.
    * The olanning algorithm used is a combination of:
    *  - bulk distributed rrt.
    *  - informed rrt*
    *  - adaptive dynamic domain
    *
    * bulks and batches are used as synonyms.
    */
    class Task :
        public virtual cprs::CPRSAwareMotionPlanningTask,
        public virtual TaskBase,
        public virtual MotionPlanningMultiPathWithCostTaskCI
    {
    public:
        /**
        * @brief Ctor.
        * @param cspace The planning cspace.
        * @param planningComputingPowerRequestStrategy  The used cprs.
        * @param startCfg The start configuration.
        * @param goalCfg The goal configuration.
        * @param addParams The parameters for adaptive dynamic domain.
        * @param targetCost The target cost. (planning stops when a path with a length <= was found)
        * @param dcdStep The dcd step size.
        * @param maximalPlanningTimeInSeconds The maximal planning time in seconds. (planning will stop after this time)
        * @param batchSize The size of a batch.
        * @param nodeCountDeltaForGoalConnectionTries Number of nodes created (by a worker) before a connect to the goal node is tried (by this worker).
        * @param initialWorkerCount The in itaial number of worker processes.
        * @param maximalWorkerCount The maximal number of worker processes.
        */
        Task( //problem
            CSpaceBasePtr cspace,
            const cprs::ComputingPowerRequestStrategyBasePtr& planningComputingPowerRequestStrategy,
            VectorXf startCfg,
            VectorXf goalCfg,
            const std::string& taskName = "ADDIRRTStarTask",
            Ice::Long maximalPlanningTimeInSeconds = 300,
            AdaptiveDynamicDomainParameters addParams = generateADDParamsFromDCDStepsize(0.01f),
            float targetCost = 0,
            //general
            float dcdStep = 0.01f,
            Ice::Long batchSize = 10,
            Ice::Long nodeCountDeltaForGoalConnectionTries = 50,
            //management
            Ice::Long initialWorkerCount = 1,
            Ice::Long maximalWorkerCount = std::numeric_limits<Ice::Long>::max());
 
        PathWithCost
        getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return MotionPlanningMultiPathWithCostTaskCI::getPathWithCost();
        }
 
        Path
        getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
        {
            return MotionPlanningMultiPathWithCostTaskCI::getNthPath(n);
        }
 
        Path
        getPath(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return MotionPlanningWithCostTaskCI::getPath();
        }
 
        //TaskControlInterface
        /**
        * @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 index,
                                        const Ice::Current& = Ice::emptyCurrent) const override;
        /**
        * @return All found paths.
        */
        PathWithCostSeq getAllPathsWithCost(const Ice::Current& = Ice::emptyCurrent) const override;
 
        //PlanningControlInterface
        /**
        * @brief Aborts the task.
        */
        void abortTask(const Ice::Current& = Ice::emptyCurrent) override;
 
        //PlanningTaskBase
        /**
        * @brief Runs the task.
        * @param remoteNodes The list of \ref RemoteObjectNodeInterfacePrx used to distribute work to computers.
        */
        void run(const RemoteObjectNodePrxList& remoteNodes,
                 const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
        * @brief Used by the manager to store its found paths.
        * @param newPathList The paths.
        */
        void setPaths(const PathWithCostSeq& newPathList,
                      const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
        * @return The current 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:
        template <class Base, class Derived>
        friend class ::armarx::GenericFactory;
 
        /**
        * @brief Checks for illegal parameters
        */
        void checkParameters();
 
        /**
        * @brief Ctor used by object factories.
        */
        Task() : TaskBase(), cachedNodeCount{0}
        {
        }
 
        /**
        * @brief Mutex to protect internal structures.
        */
        mutable std::recursive_mutex mutex;
        //we have to ensure that the waiting thread has only locked the recursive mutex once,
        //since the condition variable only will use the unlock method on the unique_lock once during the wait.
        //http://stackoverflow.com/questions/14323340/can-you-combine-stdrecursive-mutex-with-stdcondition-variable
        //
        //this is given for run
        /**
        * @brief CV used by the dispatcher thread to wait until planning is done.
        */
        std::condition_variable_any managerDone;
 
        /**
        * @brief The manager node.
        */
        RemoteHandle<ManagerNodeBasePrx> manager;
        /**
        * @brief All found paths
        */
        PathWithCostSeq paths;
 
        /**
        * @brief The cahced node count. The cache is filled when the manager node shuts down.
        */
        Ice::Long cachedNodeCount;
    };
} // namespace armarx::addirrtstar
 
namespace armarx
{
    using ADDIRRTStarTask = addirrtstar::Task;
    using ADDIRRTStarTaskPtr = IceUtil::Handle<ADDIRRTStarTask>;
    using ADDIRRTStarTaskHandle =
        RemoteHandle<armarx::MotionPlanningMultiPathWithCostTaskControlInterfacePrx>;
} // namespace armarx