MotionPlanningServer.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 <Ice/ObjectAdapter.h>
 
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/system/DynamicLibrary.h>
#include <ArmarXCore/core/CoreObjectFactories.h>
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include "Tasks/MotionPlanningTask.h"
 
#include "MotionPlanningServer.h"
#include "util/PlanningUtil.h"
 
namespace armarx
{
 
    void MotionPlanningServer::onInitComponent()
    {
        setTag("Server");
 
        const std::string remNames = getProperty<std::string>("RemoteObjectNodes").getValue();
        startLocalNode = getProperty<bool>("LocalRONStart").getValue();
        localNodePercent = getProperty<float>("LocalRONCorePerc").getValue();
        remoteObjectNodeNames = Split(remNames, ";,");
        //del empty names
        remoteObjectNodeNames.resize(
            std::distance(
                remoteObjectNodeNames.begin(),
                std::remove_if(remoteObjectNodeNames.begin(), remoteObjectNodeNames.end(), [](const std::string & s)
        {
            return s == "";
        })
            )
        );
 
        for (const auto& name : remoteObjectNodeNames)
        {
            ARMARX_VERBOSE_S << "Using RemoteObjectNode with name: " << name;
            usingProxy(name);
        }
 
 
        //start dispatcher
        dispatcherThread = std::thread {[this]{this->dispatcherTask();}};
    }
 
    void MotionPlanningServer::onConnectComponent()
    {
        remoteObjectNodes.clear();
 
        for (const auto& name : remoteObjectNodeNames)
        {
            remoteObjectNodes.emplace_back(getProxy<armarx::RemoteObjectNodeInterfacePrx>(name));
        }
 
        if (remoteObjectNodeNames.empty())
        {
            if (startLocalNode)
            {
                localNode = Component::create<RemoteObjectNode>(getIceProperties(), generateSubObjectName("RemoteObjectNode"));
                localNode->setCoreCount(localNodePercent * localNode->getDefaultCoreCount());
                //registration is required since a proxy is required
                getArmarXManager()->addObject(localNode);
                remoteObjectNodes.emplace_back(RemoteObjectNodeInterfacePrx::uncheckedCast(localNode->getProxy(-1)));
            }
            else
            {
                ARMARX_WARNING_S << "Starting motion planning server without any remote object nodes! (some algorithms may still work)";
            }
        }
        auto wm = getProxy<memoryx::WorkingMemoryInterfacePrx>("WorkingMemory", false, "", false);
        auto cs = getProxy<memoryx::CommonStorageInterfacePrx>("CommonStorage", false, "", false);
        auto rsc = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent", false, "", false);
        if (wm && cs & rsc)
        {
            cacheCSpace = SimoxCSpace::PrefetchWorkingMemoryObjects(wm, cs, rsc);
        }
    }
 
    void MotionPlanningServer::onExitComponent()
    {
        //request kill on dispatcher
        dispatcherKillRequest = true;
        {
            //start aborting current task since this could take a while
            std::lock_guard<std::recursive_mutex> lock {queueMutex};
 
            //abort all queued tasks
            for (auto taskId : taskQueue)
            {
                tasks.at(taskId).task->abortTask();
            }
            taskQueue.clear();
 
            for (auto& task : tasks)
            {
                task.second.rh->forceDeletion();
            }
 
            tasks.clear();
        }
        //join dispatcher
        waitForTaskOrDispatcherKill.notify_all();
        ARMARX_CHECK_EXPRESSION(dispatcherThread.joinable());
        dispatcherThread.join();
        ARMARX_VERBOSE_S << "joined dispatcher";
        //del local node
        if (localNode)
        {
            getArmarXManager()->removeObjectNonBlocking(localNode);
        }
    }
 
