ControlDevice.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
 * @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 <string>
 
#include "../ControlModes.h"
#include "../JointControllers/JointController.h"
#include "../util.h"
#include "DeviceBase.h"
 
namespace armarx::RobotUnitModule
{
    TYPEDEF_PTRS_HANDLE(Devices);
}
 
namespace armarx::ControlDeviceTags
{
    using namespace DeviceTags;
    const static std::string PeriodicPosition = "ControlDeviceTag_PeriodicPosition";
    const static std::string CreateNoDefaultController =
        "ControlDeviceTag_CreateNoDefaultController";
    const static std::string ForcePositionThroughVelocity =
        "ControlDeviceTag_ForcePositionThroughVelocity";
} // namespace armarx::ControlDeviceTags
 
namespace armarx
{
    TYPEDEF_PTRS_SHARED(ControlDevice);
 
    /**
     * @brief The ControlDevice class represents a logical unit accepting commands via its JointControllers.
     *
     * It holds a set of JointControllers with different Controll modes.
     * It always has a JointController with mode ControlModes::EmergencyStop which is used in case of an error.
     * It always has a JointController with mode ControlModes::StopMovement which is used when the controled device should stop any movement.
     *
     * It holds a set of tags.
     * These tags are used by different components to create default controllers, check for abilities or present output to the user.
     */
    class ControlDevice : public virtual DeviceBase
    {
    public:
        /// @brief A static const nullptr in case a const ref to a nullptr needs to be returned
        static const ControlDevicePtr NullPtr;
 
        /// @brief Create a ControlDevice with the given name
        ControlDevice(const std::string& name) : DeviceBase{name}
        {
        }
 
        //controller management
        /// @return the jointEmergencyStopController of this ControlDevice
        JointController* rtGetJointEmergencyStopController();
        /// @return the jointEmergencyStopController of this ControlDevice
        JointController* getJointEmergencyStopController();
        /// @return the jointStopMovementController of this ControlDevice
        JointController* rtGetJointStopMovementController();
        /// @return the jointStopMovementController of this ControlDevice
        JointController* getJointStopMovementController();
        /// @return the active JointController of this ControlDevice
        JointController* rtGetActiveJointController();
        /// @return all JointControllers of this ControlDevice
        const std::vector<JointController*>& rtGetJointControllers();
        std::vector<const JointController*> getJointControllers() const;
        /// @return the JointController for the given mode of this ControlDevice (or nullptr if there is no controller for this mode)
        JointController* getJointController(const std::string& mode);
        const JointController* getJointController(const std::string& mode) const;
        JointController* getJointController(std::size_t i);
        const JointController* getJointController(std::size_t i) const;
        bool hasJointController(const std::string& mode) const;
        const std::vector<std::string>& getControlModes() const;
        /**
         * @brief Activates the given JointController for this device
         * @throw std::logic_error if the Joint controller does not belong to this ControlDevice
         */
        virtual void rtSetActiveJointController(JointController* jointCtrl);
        /**
         * @brief runs the active Joint Controller and write the target values into the control device
         * @param sensorValuesTimestamp
         * @param timeSinceLastIteration
         * @see writeTargetValues
         */
        void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                   const IceUtil::Time& timeSinceLastIteration);
 
        /**
         * @brief This is a hook for implementations to write the setpoints to a bus.
         * @param sensorValuesTimestamp The timestamp of the current \ref SensorValueBase "SensorValues"
         * @param timeSinceLastIteration The time delta between the last two updates of \ref SensorValueBase "SensorValues"
         */
        virtual void
        rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
                            const IceUtil::Time& timeSinceLastIteration)
        {
        }
 
        RobotUnitModule::Devices* getOwner() const;
 
        std::map<std::string, std::string> getJointControllerToTargetTypeNameMap() const;
 
    protected:
        /**
         * @brief adds the Joint controller to this ControlDevice
         * @throw std::logic_error if the JointController was already added to a ControlDevice
         * @throw std::invalid_argument if a JointController with the same control mode was already added to this device
        */
        void addJointController(JointController* jointCtrl);
 
        ControlTargetBase::ControlDeviceAccessToken
        getControlTargetAccessToken() const
        {
            return {};
        }
 
    private:
        friend class RobotUnitModule::Devices;
        /// @brief The owning \ref RobotUnit. (is set when this \ref SensorDevice is added to the \ref RobotUnit)
        std::atomic<RobotUnitModule::Devices*> owner{nullptr};
        /// @brief The \ref JointController "JointControllers" managed by this \ref ControlDevice
        KeyValueVector<std::string, JointController*> jointControllers;
        /// @brief The currently executed \ref JointController
        JointController* activeJointController{nullptr};
        /// @brief This \ref ControlDevice "ControlDevice's" emergency stop controller
        JointController* jointEmergencyStopController{nullptr};
        /// @brief This \ref ControlDevice "ControlDevice's" stop movement controller
        JointController* jointStopMovementController{nullptr};
    };
} // namespace armarx
 
//inline functions
namespace armarx
{
    inline JointController*
    ControlDevice::rtGetJointEmergencyStopController()
    {
        return jointEmergencyStopController;
    }
 
    inline JointController*
    ControlDevice::rtGetJointStopMovementController()
    {
        return jointStopMovementController;
    }
 
    inline JointController*
    ControlDevice::rtGetActiveJointController()
    {
        return activeJointController;
    }
 
    inline const std::vector<JointController*>&
    ControlDevice::rtGetJointControllers()
    {
        return jointControllers.values();
    }
 
    inline std::vector<const JointController*>
    ControlDevice::getJointControllers() const
    {
        return {jointControllers.values().begin(), jointControllers.values().end()};
    }
 
    inline JointController*
    ControlDevice::getJointController(const std::string& mode)
    {
        return jointControllers.at(mode, nullptr);
    }
 
    inline const JointController*
    ControlDevice::getJointController(const std::string& mode) const
    {
        return jointControllers.at(mode, nullptr);
    }
 
    inline JointController*
    ControlDevice::getJointController(std::size_t i)
    {
        return jointControllers.at(i);
    }
 
    inline const JointController*
    ControlDevice::getJointController(std::size_t i) const
    {
        return jointControllers.at(i);
    }
 
    inline bool
    ControlDevice::hasJointController(const std::string& mode) const
    {
        return jointControllers.has(mode);
    }
 
    inline const std::vector<std::string>&
    ControlDevice::getControlModes() const
    {
        return jointControllers.keys();
    }
} // namespace armarx