RobotUnitModuleControlThreadDataBuffer.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 <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/util/TripleBuffer.h>
 
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 
#include "../SensorValues/SensorValue1DoFActuator.h"
#include "../util/ControlThreadOutputBuffer.h"
#include "../util/JointAndNJointControllers.h"
#include "RobotUnitModuleBase.h"
 
namespace armarx
{
    class SensorValue1DoFActuatorPosition;
}
 
namespace armarx::RobotUnitModule
{
    /**
     * @ingroup Library-RobotUnit-Modules
     * @brief This \ref ModuleBase "Module" manages all communication into and out of the \ref ControlThread.
     *
     * \section buffers Data Buffers
     * This class holds three data buffers
     *
     * \subsection bufferreq Buffer for requested controllers
     * \note This buffer's direction is ArmarX -> \ref ControlThread
     *
     * This buffer contains all \ref NJointControllerBase "NJointControllers"
     * requested by the user (e.g.: via \ref NJointControllerBase::activate) and
     * the set of \ref JointController "JointControllers" implicitly defined by this.
     * These controllers will be activated by \ref ControlThread::rtSwitchControllerSetup.
     *
     *
     * \subsection bufferact Buffer for activated controllers
     * \note This buffer's direction is \ref ControlThread -> ArmarX
     *
     * This buffer contains all \ref JointController "Joint-" and
     * \ref NJointControllerBase "NJointControllers" executed by the \ref ControlThread.
     *
     *
     * \subsection bufferout Buffer for iteration data
     * \note This buffer's direction is \ref ControlThread -> ArmarX
     *
     * This buffer contains all \ref SensorValueBase "SensorValues" and \ref ControlTargetBase "ControlTargets"
     * for each iteration.
     * It also contains messages logged via \ref ARMARX_RT_LOGF.
     *
     * This data is used to update \ref Units and is reported to topics (see \ref Publish).
     *
     * This buffer is organized as a ringbuffer and holds several old iterations.
     *
     * @see ModuleBase
     */
    class ControlThreadDataBuffer : virtual public ModuleBase
    {
        friend class ModuleBase;
 