    void MotionPlanningServer::dispatcherTask()
    {
        decltype(remoteObjectNodes) ronCache;
        while (true)
        {
            if (dispatcherKillRequest)
            {
                break;
            }
 
            //the task may be deleted concurrently from the map
            //keep the smartpointer to ensure the task handle to stay alive until we are done with it
            TaskAndRemoteHandle handle;
            decltype(taskQueue)::value_type taskId; //simple to change taskids later
            {
                std::unique_lock<std::recursive_mutex> lock {queueMutex};
 
                if (taskQueue.empty())
                {
                    //no task pending. so wait for a signal
                    waitForTaskOrDispatcherKill.wait(lock);
                    //could be woken up by kill request, rando wakeup or a new task=> redo checks
                    //since we perform a special action on dispatcherKillRequest we cant use a predicate
                    continue;
                }
 
                //there is a task to perform
                //we have to save the id since deleteTaskById deletes the id from the queue
                taskId = *taskQueue.begin();
                handle = tasks.at(taskId);
            }
            //if disconnect+connect is called the vector may change. to remove the requirement of sync: copy
            ronCache = remoteObjectNodes;
 
            //what every task prints:
#define task_info    "TASK FAILED!"\
            << "\n\tname: " << handle.task->getTaskName()\
            << "\n\tID: " << taskId\
            << "\n\tice id = " << handle.task->ice_id()\
            << "\n\tstatus " << TaskStatus::toString(handle.task->getTaskStatus())
 
            try
            {
                ARMARX_CHECK_EXPRESSION(handle.task);
                currentTask.rh = handle.rh;
                currentTask.task = handle.task;
                auto start = IceUtil::Time::now();
                MotionPlanningTaskPtr t = handle.task;
                while (t)
                {
                    auto tp = t.get();
                    t->addTaskStatusCallback([tp, start, this](TaskStatus::Status s)
                    {
                        if (s == TaskStatus::eRefining)
                        {
                            auto duration = IceUtil::Time::now() - start;
                            tp->planningTime = duration.toMicroSeconds();
                        }
                        if (TaskStatus::finishedRunning(s))
                        {
                            auto duration = IceUtil::Time::now() - start;
                            if (tp->planningTime > 0)
                            {
                                tp->refiningTime = duration.toMicroSeconds() - tp->planningTime;
                            }
                            else
                            {
                                tp->planningTime = duration.toMicroSeconds();
                            }
                            ARMARX_VERBOSE << tp->getTaskName() << ": Planning time: " << tp->planningTime * 0.001f << " ms, " << tp->refiningTime * 0.001f << " ms";
                        }
                    });
                    PostprocessingMotionPlanningTaskPtr tmp = PostprocessingMotionPlanningTaskPtr::dynamicCast(t);
                    if (tmp)
                    {
                        t = MotionPlanningTaskPtr::dynamicCast(tmp->previousStep);
                    }
                    else
                    {
                        t = nullptr;
                    }
                }
                handle.task->run(ronCache);
            }
            catch (Ice::Exception& e)
            {
                ARMARX_ERROR_S << task_info
                               << "\n\tWHAT:\n" << e.what()
                               << "\n\tSTACK:\n" << e.ice_stackTrace();
                handle.task->setTaskStatus(TaskStatus::eException);
            }
            catch (std::exception& e)
            {
                ARMARX_ERROR_S << task_info
                               << "\n\tWHAT:\n" << e.what();
                handle.task->setTaskStatus(TaskStatus::eException);
            }
            catch (...)
            {
                ARMARX_ERROR_S << task_info
                               << "\n\tsomething not derived from std::exception was thrown";
                handle.task->setTaskStatus(TaskStatus::eException);
            }
            currentTask.rh = nullptr;
            currentTask.task = nullptr;
#undef task_info
            //task done dequeue it
            {
                std::lock_guard<std::recursive_mutex> lock {queueMutex};
                taskQueue.erase(taskId);
            }
            //loop to wait for next task or kill
        }
        ARMARX_DEBUG << "dispatcher task exit";
    }
 
    bool MotionPlanningServer::deleteTaskById(Ice::Long id)
    {
        std::lock_guard<std::recursive_mutex> lock {queueMutex};
        auto it = tasks.find(id);
 
        if (it == tasks.end())
        {
            ARMARX_VERBOSE_S << "no task with id " << id << " to delete";
            return false;
        }
 
        //request task kill
        it->second.task->abortTask();
 
        //don't remove it from the adapter yet. the proxy may be required to pass data to the task object.
        //the task will remove it self from the adapter;
 
        taskQueue.erase(id);
        tasks.erase(it);
        return true;
    }
 
    armarx::ClientSideRemoteHandleControlBlockBasePtr MotionPlanningServer::enqueueTask(const MotionPlanningTaskBasePtr& task, const Ice::Current&)
    {
        if (dispatcherKillRequest)
        {
            throw armarx::ServerShuttingDown {"MotionPlanningServerComponent"};
        }
 
        MotionPlanningTaskPtr planningTask = MotionPlanningTaskPtr::dynamicCast(task);
        ARMARX_CHECK_EXPRESSION(planningTask);
        //register the task and add the data to unregister the task
        auto adapter = getArmarXManager()->getAdapter();
 
        auto id = getNewTaskId();
 
        Ice::Identity ident;
        ident.name = generateSubObjectName(task->getTaskName() + "_ID_" + to_string(id));
        planningTask->registerAtIceAdapter(adapter, ident);
 
        //add the task
 
        auto remoteHandleHandles = armarx::RemoteHandleControlBlock::create(
                                       getArmarXManager(),
                                       MotionPlanningTaskControlInterfacePrx::uncheckedCast(planningTask->getProxy()),
                                       [id, this, adapter, ident]
        {
            adapter->remove(ident);
            //del from server
            deleteTaskById(id);
        }
                                   );
 
        {
            std::lock_guard<std::recursive_mutex> lock {queueMutex};
            ARMARX_CHECK_EXPRESSION(planningTask);
            tasks[id] = TaskAndRemoteHandle
            {
                remoteHandleHandles.directHandle,
                planningTask
            };
            taskQueue.insert(id);
            planningTask->postEnqueueing();
        }
        //wake up dispatcher
        //holding the mutex a cv is holding on is inefficient since it may cause additional thread switches
        waitForTaskOrDispatcherKill.notify_all();
        return remoteHandleHandles.clientSideRemoteHandleControlBlock;
    }
 
