MotionPlanningServer.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 <thread>
#include <unordered_map>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/RemoteObjectNode.h>
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandleControlBlock.h>
 
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
 
#include "Tasks/MotionPlanningTask.h"
 
namespace armarx
{
    class MotionPlanningTask;
    using MotionPlanningTaskPtr = IceInternal::Handle<MotionPlanningTask>;
    class DynamicLibrary;
    using DynamicLibraryPtr = std::shared_ptr<DynamicLibrary>;
 
    class MotionPlanningServer;
    /**
     * @brief A ice handle for a MotionPlanningServerComponent
     */
    using MotionPlanningServerComponentPtr = IceInternal::Handle<MotionPlanningServer>;
 
    /**
     * @brief Properties for a MotionPlanningServerComponent
     */
    class MotionPlanningServerPropertyDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        /**
         * @brief ctor
         * @param prefix the used prefix for properties
         */
        MotionPlanningServerPropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "RemoteObjectNodes", "", "CSV of RemoteObjectNode names");
            defineOptionalProperty<bool>(
                "LocalRONStart",
                true,
                "start a local remote object node if the CSV RemoteObjectNodes is empty");
            defineOptionalProperty<float>(
                "LocalRONCorePerc",
                0.5,
                "Local remote object node core count percentage. (has to be >0; 0.5 => 50%");
        }
    };
 
    /**
     * @defgroup Component-MotionPlanningServer MotionPlanningServer
     * @ingroup RobotComponents-Components
     * @brief Implementation of the slice interface MotionPlanningServerInterface
     * @see \ref RobotComponents-Tutorials-MotionPlanning
     * @see \ref RobotComponents-Tutorials-SimoxCSpace
     * @see \ref RobotComponents-GuiPlugins-SimoxCSpaceVisualizer
     */
    class MotionPlanningServer :
        virtual public MotionPlanningServerInterface,
        virtual public armarx::Component
    {
    public:
        /**
         * @brief ctor
         */
        MotionPlanningServer() = default;
 
        /**
         * @brief dtor
         */
        ~MotionPlanningServer() override
        {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wterminate"
            ARMARX_CHECK_EXPRESSION(tasks.empty());
            ARMARX_VERBOSE_S << "dtor";
            ARMARX_CHECK_EXPRESSION(!dispatcherThread.joinable());
#pragma GCC diagnostic pop
        }
 
        // inherited from Component
        /**
         * @brief Returns the server's default name.
         * @return The server's default name.
         */
        std::string
        getDefaultName() const override
        {
            return "MotionPlanningServer";
        }
 
        /**
         * @brief Initializes the server and starts the dispatcher thread
         */
        void onInitComponent() override;
        /**
         * @brief Connects to the used RemoteObjectNodes
         */
        void onConnectComponent() override;
 
        /**
         * @brief noop
         */
        void
        onDisconnectComponent() override
        {
        }
 
        /**
         * @brief cleans up and joins the dispatcher
         */
        void onExitComponent() override;
 
        /**
         */
        /**
         * @see PropertyUser::createPropertyDefinitions()
         * @return The planning server's PropertyDefinitions
         */
        armarx::PropertyDefinitionsPtr
        createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr(
                new MotionPlanningServerPropertyDefinitions(getConfigIdentifier()));
        }
 
        //from MotionPlanningServerInterface
        /**
         * @brief Returns a task's cspace and all found paths.
         * @param id The id.
         * @return The task's cspace and all found paths.
         */
        CSpaceAndPaths getTaskCSpaceAndPathsById(Ice::Long id,
                                                 const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Enqueues a task
         * @param task The task.
         * @return The task's proxy.
         */
        armarx::ClientSideRemoteHandleControlBlockBasePtr
        enqueueTask(const MotionPlanningTaskBasePtr& task,
                    const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Returns the number of tasks.
         * @return The number of tasks.
         */
        Ice::Long getNumberOfTasks(const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Returns the number of queued tasks.
         * @return The number of queued tasks.
         */
        Ice::Long getNumberOfQueuedTasks(const Ice::Current&) const override;
 
        /**
         * @brief Returns information about all tasks.
         * @return Information about all tasks.
         */
        TaskInfoSeq getAllTaskInfo(const Ice::Current& = Ice::emptyCurrent) const override;
 
        ClientSideRemoteHandleControlBlockBasePtr
        getCurrentTaskHandle(const Ice::Current&) override;
 
        bool loadLibFromPath(const std::string& path,
                             const Ice::Current& = Ice::emptyCurrent) override;
        bool loadLibFromPackage(const std::string& package,
                                const std::string& lib,
                                const Ice::Current& = Ice::emptyCurrent) override;
 
    protected:
        bool deleteTaskById(Ice::Long id);
        /**
         * @brief the dispatcher task. (executed by the dispatcherThread)
         */
        void dispatcherTask();
 
        /**
         * @brief the next task id.
         */
        std::atomic<Ice::Long> nextTaskId{std::numeric_limits<Ice::Long>::min()};
 
        /**
         * @brief Returns a new task id.
         * @return A new task id.
         */
        Ice::Long
        getNewTaskId()
        {
            return nextTaskId++;
        }
 
        /**
         * @brief The used remote object nodes' names
         */
        std::vector<std::string> remoteObjectNodeNames;
        /**
         * @brief The used remote object nodes
         */
        std::vector<armarx::RemoteObjectNodeInterfacePrx> remoteObjectNodes;
 
        /**
         * @brief Whether a local node should be started, if no rons were provided.
         */
        bool startLocalNode;
        /**
         * @brief Percentage of cores used by the local ron (if started).
         */
        float localNodePercent;
 
        RemoteObjectNodePtr localNode;
 
        struct TaskAndRemoteHandle
        {
            armarx::RemoteHandleControlBlockPtr rh;
            MotionPlanningTaskPtr task;
        };
 
        /**
         * @brief The type of the map of tasks
         */
        using TaskMap = std::unordered_map<Ice::Long, TaskAndRemoteHandle>;
        /**
         * @brief The map of tasks.
         */
        TaskMap tasks;
 
        /**
         * @brief currentTask stores a handle to the currently dispatched planning task.
         */
        TaskAndRemoteHandle currentTask;
 
        /**
         * @brief Contains the queue of task ids to execute.
         *
         * If not empty the first id is the next task to execute
         * A set is used since the queue lacks an erase function and ids are
         * added in ascending order.
         */
        std::set<Ice::Long> taskQueue;
 
        /**
         * @brief mutex protecting the task queue
         */
        mutable std::recursive_mutex queueMutex;
 
        //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 dispatcherThread
        /**
         * @brief The dispatcher's cv
         */
        std::condition_variable_any waitForTaskOrDispatcherKill;
        /**
         * @brief The dispatcher's thread
         */
        std::thread dispatcherThread;
        /**
         * @brief Whether the dispatcher should shut down
         */
        std::atomic_bool dispatcherKillRequest{false};
 
        std::map<std::string, DynamicLibraryPtr> loadedLibs;
 
        SimoxCSpacePtr cacheCSpace;
 
 
    private:
        bool loadLibFromAbsolutePath(const std::string& path);
    };
 
} // namespace armarx