RobotUnitModuleDevices.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::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <mutex>
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 
#include "../Devices/ControlDevice.h"
#include "../Devices/RTThreadTimingsSensorDevice.h"
#include "../Devices/SensorDevice.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
#include "RobotUnitModuleBase.h"
 
namespace armarx::RobotUnitModule
{
    class DevicesPropertyDefinitions : public ModuleBasePropertyDefinitions
    {
    public:
        DevicesPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "ControlDevices_HardwareControlModeGroups",
                "",
                "Groups of control devices for which the same hardware control mode has to be set. "
                "For a control mode, the JointControllers of all devices in a group must have the "
                "identical hardware control mode. "
                "The kinematic unit will change the control mode of all joints in a group,"
                " if their current hw control mode does not match the new hw control mode. "
                "This can be used if the robot has a multi DOF joint which is represented by "
                "multiple  1DOF control devices "
                "(in such a case all of these 1DOF joints should have the same hw control mode, "
                "except he multi DOF joint can handle different control modes at once). "
                "Syntax: semicolon separated groups , each group is CSV of joints "
                "e.g. j1,j2;j3,j4,j5 -> Group 1 = j1+j2 Group 2 = j3+j4+j5; No joint may be in two "
                "groups!");
            defineOptionalProperty<bool>("GlobalPoseCorrectionAndLocalizationSensorEnabled",
                                         true,
                                         "Whether to enable or disable the global pose correction "
                                         "and localization sensors.");
        }
    };
 
    /**
     * @ingroup Library-RobotUnit-Modules
     * @brief This \ref ModuleBase "Module" manages sensor and control devices for a RobotUnit and only allows save and sane access.
     *
     * @see ModuleBase
     */
    class Devices : virtual public ModuleBase, virtual public RobotUnitDevicesInterface
    {
        friend class ModuleBase;
 
    public:
        struct ControlDeviceHardwareControlModeGroups;
 
        /**
         * @brief Returns the singleton instance of this class
         * @return The singleton instance of this class
         */
        static Devices&
        Instance()
        {
            return ModuleBase::Instance<Devices>();
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @see ModuleBase::_preOnInitRobotUnit
        void _preOnInitRobotUnit();
        /// @see ModuleBase::_preFinishDeviceInitialization
        void _preFinishDeviceInitialization();
        /// @see ModuleBase::_postFinishDeviceInitialization
        void _postFinishDeviceInitialization();
        /// @see ModuleBase::_postFinishRunning
        void _postFinishRunning();
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// ice interface //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        /**
         * @brief Returns the names of all \ref ControlDevice "ControlDevices" for the robot.
         * @return The names of all \ref ControlDevice "ControlDevices" for the robot.
         */
        Ice::StringSeq
        getControlDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @brief Return the \ref ControlDeviceDescription for the given \ref ControlDevice
         * @param name The \ref ControlDevice
         * @return The \ref ControlDeviceDescription
         * @see ControlDeviceDescription
         * @see getControlDeviceDescriptions
         */
        ControlDeviceDescription
        getControlDeviceDescription(const std::string& name,
                                    const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Return the \ref ControlDeviceDescription "ControlDeviceDescriptions" for all \ref ControlDevice "ControlDevices"
         * @return The \ref ControlDeviceDescription "ControlDeviceDescriptions"
         * @see ControlDeviceDescription
         * @see getControlDeviceDescription
         */
        ControlDeviceDescriptionSeq
        getControlDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @brief Return the \ref ControlDeviceStatus for the given \ref ControlDevice
         * @param name The \ref ControlDevice
         * @return The \ref ControlDeviceStatus
         * @see ControlDeviceStatus
         * @see getControlDeviceStatuses
         */
        ControlDeviceStatus
        getControlDeviceStatus(const std::string& name,
                               const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Return the \ref ControlDeviceStatus "ControlDeviceStatuses" for all \ref ControlDevice "ControlDevices"
         * @return The \ref ControlDeviceStatus "ControlDeviceStatuses"
         * @see ControlDeviceStatus
         * @see getControlDeviceStatus
         */
        ControlDeviceStatusSeq
        getControlDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @brief Returns the names of all \ref SensorDevice "SensorDevices" for the robot.
         * @return The names of all \ref SensorDevice "ControlDevices" for the robot.
         */
        Ice::StringSeq getSensorDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @brief Return the \ref SensorDeviceDescription for the given \ref SensorDevice
         * @param name The \ref SensorDevice
         * @return The \ref SensorDeviceDescription
         * @see SensorDeviceDescription
         * @see getSensorDeviceDescriptions
         */
        SensorDeviceDescription
        getSensorDeviceDescription(const std::string& name,
                                   const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Return the \ref SensorDeviceDescription "SensorDeviceDescriptions" for all \ref SensorDevice "SensorDevices"
         * @return The \ref SensorDeviceDescription "SensorDeviceDescriptions"
         * @see SensorDeviceDescription
         * @see getSensorDeviceDescription
         */
        SensorDeviceDescriptionSeq
        getSensorDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @brief Return the \ref SensorDeviceStatus for the given \ref SensorDevice
         * @param name The \ref SensorDevice
         * @return The \ref SensorDeviceStatus
         * @see SensorDeviceStatus
         * @see getSensorDeviceStatuses
         */
        SensorDeviceStatus
        getSensorDeviceStatus(const std::string& name,
                              const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Return the \ref SensorDeviceStatus "SensorDeviceStatuses" for all \ref SensorDevice "SensorDevices"
         * @return The \ref SensorDeviceStatus "SensorDeviceStatuses"
         * @see SensorDeviceStatus
         * @see getSensorDeviceStatus
         */
        SensorDeviceStatusSeq
        getSensorDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////// Module interface /////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        /**
         * @brief Returns the number of \ref ControlDevice "ControlDevices"
         * @return The number of \ref ControlDevice "ControlDevices"
         */
        std::size_t getNumberOfControlDevices() const;
        /**
         * @brief Returns the number of \ref SensorDevice "SensorDevices"
         * @return The number of \ref SensorDevice "SensorDevices"
         */
        std::size_t getNumberOfSensorDevices() const;
 
        /**
         * @brief Return the \ref ControlDeviceDescription for the given \ref ControlDevice
         * @param idx The \ref ControlDevice's index
         * @return The \ref ControlDeviceDescription for the given \ref ControlDevice
         * @see ControlDeviceDescription
         * @see getControlDeviceDescription
         * @see getControlDeviceDescriptions
         */
        ControlDeviceDescription getControlDeviceDescription(std::size_t idx) const;
        /**
         * @brief Return the \ref ControlDeviceStatus for the given \ref ControlDevice
         * @param idx The \ref ControlDevice's index
         * @return The \ref ControlDeviceStatus
         * @see ControlDeviceStatus
         * @see getControlDeviceStatus
         * @see getControlDeviceStatuses
         */
        ControlDeviceStatus getControlDeviceStatus(std::size_t idx) const;
 
        /**
         * @brief Return the \ref SensorDeviceDescription for the given \ref SensorDevice
         * @param idx The \ref SensorDevice's index
         * @return The \ref SensorDeviceDescription for the given \ref SensorDevice
         * @see SensorDeviceDescription
         * @see getSensorDeviceDescription
         * @see getSensorDeviceDescriptions
         */
        SensorDeviceDescription getSensorDeviceDescription(std::size_t idx) const;
        /**
         * @brief Return the \ref SensorDeviceStatus for the given \ref SensorDevice
         * @param idx The \ref SensorDevice's index
         * @return The \ref SensorDeviceStatus
         * @see SensorDeviceStatus
         * @see getSensorDeviceStatus
         * @see getSensorDeviceStatuses
         */
        SensorDeviceStatus getSensorDeviceStatus(std::size_t idx) const;
 
        /**
         * @brief Updates the given VirtualRobot with the given \ref SensorValue "SensorValues"
         * @param robot The VirtualRobot to update
         * @param sensors The \ref SensorValue "SensorValues"
         */
        template <class PtrT>
        void
        updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot,
                                           const std::vector<PtrT>& sensors) const
        {
            updateVirtualRobotFromSensorValues(robot, robot->getRobotNodes(), sensors);
        }
 
        /**
         * @brief Updates the given VirtualRobot with the given \ref SensorValue "SensorValues"
         *
         * This overload is slightly faster than the other version.
         * @param robot The VirtualRobot to update
         * @param nodes The VirtualRobot's RobotNodes
         * @param sensors The \ref SensorValue "SensorValues"
         */
 
        // TODO use base type for 'sensors' element
        template <class PtrT>
        void
        updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot,
                                           const std::vector<VirtualRobot::RobotNodePtr>& nodes,
                                           const std::vector<PtrT>& sensors) const
        {
            ARMARX_CHECK_NOT_NULL(robot);
 
            for (const SimoxRobotSensorValueMapping& m : simoxRobotSensorValueMapping)
            {
                const SensorValueBase* sv = sensors.at(m.idxSens);
                ARMARX_CHECK_NOT_NULL(sv);
                const SensorValue1DoFActuatorPosition* svPos =
                    sv->asA<SensorValue1DoFActuatorPosition>();
                ARMARX_CHECK_NOT_NULL(svPos);
                nodes.at(m.idxRobot)->setJointValueNoUpdate(svPos->position);
            }
 
            // update global pose
            if (globalRobotLocalizationSensorDevice != nullptr)
            {
                const auto sensorValue =
                    globalRobotLocalizationSensorDevice->getSensorValue()
                        ->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
                const auto global_T_root = sensorValue->global_T_root;
                robot->setGlobalPose(global_T_root, false);
            }
 
            robot->applyJointValues();
        }
 
        /**
         * @brief Returns the groups of \ref ControlDevice "ControlDevices" required to be in the same
         * hardware control mode. (explained \ref RobotUnitCtrlModeGroups "here")
         * @return The groups of \ref ControlDevice "ControlDevices" required to be in the same
         * hardware control mode.
         */
        const ControlDeviceHardwareControlModeGroups&
        getControlModeGroups() const
        {
            return ctrlModeGroups;
        }
 
        /**
         * @brief Returns the \ref ControlDevice's index
         * @param deviceName \ref ControlDevice's name.
         * @return The \ref ControlDevice's index
         */
        std::size_t getControlDeviceIndex(const std::string& deviceName) const;
        /**
         * @brief Returns the \ref SensorDevice's index
         * @param deviceName \ref SensorDevice's name.
         * @return The \ref SensorDevice's index
         */
        std::size_t getSensorDeviceIndex(const std::string& deviceName) const;
 
        /**
         * @brief Returns the \ref ControlDevice
         * @param deviceName \ref ControlDevice's name.
         * @return The \ref ControlDevice
         */
        ConstControlDevicePtr getControlDevice(
            const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
        /**
         * @brief Returns the \ref SensorDevice
         * @param deviceName \ref SensorDevice's name.
         * @return The \ref SensorDevice
         */
        ConstSensorDevicePtr getSensorDevice(
            const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
 
        /**
         * @brief Returns all \ref SensorDevice "SensorDevices"
         * @return All \ref SensorDevice "SensorDevices"
         */
        const KeyValueVector<std::string, SensorDevicePtr>&
        getSensorDevices() const ///TODO remove from interface
        {
            return sensorDevices;
        }
 
        /**
         * @brief Returns all \ref ControlDevice "ControlDevices"
         * @return All \ref ControlDevice "ControlDevices"
         */
        const KeyValueVector<std::string, ControlDevicePtr>&
        getControlDevices() const ///TODO remove from interface
        {
            return controlDevices;
        }
 
        /**
         * @brief Returns const pointers to  all \ref SensorDevice "SensorDevices"
         * @return Const pointers to all \ref SensorDevice "SensorDevices"
         */
        const std::map<std::string, ConstSensorDevicePtr>&
        getSensorDevicesConstPtr() const ///TODO rename to getSensorDevices
        {
            return sensorDevicesConstPtr;
        }
 
        /**
         * @brief Returns const pointers to all \ref ControlDevice "ControlDevices"
         * @return Const pointers to all \ref ControlDevice "ControlDevices"
         */
        const std::map<std::string, ConstControlDevicePtr>&
        getControlDevicesConstPtr() const ///TODO rename to getControlDevices
        {
            return controlDevicesConstPtr;
        }
 
    protected:
        /**
         * @brief Adds a \ref ControlDevice to the robot.
         * \warning Must not be called when the \ref RobotUnit's \ref RobotUnitState "state" is not \ref RobotUnitState::InitializingDevices
         * @param cd The \ref ControlDevice to add
         */
        void addControlDevice(const ControlDevicePtr& cd);
        /**
         * @brief Adds a \ref SensorDevice to the robot.
         * \warning Must not be called when the \ref RobotUnit's \ref RobotUnitState "state" is not \ref RobotUnitState::InitializingDevices
         * @param cd The \ref SensorDevice to add
         */
        void addSensorDevice(const SensorDevicePtr& sd);
        /**
         * @brief Creates the \ref SensorDevice used to log timings in the \ref ControlThread
         * (This function is supposed to be used in the \ref ModuleBase::finishDeviceInitialization)
         * @return The \ref SensorDevice used to log timings in the \ref ControlThread
         * @see rtGetRTThreadTimingsSensorDevice
         */
        virtual RTThreadTimingsSensorDevicePtr createRTThreadTimingSensorDevice() const;
 
        /**
         * @brief Returns the \ref SensorDevice used to log timings in the \ref ControlThread.
         * (This function is supposed to be used in the \ref ControlThread)
         * @return The \ref SensorDevice used to log timings in the \ref ControlThread
         * @see createRTThreadTimingSensorDevice
         */
        RTThreadTimingsSensorDevice&
        rtGetRTThreadTimingsSensorDevice()
        {
            return *rtThreadTimingsSensorDevice;
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // //////////////////////////////////// implementation //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /**
         * @brief Returns the stop movement \ref JointController "JointControllers"
         * for all \ref ControlDevice "ControlDevices"
         * @return The stop movement \ref JointController "JointControllers"
         */
        std::vector<JointController*> getStopMovementJointControllers() const;
        /**
         * @brief Returns the emergency stop \ref JointController "JointControllers"
         * for all \ref ControlDevice "ControlDevices"
         * @return The emergency stop \ref JointController "JointControllers"
         */
        std::vector<JointController*> getEmergencyStopJointControllers() const;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////////// Data ///////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @brief Struct correlating a \ref VirtualRobot::RobotNode to a \ref SensorDevice
        struct SimoxRobotSensorValueMapping
        {
            /// @brief The RobotNode's index
            std::size_t idxRobot;
            /// @brief The \ref SensorDevice's index
            std::size_t idxSens;
        };
 
        /// @brief Mapping from RobotNode index to \ref SensorDevice
        std::vector<SimoxRobotSensorValueMapping> simoxRobotSensorValueMapping;
        /// @brief Pointers to all \ref SensorValueBase "SensorValues"
        std::vector<const SensorValueBase*> sensorValues;
        /// @brief Pointers to all \ref ControlTargetBase "ControlTargets"
        std::vector<std::vector<PropagateConst<ControlTargetBase*>>> controlTargets;
 
    public:
        /// @brief Holds data about a device group requiring the same hardware control mode. (explained \ref RobotUnitCtrlModeGroups "here")
        struct ControlDeviceHardwareControlModeGroups
        {
            /// @brief contains a vector per group.
            /// Each vector contains the names of devices in this group.
            std::vector<std::set<std::string>> groups;
            /// @brief contains a vector per group.
            /// Each vector contains the indices of devices in this group.
            std::vector<std::vector<std::size_t>> deviceIndices;
            /// @brief contains the names of all devices in any group
            std::set<std::string> groupsMerged;
            /// @brief contains the group index for each device (or the sentinel)
            std::vector<std::size_t> groupIndices;
        };
 
    private:
        //ctrl dev
        /// @brief \ref ControlDevice "ControlDevices" added to this unit (data may only be added during and only be used after \ref  State::InitializingDevices)
        KeyValueVector<std::string, ControlDevicePtr> controlDevices;
        /// @brief const pointer to all ControlDevices (passed to GenerateConfigDescription of a NJointControllerBase)
        std::map<std::string, ConstControlDevicePtr> controlDevicesConstPtr;
        /// @brief Guards access to all \ref ControlDevice "ControlDevices"
        mutable MutexType controlDevicesMutex;
 
        /// @brief Device groups requiring the same hardware control mode.
        ControlDeviceHardwareControlModeGroups ctrlModeGroups;
 
        //sens dev
        /// @brief \ref SensorDevice "SensorDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
        KeyValueVector<std::string, SensorDevicePtr> sensorDevices;
        /// @brief const pointer to all SensorDevices (passed to GenerateConfigDescription of a NJointControllerBase)
        std::map<std::string, ConstSensorDevicePtr> sensorDevicesConstPtr;
        /// @brief Guards access to all \ref SensorDevice "SensorDevices"
        mutable MutexType sensorDevicesMutex;
 
        /// @brief a pointer to the RTThreadTimingsSensorDevice used to meassure timings in the rt loop
        RTThreadTimingsSensorDevicePtr rtThreadTimingsSensorDevice;
 
        armarx::ConstSensorDevicePtr globalRobotLocalizationSensorDevice;
 
        /// @brief The device name of rtThreadTimingsSensorDevice
        static const std::string rtThreadTimingsSensorDeviceName;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /**
        * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref Publisher.
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        friend class DevicesAttorneyForPublisher;
        /**
        * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        friend class DevicesAttorneyForControlThread;
        /**
        * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThreadDataBuffer.
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        friend class DevicesAttorneyForControlThreadDataBuffer;
        /**
         * \brief This class allows minimal access to private members of \ref armarx::RobotUnitModule::Devices in a sane fashion for \ref armarx::NJointControllerBase.
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        friend class DevicesAttorneyForNJointController;
    };
} // namespace armarx::RobotUnitModule