    Ice::Long MotionPlanningServer::getNumberOfTasks(const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock {queueMutex};
        return tasks.size();
    }
 
    Ice::Long MotionPlanningServer::getNumberOfQueuedTasks(const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock {queueMutex};
        return taskQueue.size();
    }
 
    TaskInfoSeq MotionPlanningServer::getAllTaskInfo(const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock {queueMutex};
        TaskInfoSeq result;
        result.reserve(tasks.size());
        transform(
            tasks.begin(), tasks.end(), std::back_inserter(result),
            [](const decltype(tasks)::value_type & v)
        {
            const TaskAndRemoteHandle& rhs = v.second;
            //ident, status, iceType
            return TaskInfo
            {
                v.first,
                rhs.rh->getManagedObjectProxy()->ice_getIdentity(),
                rhs.task->getTaskStatus(),
                rhs.task->ice_id(),
                rhs.task->getTaskName()
            };
        });
        return result;
    }
 
    ClientSideRemoteHandleControlBlockBasePtr MotionPlanningServer::getCurrentTaskHandle(const Ice::Current&)
    {
        ClientSideRemoteHandleControlBlockBasePtr remoteHandle;
        if (currentTask.rh)
        {
            remoteHandle = currentTask.rh->getClientSideRemoteHandleControlBlock();
        }
        return remoteHandle;
    }
 
    CSpaceAndPaths MotionPlanningServer::getTaskCSpaceAndPathsById(Ice::Long id, const Ice::Current&)
    {
        CSpaceAndPaths result;
        std::lock_guard<std::recursive_mutex> lock {queueMutex};
        auto it = tasks.find(id);
        if (it != tasks.end())
        {
            auto task = MotionPlanningTaskBasePtr::dynamicCast(it->second.task);
 
            result.cspace   = task ? task->getCSpace()->clone() : nullptr;
            auto multPathTask = MotionPlanningMultiPathTaskControlInterfacePtr::dynamicCast(task);
            if (multPathTask)
            {
                result.paths = multPathTask->getAllPaths();
            }
            else
            {
                result.paths.emplace_back(it->second.task->getPath());
            }
        }
        else
        {
            ARMARX_WARNING_S << "no task with id " << id << " on the server " << getName();
        }
        return result;
    }
 
 
    bool MotionPlanningServer::loadLibFromAbsolutePath(const std::string& path)
    {
        if (loadedLibs.count(path) > 0)
        {
            return true;
        }
        DynamicLibraryPtr lib(new DynamicLibrary());
        try
        {
            lib->load(path);
        }
        catch (...)
        {
            handleExceptions();
            return false;
        }
 
        if (lib->isLibraryLoaded())
        {
            loadedLibs[path] = lib;
        }
        else
        {
            ARMARX_ERROR << "Could not load lib " + path + ": " + lib->getErrorMessage();
            return false;
        }
 
        ArmarXManager::RegisterKnownObjectFactoriesWithIce(this->getIceManager()->getCommunicator());
        return true;
    }
 
    bool MotionPlanningServer::loadLibFromPath(const std::string& path, const Ice::Current&)
    {
        std::string absPath;
        if (ArmarXDataPath::getAbsolutePath(path, absPath))
        {
            return loadLibFromAbsolutePath(absPath);
        }
        else
        {
            ARMARX_ERROR << "Could not find library " + path;
            return false;
        }
    }
 
    bool MotionPlanningServer::loadLibFromPackage(const std::string& package, const std::string& name, const Ice::Current&)
    {
        CMakePackageFinder finder(package);
        if (!finder.packageFound())
        {
            ARMARX_ERROR << "Could not find package '" << package << "'";
            return false;
        }
 
        for (auto libDirPath : armarx::Split(finder.getLibraryPaths(), ";"))
        {
            std::filesystem::path fullPath = libDirPath;
            fullPath /= "lib" + name  + "." + DynamicLibrary::GetSharedLibraryFileExtension();
            if (!std::filesystem::exists(fullPath))
            {
                fullPath = libDirPath;
                fullPath /= name;
                if (!std::filesystem::exists(fullPath))
                {
                    continue;
                }
            }
            if (loadLibFromAbsolutePath(fullPath.string()))
            {
                return true;
            }
        }
        ARMARX_ERROR << "Could not find library " <<  name << " in package " << package;
        return false;
    }
}