    public:
        /**
         * @brief Returns the singleton instance of this class
         * @return The singleton instance of this class
         */
        static ControlThreadDataBuffer&
        Instance()
        {
            return ModuleBase::Instance<ControlThreadDataBuffer>();
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @see ModuleBase::_postFinishDeviceInitialization
        void _postFinishDeviceInitialization();
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////// Module interface /////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        /**
         * @brief Updates the given VirtualRobot with the current joint values
         * @param robot The robot to update
         */
        void updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const;
        /**
         * @brief Updates the given VirtualRobot with the current joint values
         * @param robot The robot to update
         * @param nodes The robot's nodes
         */
        void updateVirtualRobot(const VirtualRobot::RobotPtr& robot,
                                const std::vector<VirtualRobot::RobotNodePtr>& nodes) const;
 
        /**
         * @brief Returns whether the set of activated \ref JointController "Joint-" and
         * \ref NJointControllerBase "NJointControllers" changed.
         * @return Whether the set of activated \ref JointController "Joint-" and
         * \ref NJointControllerBase "NJointControllers" changed.
         * @see ControlThread
         * @see ControlThread::rtSwitchControllerSetup
         */
        bool activatedControllersChanged() const; ///TODO private + attorney for publish
        /**
         * @brief Returns the set of activated \ref JointController "Joint-" and
         * \ref NJointControllerBase "NJointControllers".
         * @return The set of activated \ref JointController "Joint-" and
         * \ref NJointControllerBase "NJointControllers".
         * @see ControlThread
         * @see ControlThread::rtSwitchControllerSetup
         */
        JointAndNJointControllers getActivatedControllers() const;
        /**
         * @brief Returns the set of activated \ref JointController "JointControllers".
         * @return The set of activated \ref JointController "JointControllers".
         * @see ControlThread
         * @see ControlThread::rtSwitchControllerSetup
         */
        std::vector<JointController*> getActivatedJointControllers() const;
        /**
         * @brief Returns the set of activated \ref NJointControllerBase "NJointControllers".
         * @return The set of activated \ref NJointControllerBase "NJointControllers".
         * @see ControlThread
         * @see ControlThread::rtSwitchControllerSetup
         */
        std::vector<NJointControllerBase*> getActivatedNJointControllers() const;
 
        /**
         * @brief Returns the set of requsted \ref JointController "Joint-" and
         * \ref NJointControllerBase "NJointControllers".
         * @return The set of requested \ref JointController "Joint-" and
         * \ref NJointControllerBase "NJointControllers".
         * @see ControllerManagement
         * @see ControllerManagement::activateNJointController
         * @see ControllerManagement::deactivateNJointController
         * @see ControllerManagement::switchNJointControllerSetup
         * @see NJointControllerBase::activateController
         * @see NJointControllerBase::deactivateController
         * @see setActivateControllersRequest
         */
        JointAndNJointControllers copyRequestedControllers() const;
        /**
         * @brief Returns the set of requsted \ref JointController "JointControllers".
         * @return The set of requested \ref JointController "JointControllers".
         * @see ControllerManagement
         * @see ControllerManagement::activateNJointController
         * @see ControllerManagement::deactivateNJointController
         * @see ControllerManagement::switchNJointControllerSetup
         * @see NJointControllerBase::activateController
         * @see NJointControllerBase::deactivateController
         * @see setActivateControllersRequest
         */
        std::vector<JointController*> copyRequestedJointControllers() const;
        /**
         * @brief Returns the set of requsted \ref NJointControllerBase "NJointControllers".
         * @return The set of requested \ref NJointControllerBase "NJointControllers".
         * @see ControllerManagement
         * @see ControllerManagement::activateNJointController
         * @see ControllerManagement::deactivateNJointController
         * @see ControllerManagement::switchNJointControllerSetup
         * @see NJointControllerBase::activateController
         * @see NJointControllerBase::deactivateController
         * @see setActivateControllersRequest
         */
        std::vector<NJointControllerBase*> copyRequestedNJointControllers() const;
 
        /**
         * @brief Updates the sensor and control buffer and returns whether the buffer was updated.
         * @return Whether the buffer was updated.
         */
        bool sensorAndControlBufferChanged() const; /// TODO private + attorney for publish
        /**
         * @brief Returns the last updated sensor and control buffer.
         * @return The last updated sensor and control buffer.
         */
        const SensorAndControl& getSensorAndControlBuffer() const;
 
        /**
         * @brief Returns the \ref ControlThreadOutputBuffer
         * \warning This function is dangerous to use.
         * @return The \ref ControlThreadOutputBuffer
         */
        ControlThreadOutputBuffer&
        getControlThreadOutputBuffer() ///TODO refactor logging and remove this function
        {
            return controlThreadOutputBuffer;
        }
 
        /**
         * @brief Returns the writer buffer for sensor and control values.
         * @return The writer buffer for sensor and control values.
         */
        SensorAndControl&
        rtGetSensorAndControlBuffer() ///TODO move to attorney
        {
            return controlThreadOutputBuffer.getWriteBuffer();
        }
 
        /**
         * @brief Commits the writer buffer for sensor and control values.
         */
        void
        rtSensorAndControlBufferCommitWrite() ///TODO move to attorney
        {
            return controlThreadOutputBuffer.commitWrite();
        }
 
        /**
         * @brief Sets the set of requested \ref NJointControllerBase "NJointControllers" to \param ctrls.
         * @param ctrls \ref NJointControllerBase "NJointControllers" requested.
         * @see ControllerManagement
         * @see ControllerManagement::activateNJointController
         * @see ControllerManagement::deactivateNJointController
         * @see ControllerManagement::switchNJointControllerSetup
         * @see NJointControllerBase::activateController
         * @see NJointControllerBase::deactivateController
         */
        void setActivateControllersRequest(
            std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls);
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // //////////////////////////////////// implementation //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    protected:
        /**
         * @brief This hook is called prior to writing the set of requested controllers.
         * A deriving class can override this function to perform some additional checks
         * (e.g. two controll devices may always need to have the same control mode, this can be checked here)
         * If any check fails, throw an exception with a descriptive message.
         * @param controllers The controllers to activate
         */
        virtual void
        checkSetOfControllersToActivate(const JointAndNJointControllers& /*controllers*/) const
        {
        }
 
    private:
        void writeRequestedControllers(JointAndNJointControllers&& setOfControllers);
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////////// Data ///////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT) (some NJoint controllers may be null)
        /// data may only be used after State::InitializingDevices
        /// all NJointControllerBase have to be sorted by id (null controllers are at the end)
        WriteBufferedTripleBuffer<JointAndNJointControllers> controllersRequested;
        /// @brief Mutex guarding \ref controllersRequested
        mutable std::recursive_mutex controllersRequestedMutex;
 
        /// @brief Controllers the RT thread is currently running (direction RT -> nonRT) (some NJoint controllers may be null)
        /// data may only be used after State::InitializingDevices
        /// all NJointControllerBase are sorted by id (null controllers may be anywhere)
        WriteBufferedTripleBuffer<JointAndNJointControllers> controllersActivated;
        /// @brief Mutex guarding \ref controllersActivated
        mutable std::recursive_mutex controllersActivatedMutex;
 
        /// @brief Transfers Data  (current sensor values and control targets) from the control thread to other threads (direction RT -> nonRT)
        /// \warning DO NOT ACCESS EXCEPT YOU KNOW WHAT YOU ARE DOING!
        ControlThreadOutputBuffer controlThreadOutputBuffer;
        /// @brief Mutex guarding \ref controlThreadOutputBuffer
        mutable std::recursive_mutex
            controlThreadOutputBufferMutex; /// TODO use this mutex instead of the global one
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /**
        * \brief This class allows minimal access to private members of \ref ControlThreadDataBuffer 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 ControlThreadDataBufferAttorneyForControlThread;
    };
} // namespace armarx::RobotUnitModule