NJointControllerBase.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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    RobotAPI::ArmarXObjects::NJointController
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <atomic>
#include <functional>
#include <map>
#include <mutex>
#include <optional>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h> // for ARMARX_CHECK
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
 
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
 
namespace IceProxy::armarx
{
    class DebugDrawerInterface;
    class DebugObserverInterface;
    class RobotUnitInterface;
} // namespace IceProxy::armarx
 
namespace armarx
{
    namespace RobotUnitModule
    {
        class NJointControllerAttorneyForPublisher;
        class NJointControllerAttorneyForControlThread;
        class NJointControllerAttorneyForControllerManagement;
    } // namespace RobotUnitModule
 
    namespace detail
    {
        template <class>
        class NJointControllerRegistryEntryHelper;
        class ControlThreadOutputBufferEntry;
    } // namespace detail
 
    namespace WidgetDescription
    {
        class Widget;
        typedef ::IceInternal::Handle<::armarx::WidgetDescription::Widget> WidgetPtr;
        typedef ::std::map<::std::string, ::armarx::WidgetDescription::WidgetPtr>
            StringWidgetDictionary;
    } // namespace WidgetDescription
 
    using SensorAndControl = detail::ControlThreadOutputBufferEntry;
    class JointController;
    class SensorValueBase;
    class ControlTargetBase;
    using ConstControlDevicePtr = std::shared_ptr<const class ControlDevice>;
    using ConstSensorDevicePtr = std::shared_ptr<const class SensorDevice>;
 
    typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface>
        DebugDrawerInterfacePrx;
    typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface>
        DebugObserverInterfacePrx;
    typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::RobotUnitInterface>
        RobotUnitInterfacePrx;
 
    using ThreadPoolPtr = std::shared_ptr<class ThreadPool>;
 
    TYPEDEF_PTRS_HANDLE(NJointControllerBase);
    TYPEDEF_PTRS_HANDLE(SynchronousNJointController);
    TYPEDEF_PTRS_HANDLE(AsynchronousNJointController);
 
    TYPEDEF_PTRS_HANDLE(RobotUnit);
 
    class NJointControllerBase :
        virtual public NJointControllerInterface,
        virtual public ManagedIceObject
    {
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////////// typedefs /////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        using ConfigPtrT = NJointControllerConfigPtr;
        using GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr (*)(
            const VirtualRobot::RobotPtr&,
            const std::map<std::string, ConstControlDevicePtr>& controlDevices,
            const std::map<std::string, ConstSensorDevicePtr>& sensorDevices);
        template <class ConfigPrtType>
        using GenerateConfigFromVariantsFunctionSignature =
            ConfigPrtType (*)(const StringVariantBaseMap&);
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////// constructor setup functions ////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        ConstSensorDevicePtr peekSensorDevice(const std::string& deviceName) const;
        ConstControlDevicePtr peekControlDevice(const std::string& deviceName) const;
 
        ControlTargetBase* useControlTarget(const std::string& deviceName,
                                            const std::string& controlMode);
 
        template <class T>
        T*
        useControlTarget(const std::string& deviceName, const std::string& controlMode)
        {
            static_assert(std::is_base_of<ControlTargetBase, T>::value,
                          "The given type does not derive ControlTargetBase");
            ControlTargetBase* const ptr = useControlTarget(deviceName, controlMode);
            return dynamic_cast<T*>(ptr);
        }
 
        const SensorValueBase* useSensorValue(const std::string& sensorDeviceName) const;
 
        template <class T>
        const T*
        useSensorValue(const std::string& deviceName) const
        {
            static_assert(std::is_base_of<SensorValueBase, T>::value,
                          "The given type does not derive SensorValueBase");
            const SensorValueBase* const ptr = useSensorValue(deviceName);
            return dynamic_cast<const T*>(ptr);
        }
 
        const VirtualRobot::RobotPtr& useSynchronizedRtRobot(bool updateCollisionModel = false);
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ////////////////////////////////// Component interface ///////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    protected:
        std::string
        getDefaultName() const override
        {
            return getClassName();
        }
 
        void onInitComponent() final;
        void onConnectComponent() final;
        void onDisconnectComponent() final;
        void onExitComponent() final;
 
        virtual void
        onInitNJointController()
        {
        }
 
        virtual void
        onConnectNJointController()
        {
        }
 
        virtual void
        onDisconnectNJointController()
        {
        }
 
        virtual void
        onExitNJointController()
        {
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ////////////////////////////////// ThreadPool functionality///////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    protected:
        ThreadPoolPtr getThreadPool() const;
 
        template <typename Task>
        void
        runTask(const std::string& taskName, Task&& task)
        {
            std::unique_lock lock(threadHandlesMutex);
            ARMARX_CHECK_EXPRESSION(!taskName.empty());
            ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
            ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
            ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName
                           << "' - current available thread count: "
                           << getThreadPool()->getAvailableTaskCount();
            auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task));
            ARMARX_CHECK_EXPRESSION(handlePtr->isValid())
                << "Could not add task (" << taskName << " - " << GetTypeString(task)
                << " ) - available threads: " << getThreadPool()->getAvailableTaskCount();
            threadHandles[taskName] = handlePtr;
        }
 
