RobotUnitModulePublisher.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 <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/observers/DebugObserver.h>
 
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
#include "../RobotUnitObserver.h"
#include "RobotUnitModuleBase.h"
 
namespace armarx::detail
{
    struct ControlThreadOutputBufferEntry;
}
 
namespace armarx
{
    TYPEDEF_PTRS_HANDLE(RobotUnitObserver);
    using SensorAndControl = detail::ControlThreadOutputBufferEntry;
} // namespace armarx
 
namespace armarx::RobotUnitModule
{
    class PublisherPropertyDefinitions : public ModuleBasePropertyDefinitions
    {
    public:
        PublisherPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("RobotUnitListenerTopicName",
                                                "RobotUnitListenerTopic",
                                                "The topic receiving events for RobotUnitListener");
            defineOptionalProperty<std::string>("DebugDrawerUpdatesTopicName",
                                                "DebugDrawerUpdates",
                                                "The topic receiving events for debug drawing");
            defineOptionalProperty<std::size_t>(
                "PublishPeriodMs", 10, "Milliseconds between each publish");
 
            defineOptionalProperty<bool>("ObserverEnablePublishing",
                                         true,
                                         "Whether the publishing thread is started or not",
                                         PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("ObserverPublishSensorValues",
                                         true,
                                         "Whether sensor values are send to the observer",
                                         PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("ObserverPublishControlTargets",
                                         true,
                                         "Whether control targets are send to the observer",
                                         PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("ObserverPublishTimingInformation",
                                         true,
                                         "Whether timing information are send to the observer",
                                         PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("ObserverPublishAdditionalInformation",
                                         true,
                                         "Whether additional information are send to the observer",
                                         PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<std::string>(
                "DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
            defineOptionalProperty<std::uint64_t>(
                "ObserverPrintEveryNthIterations",
                1,
                "Only every nth iteration data is send to the debug observer",
                PropertyDefinitionBase::eModifiable);
        }
    };
 
    /**
     * @ingroup Library-RobotUnit-Modules
     * @brief This \ref ModuleBase "Module" manages publishing of all data to Topics,
     * updating of all units managed by \ref Units "the Units module" and calling of \ref NJointControllerBase hooks.
     *
     * @see ModuleBase
     */
    class Publisher : virtual public ModuleBase, virtual public RobotUnitPublishingInterface
    {
        friend class ModuleBase;
 
    public:
        /**
         * @brief Returns the singleton instance of this class
         * @return The singleton instance of this class
         */
        static Publisher&
        Instance()
        {
            return ModuleBase::Instance<Publisher>();
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @see ModuleBase::_icePropertiesInitialized
        void _icePropertiesInitialized();
        /// @see ModuleBase::_componentPropertiesUpdated
        void _componentPropertiesUpdated(const std::set<std::string>& changedProperties);
 
        /// @see ModuleBase::_preOnInitRobotUnit
        void _preOnInitRobotUnit();
        /// @see ModuleBase::_preOnConnectRobotUnit
        void _preOnConnectRobotUnit();
        /// @see ModuleBase::_postFinishControlThreadInitialization
        void _postFinishControlThreadInitialization();
        /// @see ModuleBase::_preFinishRunning
        void _preFinishRunning();
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// ice interface //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        /**
         * @brief Returns the name of the used DebugDrawerTopic
         * @return The name of the used DebugDrawerTopic
         */
        std::string getDebugDrawerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Returns the name of the used DebugObserverTopic
         * @return The name of the used DebugObserverTopic
         */
        std::string
        getDebugObserverTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Returns the name of the used RobotUnitListenerTopic
         * @return The name of the used RobotUnitListenerTopic
         */
        std::string
        getRobotUnitListenerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @brief Returns the used DebugDrawerProxy
         * @return The used DebugDrawerProxy
         */
        DebugDrawerInterfacePrx
        getDebugDrawerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Returns the used RobotUnitListenerProxy
         * @return The used RobotUnitListenerProxy
         */
        RobotUnitListenerPrx
        getRobotUnitListenerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
        /**
         * @brief Returns the used DebugObserverProxy
         * @return The used DebugObserverProxy
         */
        DebugObserverInterfacePrx
        getDebugObserverProxy(const Ice::Current& = Ice::emptyCurrent) const override;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////// Module interface /////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    protected:
        /**
         * @brief Publishes data
         * @param additionalMap Data published to the RobotUnitObserver's additional channel (This field is used by deriving classes)
         * @param timingMap Data published to the RobotUnitObserver's timing channel (This field is used by deriving classes)
         */
        virtual void publish(StringVariantBaseMap additionalMap = {},
                             StringVariantBaseMap timingMap = {});
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // //////////////////////////////////// implementation //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /**
         * @brief Returns the used RobotUnitObserver
         * @return The used RobotUnitObserver
         */
        const RobotUnitObserverPtr&
        getRobotUnitObserver() const
        {
            ARMARX_CHECK_EXPRESSION(robotUnitObserver);
            return robotUnitObserver;
        }
 
        /**
         * @brief Publishes updtes about new classes od \ref NJointControllerBase "NJointControllers"
         * @return The time required by this function as a \ref Variant
         */
        TimedVariantPtr publishNJointClassNames();
        /**
         * @brief Publishes updates of \ref NJointControllerBase "NJointControllers" ([de]activate + publish hooks)
         * @param timingMap Timings of this publish iteration (out param)
         * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
         * @return The time required by this function as a \ref Variant
         * @see NJointControllerBase::onPublish
         */
        TimedVariantPtr
        publishNJointControllerUpdates(StringVariantBaseMap& timingMap,
                                       const SensorAndControl& controlThreadOutputBuffer);
        /**
         * @brief Updates all sub units and publishes the timings of these updates
         * @param timingMap Timings of this publish iteration (out param)
         * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
         * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointControllerBase "NJointControllers"
         * active in the published \ref ControlThread iteration
         * @return The time required by this function as a \ref Variant
         */
        TimedVariantPtr publishUnitUpdates(StringVariantBaseMap& timingMap,
                                           const SensorAndControl& controlThreadOutputBuffer,
                                           const JointAndNJointControllers& activatedControllers);
        /**
         * @brief Publishes data about updates of \ref JointController "JointControllers" and their \ref ControlTargetBase "ControlTargets"
         * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
         * @param haveSensorAndControlValsChanged Whether \ref ControlTargetBase "ControlTargets" were updated by the \ref ControlThread
         * @param publishToObserver Whether data should be published to observers
         * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointControllerBase "NJointControllers"
         * active in the published \ref ControlThread iteration
         * @param requestedJointControllers
         * @return The time required by this function as a \ref Variant
         */
        TimedVariantPtr
        publishControlUpdates(const SensorAndControl& controlThreadOutputBuffer,
                              bool haveSensorAndControlValsChanged,
                              bool publishToObserver,
                              const JointAndNJointControllers& activatedControllers,
                              const std::vector<JointController*>& requestedJointControllers);
        /**
         * @brief Publishes Updates about \ref SensorValueBase "SensorValues" (To \ref RobotUnitListener and \ref RobotUnitObserver)
         * @param publishToObserver Whether data should be published to observers
         * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
         * @return The time required by this function as a \ref Variant
         */
        TimedVariantPtr publishSensorUpdates(bool publishToObserver,
                                             const SensorAndControl& controlThreadOutputBuffer);
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////////// Data ///////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        static constexpr int spamdelay = 30;
 
        using PublisherTaskT = SimplePeriodicTask<std::function<void(void)>>;
 
        /// @brief The name of the used RobotUnitListenerTopic
        std::string robotUnitListenerTopicName;
        /// @brief The name of the used DebugDrawerTopic
        std::string debugDrawerUpdatesTopicName;
        /// @brief The name of the used DebugObserverTopic
        std::string debugObserverTopicName;
 
        /// @brief The used DebugDrawerProxy
        DebugDrawerInterfacePrx debugDrawerPrx;
        /// @brief The used DebugObserverProxy
        DebugObserverInterfacePrx debugObserverPrx;
 
        /// @brief the number of calles to \ref publish
        std::uint64_t publishIterationCount{0};
 
        /// @brief The time required by the last iteration of \ref publish to publish sensor data
        IceUtil::Time publishNewSensorDataTime;
        /// @brief The thread executing the publisher loop
        PublisherTaskT::pointer_type publisherTask; ///TODO use std thread
        /// @brief The already reported classes of \ref NJointControllerBase "NJointControllers"
        std::set<std::string> lastReportedClasses;
 
        /// @brief Whether \ref SensorValueBase "SensorValues" should be published to the observers
        std::atomic_bool observerPublishSensorValues;
        /// @brief Whether the publishing thread should be started or not
        std::atomic_bool observerEnablePublishing;
        /// @brief Whether \ref ControlTargetBase "ControlTargets" should be published to the observers
        std::atomic_bool observerPublishControlTargets;
        /// @brief Whether \ref Timing information should be published to the observers
        std::atomic_bool observerPublishTimingInformation;
        /// @brief Whether \ref Additional information should be published to the observers
        std::atomic_bool observerPublishAdditionalInformation;
 
        /// @brief How many iterations of \ref publish shold not publish data to the debug observer.
        std::atomic<std::uint64_t> debugObserverSkipIterations;
 
        /// @brief The time required by the last iteration of \ref publish
        /// \warning May only be accessed by the publish thread.
        IceUtil::Time lastPublishLoop;
 
        /// @brief The used RobotUnitObserver
        RobotUnitObserverPtr robotUnitObserver;
 
        /// @brief The period of the publisher loop
        std::size_t publishPeriodMs{1};
 
        /// @brief A proxy to the used RobotUnitListener topic
        RobotUnitListenerPrx robotUnitListenerPrx;
        /// @brief A batch proxy to the used RobotUnitListener topic
        /// \warning May only be accessed by the publish thread.
        RobotUnitListenerPrx robotUnitListenerBatchPrx;
 
        /// \warning May only be accessed by the publish thread.
        IceUtil::Time lastControlThreadTimestamp;
    };
} // namespace armarx::RobotUnitModule