MotionPlanningTask.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 <deque>
#include <functional>
 
#include <Ice/ObjectAdapter.h>
 
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/distributed/AMDCallbackCollection.h>
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
 
#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
 
#include "../MotionPlanningServer.h"
#include "MotionPlanningTaskControlInterface.h"
 
namespace armarx
{
    class MotionPlanningTask;
    using MotionPlanningTaskPtr = IceInternal::Handle<MotionPlanningTask>;
    using MotionPlanningTaskHandle = RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
 
    class MotionPlanningTaskWithDefaultMembers;
    using MotionPlanningTaskWithDefaultMembersPtr =
        IceInternal::Handle<MotionPlanningTaskWithDefaultMembers>;
    using MotionPlanningTaskHandle = RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
 
    class PostprocessingMotionPlanningTask;
    using PostprocessingMotionPlanningTaskPtr =
        IceInternal::Handle<PostprocessingMotionPlanningTask>;
    using PostprocessingMotionPlanningTaskHandle =
        RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
 
    class MotionPlanningTask :
        virtual public MotionPlanningTaskBase,
        virtual public MotionPlanningTaskCI
    {
        friend class MotionPlanningServer;
 
    public:
        MotionPlanningTask() = default;
 
        ~MotionPlanningTask() override = default;
 
        /**
        * @brief Called by the planning server after the task was enqueued.
        * Override this function to prepare some work (e.g. setting the status from eNew to eQueued)
        */
        virtual void
        postEnqueueing()
        {
            setTaskStatus(TaskStatus::eQueued);
        }
 
        virtual void
        onPlanningDone()
        {
        }
 
        virtual void
        onRefiningDone()
        {
        }
 
        MotionPlanningTaskBasePrx&
        getProxy()
        {
            return selfProxy;
        }
 
        bool setTaskStatus(TaskStatus::Status newTaskStatus,
                           const Ice::Current& = Ice::emptyCurrent) override;
 
        TaskStatus::Status
        getTaskStatus(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return taskStatus;
        }
 
        virtual void
        registerAtIceAdapter(Ice::ObjectAdapterPtr& adapter, const Ice::Identity ident)
        {
            if (selfProxy)
            {
                throw std::logic_error{"Task already registered as: category: " +
                                       selfProxy->ice_getIdentity().category +
                                       ", name: " + selfProxy->ice_getIdentity().name};
            }
            selfProxy = MotionPlanningTaskBasePrx::uncheckedCast(adapter->add(this, ident));
        }
 
        std::string
        getTaskName(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return taskName;
        }
 
        void
        addTaskStatusCallback(std::function<void(TaskStatus::Status)> cb)
        {
            taskStatusCallbacks.emplace_back(cb);
        }
 
        Ice::Long
        getPlanningTime(const Ice::Current&) const override
        {
            return planningTime;
        }
 
        Ice::Long
        getRefiningTime(const Ice::Current&) const override
        {
            return refiningTime;
        }
 
        Ice::Long
        getRunningTime(const Ice::Current&) const override
        {
            return planningTime + refiningTime;
        }
 
    protected:
        /**
         * The planning time in microseconds
         */
        Ice::Long planningTime = 0;
        /**
         * The refining time in microseconds
         */
        Ice::Long refiningTime = 0;
 
    private:
        /**
        * @brief A self proxy. (may be required for callbacks)
        */
        MotionPlanningTaskBasePrx selfProxy;
        std::atomic<TaskStatus::Status> taskStatus{TaskStatus::eNew};
        std::deque<std::function<void(TaskStatus::Status)>> taskStatusCallbacks;
    };
 
    class MotionPlanningTaskWithDefaultMembers :
        virtual public MotionPlanningTaskWithDefaultMembersBase,
        virtual public MotionPlanningTask
    {
    public:
        /**
        * @brief ctor
        * @param startCfg the start point
        * @param cspace the planning cspace
        * @param dcdStep the dcd step size
        * @param maximalPlanningTimeInSeconds the maximal time in seconds
        */
        MotionPlanningTaskWithDefaultMembers(const VectorXf& startCfg,
                                             const VectorXf& goalCfg,
                                             const CSpaceBasePtr& cspace,
                                             Ice::Float dcdStep,
                                             Ice::Long maximalPlanningTimeInSeconds,
                                             const std::string& taskName) :
            MotionPlanningTaskBase(taskName),
            MotionPlanningTaskWithDefaultMembersBase(taskName,
                                                     startCfg,
                                                     goalCfg,
                                                     cspace,
                                                     dcdStep,
                                                     maximalPlanningTimeInSeconds)
        {
        }
 
        /**
        * @return The task's start configuration.
        */
        VectorXf
        getStart(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return startCfg;
        }
 
        /**
        * @return The task's start configuration.
        */
        VectorXf
        getGoal(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return goalCfg;
        }
 
        /**
        * @return The task's CSpace .
        */
        CSpaceBasePtr
        getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return cspace;
        }
 
        /**
        * @return The task's  .
        */
        float
        getDcdStep(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return dcdStep;
        }
 
        /**
        * @return The task's  .
        */
        Ice::Long
        getMaximalPlanningTimeInSeconds(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return maximalPlanningTimeInSeconds;
        }
 
    protected:
        MotionPlanningTaskWithDefaultMembers() = default;
    };
 
    class PostprocessingMotionPlanningTask :
        virtual public MotionPlanningTask,
        virtual public PostprocessingMotionPlanningTaskBase
    {
    public:
        PostprocessingMotionPlanningTask(const MotionPlanningTaskBasePtr& previousStep,
                                         const std::string& taskName) :
            MotionPlanningTaskBase(taskName),
            PostprocessingMotionPlanningTaskBase(taskName, previousStep)
        {
            ARMARX_CHECK_EXPRESSION(this->previousStep);
        }
 
        CSpaceBasePtr
        getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
        {
            ARMARX_CHECK_EXPRESSION(previousStep->getCSpace());
            return previousStep->getCSpace();
        }
 
        void
        registerAtIceAdapter(Ice::ObjectAdapterPtr& adapter, const Ice::Identity ident) override
        {
            MotionPlanningTask::registerAtIceAdapter(adapter, ident);
 
            auto prev = MotionPlanningTaskPtr::dynamicCast(previousStep);
            ARMARX_CHECK_EXPRESSION(prev);
 
            Ice::Identity subIdent;
            subIdent.name = ManagedIceObject::generateSubObjectName(
                ident.category + ident.name, "PreviousStep_" + previousStep->ice_id());
            prev->registerAtIceAdapter(adapter, subIdent);
        }
 
        void
        postEnqueueing() override
        {
            MotionPlanningTask::postEnqueueing();
            MotionPlanningTaskPtr::dynamicCast(previousStep)->postEnqueueing();
        }
 
    protected:
        PostprocessingMotionPlanningTask() = default;
    };
} // namespace armarx