        std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;
        std::mutex threadHandlesMutex;
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// ice interface //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        bool
        isControllerActive(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return isActive;
        }
 
        bool
        isControllerRequested(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return isRequested;
        }
 
        bool
        isDeletable(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return deletable;
        }
 
        bool
        hasControllerError(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return deactivatedBecauseOfError;
        }
 
        std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override = 0;
 
        std::string
        getInstanceName(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return instanceName_;
        }
 
        NJointControllerDescription
        getControllerDescription(const Ice::Current& = Ice::emptyCurrent) const final override;
        NJointControllerStatus
        getControllerStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
        NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus(
            const Ice::Current& = Ice::emptyCurrent) const final override;
        //RobotUnitInterfacePrx getRobotUnit(const Ice::Current& = Ice::emptyCurrent) const final override;
 
        void activateController(const Ice::Current& = Ice::emptyCurrent) final override;
        void deactivateController(const Ice::Current& = Ice::emptyCurrent) final override;
        void deleteController(const Ice::Current& = Ice::emptyCurrent) final override;
        void deactivateAndDeleteController(const Ice::Current& = Ice::emptyCurrent) final override;
 
        WidgetDescription::StringWidgetDictionary
        getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return {};
        }
 
        void
        callDescribedFunction(const std::string&,
                              const StringVariantBaseMap&,
                              const Ice::Current& = Ice::emptyCurrent) override
        {
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// rt interface ///////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public: 
 
        const VirtualRobot::RobotPtr&
        rtGetRobot()
        {
            return rtRobot;
        }
 
        const std::vector<VirtualRobot::RobotNodePtr>&
        rtGetRobotNodes()
        {
            return rtRobotNodes;
        }
 
        bool
        rtUsesControlDevice(std::size_t deviceIndex) const
        {
            return controlDeviceUsedBitmap.at(deviceIndex);
        }
 
        const std::vector<std::size_t>&
        rtGetControlDeviceUsedIndices() const
        {
            return controlDeviceUsedIndices;
        }
 
        bool
        rtGetErrorState() const
        {
            return errorState;
        }
 
        std::size_t
        rtGetNumberOfUsedControlDevices() const
        {
            return controlDeviceUsedIndices.size();
        }
 
        const std::string&
        rtGetClassName() const
        {
            return rtClassName_;
        }
 
        const std::string&
        rtGetInstanceName() const
        {
            return instanceName_;
        }
 
    protected:
        virtual void
        rtPreActivateController()
        {
        }
 
        virtual void
        rtPostDeactivateController()
        {
        }
 
        void
        rtSetErrorState()
        {
            errorState.store(true);
        }
 
    private:
        void rtActivateController();
        void rtDeactivateController();
        void rtDeactivateControllerBecauseOfError();
 
    public:
        static const NJointControllerBasePtr NullPtr;
 
        NJointControllerBase();
 
        ~NJointControllerBase() override;
        //ice interface (must not be called in the rt thread)
 
        //c++ interface (local calls) (must be called in the rt thread)
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// used targets ///////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        //used control devices
        StringStringDictionary
        getControlDeviceUsedControlModeMap(
            const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return controlDeviceControlModeMap;
        }
 
        const std::vector<char>&
        getControlDeviceUsedBitmap() const
        {
            return controlDeviceUsedBitmap;
        }
 
        const std::vector<std::size_t>&
        getControlDeviceUsedIndices() const
        {
            return controlDeviceUsedIndices;
        }
 
        const std::map<std::string, const JointController*>&
        getControlDevicesUsedJointController()
        {
            return controlDeviceUsedJointController;
        }
 
        //check for conflict
        std::optional<std::vector<char>>
        isNotInConflictWith(const NJointControllerBasePtr& other) const
        {
            return isNotInConflictWith(other->getControlDeviceUsedBitmap());
        }
 
        std::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
 
        template <class ItT>
        static std::optional<std::vector<char>>
        AreNotInConflict(ItT first, ItT last)
        {
            if (first == last)
            {
                return std::vector<char>{};
            }
            std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
            std::vector<char> inuse(n, false);
            while (first != last)
            {
                auto r = (*first)->isNotInConflictWith(inuse);
                if (!r)
                {
                    return r;
                }
                inuse = std::move(*r);
                ++first;
            }
            return inuse;
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ////////////////////////////////////// publishing ////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    protected:
        //publish thread hooks
        virtual void
        onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
        {
        }
 
        virtual void
        onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
        {
        }
 
        virtual void
        onPublish(const SensorAndControl&,
                  const DebugDrawerInterfacePrx&,
                  const DebugObserverInterfacePrx&)
        {
        }
 
    private:
        void publish(const SensorAndControl& sac,
                     const DebugDrawerInterfacePrx& draw,
                     const DebugObserverInterfacePrx& observer);
        void deactivatePublish(const DebugDrawerInterfacePrx& draw,
                               const DebugObserverInterfacePrx& observer);
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////////// data ///////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        RobotUnit& robotUnit;
        StringStringDictionary controlDeviceControlModeMap;
        std::map<std::string, const JointController*> controlDeviceUsedJointController;
        std::vector<char> controlDeviceUsedBitmap;
        std::vector<std::size_t> controlDeviceUsedIndices;
 
        std::string rtClassName_;
        std::string instanceName_;
 
        //this data is filled by the robot unit to provide convenience functions
        std::atomic_bool isActive{false};
        std::atomic_bool isRequested{false};
        std::atomic_bool deactivatedBecauseOfError{false};
        std::atomic_bool errorState{false};
        bool deletable{false};
        bool internal{false};
 
        std::atomic_bool publishActive{false};
 
        std::atomic_bool statusReportedActive{false};
        std::atomic_bool statusReportedRequested{false};
 
        VirtualRobot::RobotPtr rtRobot;
        std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////////// friends //////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        friend class RobotUnitModule::NJointControllerAttorneyForControllerManagement;
        friend class RobotUnitModule::NJointControllerAttorneyForControlThread;
        friend class RobotUnitModule::NJointControllerAttorneyForPublisher;
        template <class>
        friend class detail::NJointControllerRegistryEntryHelper;
    };
 
    class SynchronousNJointController : public NJointControllerBase
    {
    public: 
        virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                           const IceUtil::Time& timeSinceLastIteration) = 0;
 
        virtual void
        rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
                           const IceUtil::Time& timeSinceLastIteration)
        {
            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
        }
    };
 
    class AsynchronousNJointController : public NJointControllerBase
    {
    public: 
        virtual void rtRunIterationBegin(const IceUtil::Time& sensorValuesTimestamp,
                                         const IceUtil::Time& timeSinceLastIteration) = 0;
        virtual void rtRunIterationEnd(const IceUtil::Time& sensorValuesTimestamp,
                                       const IceUtil::Time& timeSinceLastIteration) = 0;
    };
 
    using NJointController = SynchronousNJointController;
    using NJointControllerPtr = SynchronousNJointControllerPtr;
} // namespace armarx