Observer.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
*
* 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    ArmarX::Core
* @author     Kai Welke (welke@kit.edu)
* @date       2011
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <functional>
#include <mutex>
#include <unordered_map>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <ArmarXCore/observers/ConditionCheck.h>
 
namespace armarx
{
 
    class DatafieldRef;
    using DatafieldRefPtr = IceInternal::Handle<DatafieldRef>;
 
    /**
     * \class DebugObserverPropertyDefinitions
     * \brief
     */
    class ObserverPropertyDefinitions : public ComponentPropertyDefinitions
    {
    public:
        ObserverPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<int>(
                "MaxHistorySize", 5000, "Maximum number of entries in the Observer history");
            defineOptionalProperty<float>("MaxHistoryRecordFrequency",
                                          50.f,
                                          "The Observer history is written with this maximum "
                                          "frequency. Everything faster is being skipped.");
            defineOptionalProperty<bool>(
                "CreateUpdateFrequenciesChannel",
                false,
                "If true, an additional channel is created that shows the update frequency of "
                "every other channel in that observer.");
        }
    };
 
    /**
      @class Observer
      @brief Baseclass for all ArmarX Observers
      @ingroup ObserversGrp
 
      A subclass of the observer should implement the two framework hooks onInitObserver and onConnectObserver.
      The onInitObserver method should register all channels, datafields, and available condition checks
      (see offerChannel, offerDattField, offerCheck).
      Further, a subclass should implement a SensorActorUnitListener Ice interface.
      Each interface method should correspond to one channel.
      The observer should map the actual sensor values to the observer superclass
      using the dataField method.
      Finally, after all values have been updated, the updateChannel method needs
      to be called in order to verify all currently active conditions and
      generate associated events.
     */
    class ARMARXCORE_IMPORT_EXPORT Observer :
        virtual public ObserverInterface,
        virtual public Component
    {
        // ////////////////////////////////////////////////////////////////// //
        // ice interface + local ice doubles
    public:
        Observer();
 
        ~Observer();
 
        std::string getObserverName() const;
        void getObserverName_async(const AMD_ObserverInterface_getObserverNamePtr& amd,
                                   const Ice::Current&) const override;
 
    public:
        /**
        * Installs a condition check with the observer
        *
        * @param configuration configuration of the check
        * @param listener literal of the conditional expression that is associated with this check
        * @param c ice context
        * @throw InvalidConditionException
        * @return identifier of installed condition as required for removal (see removeCondition)
        */
        CheckIdentifier installCheck(const CheckConfiguration& configuration);
        void installCheck_async(const AMD_ObserverInterface_installCheckPtr& amd,
                                const CheckConfiguration& configuration,
                                const Ice::Current&) override;
 
    public:
        /**
        * Removes a condition check from the observer. If the condition has already been removed, the function immidiately returns.
        *
        * @param id identifier of installed condition
        * @param c ice context
        */
        void removeCheck(const CheckIdentifier& id);
        void removeCheck_async(const AMD_ObserverInterface_removeCheckPtr& amd,
                               const CheckIdentifier& id,
                               const Ice::Current&) override;
 
    public:
        /**
        * Retrieve data field from observer
        * @param identifier Identifier of datafield
        * @param c ice context
        *
        * @return timestamped variant corresponding to the data field
        */
        TimedVariantBasePtr getDataField(const DataFieldIdentifierBasePtr& identifier,
                                         const Ice::Current& c = Ice::emptyCurrent) const;
        void getDataField_async(const AMD_ObserverInterface_getDataFieldPtr& amd,
                                const DataFieldIdentifierBasePtr& identifier,
                                const Ice::Current& c) const override;
 
    public:
        TimedVariantBasePtr getDatafieldByName(const std::string& channelName,
                                               const std::string& datafieldName) const;
        void getDatafieldByName_async(const AMD_ObserverInterface_getDatafieldByNamePtr& amd,
                                      const std::string& channelName,
                                      const std::string& datafieldName,
                                      const Ice::Current&) const override;
 
    public:
        DatafieldRefBasePtr getDataFieldRef(const DataFieldIdentifierBasePtr& identifier) const;
        void getDataFieldRef_async(const AMD_ObserverInterface_getDataFieldRefPtr& amd,
                                   const DataFieldIdentifierBasePtr& identifier,
                                   const Ice::Current&) const override;
 
    public:
        DatafieldRefBasePtr getDatafieldRefByName(const std::string& channelName,
                                                  const std::string& datafieldName) const;
        void getDatafieldRefByName_async(const AMD_ObserverInterface_getDatafieldRefByNamePtr& amd,
                                         const std::string& channelName,
                                         const std::string& datafieldName,
                                         const Ice::Current&) const override;
 
    public:
        /**
        * Retrieve list of data field from observer
        * @param identifier list of identifiers of datafield
        * @param c ice context
        *
        * @return list of timestamped variants corresponding to the data fields given
        */
        TimedVariantBaseList getDataFields(const DataFieldIdentifierBaseList& identifiers,
                                           const Ice::Current& c);
        void getDataFields_async(const AMD_ObserverInterface_getDataFieldsPtr& amd,
                                 const DataFieldIdentifierBaseList& identifiers,
                                 const Ice::Current& c) override;
 
    public:
        StringTimedVariantBaseMap getDatafieldsOfChannel(const std::string& channelName) const;
        void
        getDatafieldsOfChannel_async(const AMD_ObserverInterface_getDatafieldsOfChannelPtr& amd,
                                     const std::string& channelName,
                                     const Ice::Current&) const override;
 
 
    public:
        /**
        * Retrieve information on all sensory data channels available from the observer.
        * @param c ice context
        *
        * @return the ChannelRegistry contains information on each channel including its datafields and associated types and current values.
        */
        ChannelRegistryEntry getChannel(const std::string& channelName) const;
        void getChannel_async(const AMD_ObserverInterface_getChannelPtr& amd,
                              const std::string& channelName,
                              const Ice::Current&) const override;
 
 
    public:
        /**
        * Retrieve information on all sensory data channels available from the observer.
        * @param c ice context
        *
        * @return the ChannelRegistry contains information on each channel including its datafields and associated types and current values.
        */
        ChannelRegistry getAvailableChannels(bool includeMetaChannels);
        void getAvailableChannels_async(const AMD_ObserverInterface_getAvailableChannelsPtr& amd,
                                        bool includeMetaChannels,
                                        const Ice::Current&) override;
 
 
    public:
        /**
        * Retrieve list of available condition checks
        * @param c ice context
        *
        * @return list of available condition checks
        */
        StringConditionCheckMap getAvailableChecks();
        void getAvailableChecks_async(const AMD_ObserverInterface_getAvailableChecksPtr& amd,
                                      const Ice::Current&) override;
 
 
    public:
        bool existsChannel(const std::string& channelName) const;
        void existsChannel_async(const AMD_ObserverInterface_existsChannelPtr& amd,
                                 const std::string& channelName,
                                 const Ice::Current&) const override;
 
    public:
        bool existsDataField(const std::string& channelName,
                             const std::string& datafieldName) const;
        void existsDataField_async(const AMD_ObserverInterface_existsDataFieldPtr& amd,
                                   const std::string& channelName,
                                   const std::string& datafieldName,
                                   const Ice::Current&) const override;
 
 
    public:
        /**
         * @brief This function creates a new datafield with new filter on the given datafield.
         * @param filter Configured filter object compatible with the datafieldRef (see \ref DatafieldFilter::getSupportedTypes())
         * @param datafieldRef Datafield for which a filtered datafield should be created
         * @return Returns a DatafieldRef to the new, filtered datafield.
         * @see \ref removeFilteredDatafield()
         */
        DatafieldRefBasePtr createFilteredDatafield(const DatafieldFilterBasePtr& filter,
                                                    const DatafieldRefBasePtr& datafieldRef);
        void
        createFilteredDatafield_async(const AMD_ObserverInterface_createFilteredDatafieldPtr& amd,
                                      const DatafieldFilterBasePtr& filter,
                                      const DatafieldRefBasePtr& datafieldRef,
                                      const Ice::Current&) override;
 
    public:
        DatafieldRefBasePtr createNamedFilteredDatafield(const std::string& filterDatafieldName,
                                                         const DatafieldFilterBasePtr& filter,
                                                         const DatafieldRefBasePtr& datafieldRef);
        void createNamedFilteredDatafield_async(
            const AMD_ObserverInterface_createNamedFilteredDatafieldPtr& amd,
            const std::string& filterDatafieldName,
            const DatafieldFilterBasePtr& filter,
            const DatafieldRefBasePtr& datafieldRef,
            const Ice::Current&) override;
 
    public:
        /**
         * @brief Removes a previously installed filter.
         * @param datafieldRef Datafield that was returned by createFilteredDatafield()
         * @see \ref createFilteredDatafield()
         */
        void removeFilteredDatafield(const DatafieldRefBasePtr& datafieldRef);
        void
        removeFilteredDatafield_async(const AMD_ObserverInterface_removeFilteredDatafieldPtr& amd,
                                      const DatafieldRefBasePtr& datafieldRef,
                                      const Ice::Current&) override;
 
    public:
        ChannelHistory getChannelHistory(const std::string& channelName,
                                         Ice::Float timestepMs,
                                         const Ice::Current& c) const;
        void getChannelHistory_async(const AMD_ObserverInterface_getChannelHistoryPtr& amd,
                                     const std::string& channelName,
                                     Ice::Float timestepMs,
                                     const Ice::Current& c) const override;
 
    public:
        ChannelHistory getPartialChannelHistory(const std::string& channelName,
                                                Ice::Long startTimestamp,
                                                Ice::Long endTimestamp,
                                                Ice::Float timestepMs,
                                                const Ice::Current& c) const;
        void
        getPartialChannelHistory_async(const AMD_ObserverInterface_getPartialChannelHistoryPtr& amd,
                                       const std::string& channelName,
                                       Ice::Long startTimestamp,
                                       Ice::Long endTimestamp,
                                       Ice::Float timestepMs,
                                       const Ice::Current& c) const override;
 
    public:
        TimedVariantBaseList getDatafieldHistory(const std::string& channelName,
                                                 const std::string& datafieldName,
                                                 Ice::Float timestepMs,
                                                 const Ice::Current& c) const;
        void getDatafieldHistory_async(const AMD_ObserverInterface_getDatafieldHistoryPtr& amd,
                                       const std::string& channelName,
                                       const std::string& datafieldName,
                                       Ice::Float timestepMs,
                                       const Ice::Current& c) const override;
 
    public:
        TimedVariantBaseList getPartialDatafieldHistory(const std::string& channelName,
                                                        const std::string& datafieldName,
                                                        Ice::Long startTimestamp,
                                                        Ice::Long endTimestamp,
                                                        Ice::Float timestepMs,
                                                        const Ice::Current& c) const;
        void getPartialDatafieldHistory_async(
            const AMD_ObserverInterface_getPartialDatafieldHistoryPtr& amd,
            const std::string& channelName,
            const std::string& datafieldName,
            Ice::Long startTimestamp,
            Ice::Long endTimestamp,
            Ice::Float timestepMs,
            const Ice::Current& c) const override;
 
        // ////////////////////////////////////////////////////////////////// //
        //internal functions
    protected:
        /**
        * Offer a channel. Use this in an observer specialization.
        *
        * The channel is not initialized until updateChannel() was called once.
        * @param channelName name of the channel
        * @param description expressive description of the provided datafields
        * @throw InvalidChannelException
        * @see updateChannel()
        */
        void offerChannel(std::string channelName, std::string description);
 
        /**
        * Offer a datafield with default value. Use this in an observer specialization.
        *
        * @param channelName name of the channel
        * @param datafieldName name of the datafield
        * @param defaultValue defines the default value and the datatype for the field
        * @param description expressive description of the datafield
        * @throw InvalidChannelException
        * @throw InvalidDataFieldException
        */
        void offerDataFieldWithDefault(std::string channelName,
                                       std::string datafieldName,
                                       const Variant& defaultValue,
                                       std::string description);
 
        /**
        * Offer a datafield without default value. Use this in an observer specialization.
        *
        * @param channelName name of the channel
        * @param datafieldName name of the datafield
        * @param defaultValue defines the default value and the datatype for the field
        * @param description expressive description of the datafield
        * @throw InvalidChannelException
        * @throw InvalidDataFieldException
        */
        void offerDataField(std::string channelName,
                            std::string datafieldName,
                            VariantTypeId type,
                            std::string description);
        bool offerOrUpdateDataField(std::string channelName,
                                    std::string datafieldName,
                                    const Variant& value,
                                    const std::string& description);
 
        void offerOrUpdateDataFieldsFlatCopy(const std::string& channelName,
                                             const StringVariantBaseMap& valueMap);
 
        /**
        * Offer a condition check. Use this in an observer specialization.
        *
        * @param checkName name of the check
        * @param conditionCheck check to register under checkName
        * @throw InvalidCheckException
        */
        void offerConditionCheck(std::string checkName, ConditionCheck* conditionCheck);
 
        /**
        * set datafield with datafieldName and in channel channelName
        *
        * @param channelName name of the channel
        * @param datafieldName name of the datafield within channel
        * @param value value for datafield
        * @throw InvalidChannelException
        * @throw InvalidDataFieldException
        * @return reference to Variant associated with the dataField
        */
        void setDataField(const std::string& channelName,
                          const std::string& datafieldName,
                          const Variant& value,
                          bool triggerFilterUpdate = true);
        void setDataFieldFlatCopy(const std::string& channelName,
                                  const std::string& datafieldName,
                                  const VariantPtr& value,
                                  bool triggerFilterUpdate = true);
        void setDataFieldsFlatCopy(const std::string& channelName,
                                   const StringVariantBaseMap& datafieldValues,
                                   bool triggerFilterUpdate = true);
        void setDataFieldsFlatCopy(
            const std::string& channelName,
            const std::unordered_map<::std::string, ::armarx::VariantBasePtr>& datafieldValues,
            bool triggerFilterUpdate = true);
        void updateDatafieldTimestamps(
            const std::string& channelName,
            const std::unordered_map<std::string, Ice::Long>& datafieldValues);
        void updateDatafieldTimestamps(const std::string& channelName, Ice::Long timestamp);
        void maybeOfferChannelAndSetDataFieldsFlatCopy(const std::string& channelName,
                                                       const std::string& description,
                                                       const StringVariantBaseMap& datafieldValues,
                                                       bool triggerFilterUpdate = true);
 
        /**
        * Update all conditions for a channel. Call this from the sensorActorUnit listener implementation if new data is posted.
        *
        * @param channelName name of the channel to update
        * @param updatedDatafields List of datafields that were updated. If empty, all datafields are checked. Leave empty if you would give all datafields anyway (better performance).
        * @throw InvalidChannelException
        */
        void
        updateChannel(const std::string& channelName,
                      const std::set<std::string>& updatedDatafields = std::set<std::string>());
 
        /**
        * Remove a channel. Use this in an observer specialization.
        *
        * @param channelName name of the channel
        */
        void removeChannel(std::string channelName);
 
        void removeDatafield(DataFieldIdentifierBasePtr id);
 
        std::set<std::string> updateDatafieldFilter(const std::string& channelName,
                                                    const std::string& datafieldName,
                                                    const VariantBasePtr& value);
 
        void scheduleDatafieldFilterUpdate(const std::string& channelName,
                                           const std::string& datafieldName,
                                           const VariantBasePtr& value);
 
 
        /**
        * Framework hook. Called once on initialization of the Observer.
        */
        virtual void onInitObserver() = 0;
 
        /**
        * Framework hook. Called on first run, after ice setup.
        */
        virtual void onConnectObserver() = 0;
 
        /**
        * Framework hook. Called on first run, after ice setup.
        */
        virtual void
        onExitObserver()
        {
        }
 
        void metaUpdateTask();
 
        // utility methods
        int generateId();
 
    private:
        void setDataFieldFlatCopy(const DataFieldRegistry::iterator& dataFieldIter,
                                  const VariantPtr& value);
 
 
        // check handling
        ConditionCheckPtr createCheck(const CheckConfiguration& configuration) const;
        CheckIdentifier registerCheck(const ConditionCheckPtr& check);
        void evaluateCheck(const ConditionCheckPtr& check,
                           const ChannelRegistryEntry& channel) const;
 
        // inherited from Component
        std::string
        getDefaultName() const override
        {
            return "Observer";
        }
 
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        void onInitComponent() override;
        void postOnConnectComponent() override;
        void onConnectComponent() override;
        void preOnDisconnectComponent() override;
        void onExitComponent() override;
 
        void updateRefreshRateChannel(const std::string& channelName);
 
        void channelUpdateFunction();
        void doChannelUpdate(const std::string& channelName,
                             const std::set<std::string>& updatedDatafields);
 
        void updateFilters();
 
        void addToChannelHistory(const std::pair<IceUtil::Time, ChannelRegistryEntry>& historyEntry,
                                 const std::string& channelName);
 
    protected:
        void addWorkerJob(const std::string& name, std::function<void(void)>&& f) const;
        void addWorkerJob(const std::string& name, std::function<void(void)>&& f);
 
        void
        callAndPassExceptionToAMD(auto& amd, auto f) const
        {
            try
            {
                f();
            }
            catch (std::exception& e)
            {
                ARMARX_ERROR << GetHandledExceptionString();
                amd->ice_exception(e);
            }
            catch (...)
            {
                auto s = GetHandledExceptionString();
                ARMARX_ERROR << s;
                amd->ice_exception(std::runtime_error{std::move(s)});
            }
        }
 
    private:
        template <class... Params>
        void
        internalAddWorkerJob(const std::string& name,
                             auto* derivedPtr,
                             const auto& amd,
                             auto fnc,
                             const Params&... ps) const
        {
            ARMARX_CHECK_EXPRESSION(derivedPtr);
            addWorkerJob(name,
                         [this, derivedPtr, amd, fnc, params = std::make_tuple(ps...)]
                         {
                             callAndPassExceptionToAMD(
                                 amd,
                                 [&]
                                 {
                                     auto lambda = [&](auto&&... ps2)
                                     { return (derivedPtr->*fnc)(std::move(ps2)...); };
                                     using return_t = decltype(std::apply(lambda, params));
                                     if constexpr (std::is_same_v<void, return_t>)
                                     {
 
                                         std::apply(lambda, params);
                                         amd->ice_response();
                                     }
                                     else
                                     {
                                         amd->ice_response(std::apply(lambda, params));
                                     }
                                 });
                         });
        }
 
    protected:
        template <class AMDPtr, class FReturn, class FOwner, class... FParams, class... Params>
        void
        addWorkerJob(const std::string& name,
                     const AMDPtr& amd,
                     FReturn (FOwner::*fnc)(FParams...) const,
                     const Params&... ps) const
        {
            static_assert(std::is_base_of_v<Observer, FOwner>);
            internalAddWorkerJob(name, dynamic_cast<const FOwner*>(this), amd, fnc, ps...);
        }
 
        template <class AMDPtr, class FReturn, class FOwner, class... FParams, class... Params>
        void
        addWorkerJob(const std::string& name,
                     const AMDPtr& amd,
                     FReturn (FOwner::*fnc)(FParams...),
                     const Params&... ps)
        {
            static_assert(std::is_base_of_v<Observer, FOwner>);
            internalAddWorkerJob(name, dynamic_cast<FOwner*>(this), amd, fnc, ps...);
        }
 
        virtual void preWorkerJobs();
        virtual void postWorkerJobs();
        using ClockT = std::chrono::high_resolution_clock;
        using TimepointT = typename ClockT::time_point;
        static TimepointT Now();
        static float TimeDeltaInMs(TimepointT t0);
 
        void runWorker();
 
        // This mutex is used by derived classes ...
        mutable std::recursive_mutex channelsMutex;
 
        struct Impl;
        std::unique_ptr<Impl> impl;
    };
} // namespace armarx