Task.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 <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include "../../util/Metrics.h"
#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
#include <RobotComponents/interface/components/MotionPlanning/DataStructures.h>
 
#include "Task.h"
#include "ManagerNode.h"
 
namespace armarx::addirrtstar
{
    PathWithCost Task::getBestPath(const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock {mutex};
        if (finishedRunning())
        {
            return paths.at(0);
        }
 
        if (isRunning())
        {
            return manager->getBestPath();
        }
 
        //neither finished nor planning => did not start planning
        PathWithCost noPathSentinel;
        noPathSentinel.cost = std::numeric_limits<Ice::Float>::infinity();
        return noPathSentinel;
    }
    Ice::Long Task::getPathCount(const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock {mutex};
 
        if (isRunning())
        {
            return manager->getPathCount();
        }
 
        return paths.size();
    }
 
    PathWithCost Task::getNthPathWithCost(Ice::Long index, const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock {mutex};
 
        if ((index < 0) || index >= getPathCount())
        {
            throw IndexOutOfBoundsException {};
        }
 
        ARMARX_CHECK_EXPRESSION(index >= 0);
 
        if (isRunning())
        {
            ARMARX_CHECK_EXPRESSION(index < manager->getPathCount());
            return manager->getNthPathWithCost(index);
        }
 
        ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(index) < paths.size());
        return paths.at(index);
    }
 
    PathWithCostSeq Task::getAllPathsWithCost(const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock {mutex};
 
        if (isRunning())
        {
            return manager->getAllPathsWithCost();
        }
 
        return paths;
    }
 
    //PlanningControlInterface
    void Task::abortTask(const Ice::Current&)
    {
        std::lock_guard<std::recursive_mutex> lock {mutex};
 
        if (getTaskStatus() == TaskStatus::eQueued || getTaskStatus() == TaskStatus::eNew)
        {
            ARMARX_VERBOSE_S << "task stopped";
            //stop the task from executing
            setTaskStatus(TaskStatus::ePlanningAborted);
        }
 
        if (finishedRunning())
        {
            ARMARX_VERBOSE_S << "task already stopped";
            //noop
            return;
        }
 
        ARMARX_VERBOSE_S << "stopping manager";
        //should be planning
        ARMARX_CHECK_EXPRESSION(isRunning());
        ARMARX_CHECK_EXPRESSION(manager);
        //manager will set the correct status and wake run up
        manager->kill();
        ARMARX_VERBOSE_S << "stopped manager";
    }
 
    //PlanningTaskBase
    void Task::run(const RemoteObjectNodePrxList& remoteNodes, const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(planningComputingPowerRequestStrategy);
        //since post unmarshall is of this is called BEFORE post unmarshall of the member cspace is called this check can only be done here!
        const auto dim = static_cast<std::size_t>(cspace->getDimensionality());
        ARMARX_CHECK_EXPRESSION(startCfg.size() == dim);
        ARMARX_CHECK_EXPRESSION(goalCfg.size() == dim);
 
        std::unique_lock<std::recursive_mutex> lock {mutex};
 
        if (getTaskStatus() != TaskStatus::eQueued)
        {
            //tried to restart finished task or already running task
            ARMARX_WARNING_S << "Run was called with task status " << getTaskStatus();
            return;
        }
 
        //check for trivial cases
        ARMARX_CHECK_EXPRESSION(cspace);
        cspace->initCollisionTest();
 
        if (!(cspace->isCollisionFree(std::make_pair(startCfg.data(), startCfg.data() + startCfg.size()))))
        {
            ARMARX_VERBOSE_S << "trivial task! failed (start is not collision free): \n" << startCfg;
            setTaskStatus(TaskStatus::ePlanningFailed);
            return;
        }
 
        if (!(cspace->isCollisionFree(std::make_pair(goalCfg.data(), goalCfg.data() + startCfg.size()))))
        {
            ARMARX_VERBOSE_S << "trivial task! failed (goal is not collision free): \n" << goalCfg;
            setTaskStatus(TaskStatus::ePlanningFailed);
            return;
        }
 
        const auto distanceStartGoal = euclideanDistance(startCfg.begin(), startCfg.end(), goalCfg.begin());
        ARMARX_VERBOSE_S << "distance from start to goal = " << distanceStartGoal;
 
        if (distanceStartGoal < dcdStep)
        {
            ARMARX_VERBOSE_S << "trivial task! done(the distance from start to goal is smaller than the DCD step size)";
            paths.emplace_back(VectorXfSeq {startCfg, goalCfg}, distanceStartGoal, taskName + "_trivialPath_" + ice_staticId());
            setTaskStatus(TaskStatus::eDone);
            return;
        }
 
        //calculate real target cost (if a cost < distanceStartGoal is used set it to distanceStartGoal)
        targetCost = std::max(targetCost, distanceStartGoal);
        //check params
        ARMARX_CHECK_EXPRESSION(remoteNodes.size());
        RemoteObjectNodeInterfacePrx rmObjNode = remoteNodes.at(0); //copy the proxy since it is const
        ARMARX_CHECK_EXPRESSION(rmObjNode);
        //create manager node
        TaskBasePrx selfDerivedProxy = TaskBasePrx::uncheckedCast(getProxy());
        ManagerNodePtr localManagerNode
        {
            new ManagerNode{
                selfDerivedProxy, remoteNodes,
                initialWorkerCount, maximalWorkerCount, planningComputingPowerRequestStrategy,
                dcdStep, maximalPlanningTimeInSeconds, batchSize, nodeCountDeltaForGoalConnectionTries,
                CSpaceBasePtr::dynamicCast(cspace->clone()),
                startCfg, goalCfg,
                addParams, targetCost
            }
        };
        //register it
        std::stringstream remoteObjectName;
        remoteObjectName << localManagerNode->getName() << "@" << getProxy()->ice_getIdentity().name;
 
        ARMARX_DEBUG_S << "\n starting manager of task on " << rmObjNode
                       << "\n\t remoteObjectName = " << remoteObjectName.str();
 
        manager = rmObjNode->registerRemoteHandledObject(remoteObjectName.str(), localManagerNode);
        //set status
        setTaskStatus(TaskStatus::ePlanning);
        //wait
        ARMARX_VERBOSE_S << "waiting for manager";
        managerDone.wait(lock, [this] {return finishedRunning();});
        ARMARX_VERBOSE_S << "manager done";
 
        ARMARX_CHECK_EXPRESSION(finishedRunning());
        //get node count
        cachedNodeCount = manager->getNodeCount();
        //we have all required data from the manager
        manager = nullptr;
        ARMARX_VERBOSE_S << "done run";
    }
 
    void Task::setPaths(const PathWithCostSeq& newPathList, const Ice::Current&)
    {
        std::lock_guard<std::recursive_mutex> lock {mutex};
 
        paths = newPathList;
        std::sort(paths.begin(), paths.end(),  [](const PathWithCost & lhs, const PathWithCost & rhs)
        {
            return lhs.cost < rhs.cost;
        });
        for (std::size_t i = 0; i < paths.size(); ++i)
        {
            paths.at(i).pathName = taskName + "_path_" + to_string(i) + ice_staticId();
        }
    }
 
    void Task::checkParameters()
    {
        ARMARX_DEBUG_S << "\n checking parameters:"
                       << "\n\t initialWorkerCount " << initialWorkerCount
                       << "\n\t maximalWorkerCount " << maximalWorkerCount
                       << "\n\t dcdStep " << dcdStep
                       << "\n\t batchSize " << batchSize
                       << "\n\t cspace " << cspace.get()
                       << "\n\t addParams.alpha " << addParams.alpha
                       << "\n\t addParams.initialBorderRadius " << addParams.initialBorderRadius
                       << "\n\t addParams.minimalRadius " << addParams.minimalRadius
                       << "\n\t targetCost " << targetCost
                       << "\n\t dim cspace (-1 == null cspace)" << (cspace ? cspace->getDimensionality() : -1)
                       << "\n\t dim start " << startCfg.size()
                       << "\n\t dim goal " << goalCfg.size()
                       << "\n\t start " << startCfg
                       << "\n\t goal " << goalCfg;
 
        if (!(dcdStep > 0.f))
        {
            throw std::invalid_argument {"DCD stepsize <0"};
        }
 
        if (!(cspace))
        {
            throw std::invalid_argument {"cspace == nullptr"};
        }
 
        const auto dim = static_cast<std::size_t>(cspace->getDimensionality());
 
        if (!(startCfg.size() == dim))
        {
            throw std::invalid_argument {"Dimensions of cspace and start do not match"};
        }
 
        if (!(goalCfg.size() == dim))
        {
            throw std::invalid_argument {"Dimensions of cspace and goal do not match"};
        }
 
        if (!(addParams.alpha >= 0.f))
        {
            throw std::invalid_argument {"addParams.alpha < 0.f"};
        }
 
        if (!(addParams.minimalRadius > 0.f))
        {
            throw std::invalid_argument {"addParams.minimalRadius <= 0.f"};
        }
 
        if (!(addParams.initialBorderRadius > addParams.minimalRadius))
        {
            throw std::invalid_argument {"addParams.initialBorderRadius <= addParams.minimalRadius"};
        }
 
        if (nodeCountDeltaForGoalConnectionTries < 1)
        {
            throw std::invalid_argument {"nodeCountDeltaForGoalConnectionTries < 1"};
        }
        if (!planningComputingPowerRequestStrategy)
        {
            throw std::invalid_argument {"planningComputingPowerRequestStrategy == nullptr"};
        }
    }
 
    Task::Task(//problem
        CSpaceBasePtr cspace,
        const cprs::ComputingPowerRequestStrategyBasePtr& planningComputingPowerRequestStrategy,
        VectorXf startCfg,
        VectorXf goalCfg,
        const std::string& taskName,
        Ice::Long maximalPlanningTimeInSeconds,
        AdaptiveDynamicDomainParameters addParams,
        float targetCost,
        //general
        float dcdStep,
        Ice::Long batchSize,
        Ice::Long nodeCountDeltaForGoalConnectionTries,
        //management
        Ice::Long initialWorkerCount,
        Ice::Long maximalWorkerCount
    )
    {
        this->cspace = cspace;
        this->planningComputingPowerRequestStrategy = planningComputingPowerRequestStrategy;
        this->startCfg = std::move(startCfg);
        this->goalCfg = std::move(goalCfg);
        this->taskName = taskName;
        this->addParams = addParams;
        this->targetCost = targetCost;
        this->dcdStep = dcdStep;
        this->maximalPlanningTimeInSeconds = maximalPlanningTimeInSeconds;
        this->batchSize = batchSize;
        this->nodeCountDeltaForGoalConnectionTries = nodeCountDeltaForGoalConnectionTries;
        this->initialWorkerCount = initialWorkerCount;
        this->maximalWorkerCount = maximalWorkerCount;
        cachedNodeCount = 0;
        checkParameters();
    }
 
    Ice::Long Task::getNodeCount(const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock {mutex};
 
        if (isRunning())
        {
            return manager->getNodeCount();
        }
 
        ARMARX_VERBOSE_S << "fetching node count from cache";
        return cachedNodeCount;
    }
 
    void Task::setMaxCpus(Ice::Int maxCpus, const Ice::Current&)
    {
        manager->setMaxCpus(maxCpus);
    }
 
    Ice::Int Task::getMaxCpus(const Ice::Current&) const
    {
        return manager->getMaxCpus();
    }
}