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 "Task.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotComponents/interface/components/MotionPlanning/DataStructures.h>
#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
 
#include "../../util/Metrics.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();
    }
} // namespace armarx::addirrtstar