JointController.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::JointController
 * @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 <memory>
 
#include <Ice/ProxyHandle.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
 
namespace IceProxy::armarx
{
    class DebugDrawerInterface;
    class DebugObserverInterface;
} // namespace IceProxy::armarx
 
namespace armarx
{
    class ControlDevice;
    typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface>
        DebugDrawerInterfacePrx;
    typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface>
        DebugObserverInterfacePrx;
 
    /**
     * @brief The JointController class represents one joint in one control mode.
     * It holds a \ref ControlTargetBase.
     * This target is used as setpoint for the controller.
     */
    class JointController
    {
    public:
        virtual ~JointController() = default;
 
        virtual ControlTargetBase* getControlTarget() = 0;
 
        virtual const ControlTargetBase* getControlTarget() const;
 
        /// @return The \ref JointController's \ref ControlTargetBase "ControlTarget" casted to the given type (may be nullptr)
        template <class T>
        const T*
        getControlTarget() const
        {
            return getControlTarget()->asA<const T*>();
        }
 
        virtual void rtResetTarget();
 
        virtual bool rtIsTargetValid() const;
 
        virtual const std::string& getControlMode() const;
 
        virtual std::string getHardwareControlMode() const;
 
        std::size_t getControlModeHash() const;
        std::size_t rtGetControlModeHash() const;
        std::size_t getHardwareControlModeHash() const;
        std::size_t rtGetHardwareControlModeHash() const;
 
        ControlDevice& getParent() const;
 
        /**
         * Hook for publishing data from JointController, mainly for debugging purposes. The preferred way is to use the
         * return value of the function to publish the data. This function is called in the publish thread, **not** the RT thread.
         * Thus, appropriate lock-free synchronization (e.g. atomic variables or TrippleBuffer) must be used to move the data from RT
         * thread to the publish thread.
         * @param draw Interface proxy to the DebugDrawer topic.
         * @param observer Interface proxy to DebugObserver topic.
         * @return These values are published on the RobotUnitObserver in the channel "ControlDevices". The keys of the map are prepended
         * with "{ControlDeviceName}_{ControlModeName}_" and are used as keys of the datafields.
         *
         */
        virtual StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw,
                                             const DebugObserverInterfacePrx& observer) const;
 
        ControlDevice& rtGetParent() const;
 
        template <class T>
        T&
        rtGetParent() const
        {
            return dynamic_cast<T&>(rtGetParent);
        }
 
    protected:
        //called by the owning ControlDevice
        /// @brief called when this JointController is run
        virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                           const IceUtil::Time& timeSinceLastIteration) = 0;
        /// @brief called when this JointController is activated
        virtual void rtPreActivateController();
        virtual void rtPostDeactivateController();
        /// @brief called when this JointController is deactivated
    private:
        friend class ControlDevice;
        std::atomic_bool activated{false};
        ControlDevice* parent{nullptr};
        std::size_t controlModeHash{0};
        std::size_t hardwareControlModeHash{0};
        void rtActivate();
        void rtDeactivate();
    };
 
    template <class ControlTargetType>
    class JointControllerTemplate : public virtual JointController
    {
        static_assert(std::is_base_of<ControlTargetBase, ControlTargetType>::value,
                      "ControlTargetType has to inherit SensorValueBase");
 
    public:
        using JointController::JointController;
 
        ControlTargetType* getControlTarget() final;
 
        ControlTargetType controlTarget;
    };
} // namespace armarx
 
//inline functions
namespace armarx
{
    inline const ControlTargetBase*
    JointController::getControlTarget() const
    {
        return const_cast<const ControlTargetBase*>(
            const_cast<JointController*>(this)->getControlTarget());
    }
 
    inline void
    JointController::rtResetTarget()
    {
        getControlTarget()->reset();
    }
 
    inline bool
    JointController::rtIsTargetValid() const
    {
        return getControlTarget()->isValid();
    }
 
    inline const std::string&
    JointController::getControlMode() const
    {
        return getControlTarget()->getControlMode();
    }
 
    inline std::string
    JointController::getHardwareControlMode() const
    {
        return "";
    }
 
    inline std::size_t
    JointController::getControlModeHash() const
    {
        return controlModeHash;
    }
 
    inline std::size_t
    JointController::rtGetControlModeHash() const
    {
        return controlModeHash;
    }
 
    inline std::size_t
    JointController::getHardwareControlModeHash() const
    {
        return hardwareControlModeHash;
    }
 
    inline std::size_t
    JointController::rtGetHardwareControlModeHash() const
    {
        return hardwareControlModeHash;
    }
 
    inline ControlDevice&
    JointController::rtGetParent() const
    {
        return *parent;
    }
 
    inline void
    JointController::rtPreActivateController()
    {
    }
 
    inline void
    JointController::rtPostDeactivateController()
    {
    }
 
    inline void
    JointController::rtActivate()
    {
        rtPreActivateController();
        activated = true;
    }
 
    inline void
    JointController::rtDeactivate()
    {
        activated = false;
        rtPostDeactivateController();
        rtResetTarget();
    }
 
    template <class ControlTargetType>
    inline ControlTargetType*
    JointControllerTemplate<ControlTargetType>::getControlTarget()
    {
        return &controlTarget;
    }
} // namespace armarx