Authority.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_AUTHORITY_H_
#define _MIRA_AUTHORITY_H_

#ifndef _MIRA_FRAMEWORK_H_
#  error "Authority.h must be included via the Framework.h. You must not include it directly. Use #include <fw/Framework.h> instead"
#endif

#include <utils/EnumToFlags.h>
#include <rpc/RPCFuture.h>

#include <math/NearestNeighborInterpolator.h>

#include <serialization/adapters/std/set>
#include <serialization/PropertyTree.h>

#include <fw/AbstractAuthority.h>
#include <fw/AuthorityRuntime.h>
#include <fw/Channel.h>
#include <fw/FrameworkTransformer.h>
#include <fw/RPCHandler.h>
#include <fw/ServiceInterfaceHandler.h>

namespace mira {


// forward declarations
struct TransformDesc;
class AuthorityManager;
class FrameworkTransformerNode;
template<typename>
class ServiceCall;


#define MIRA_HIDDEN_AUTHORITY_PREFIX "#"

class Authority : public AbstractAuthority, public DiagnosticsModule
{
public:

    enum Flags
    {
        NORMAL = 0x0000,

        ANONYMOUS = 0x0001,

        HIDDEN = 0x0002,

        INVISIBLE_PUBLISHER_SUBSCRIBER = 0x0004,

        INTERNAL = 0x000A,

        INDEPENDENT_SUBSCRIBER_THREAD = 0x0010,

        INDEPENDENT_RPC_THREAD = 0x0020,

        NO_MAIN_THREAD = 0x0040

    };
    MIRA_ENUM_TO_FLAGS_INCLASS(Flags)

public:

    Authority(Flags flags = NORMAL);

    Authority(const ResourceName& fullname, Flags flags = NORMAL);

    Authority(const ResourceName& ns, const std::string& name, Flags flags = NORMAL);

    Authority(Authority& iParent, const std::string& name, Flags flags = ANONYMOUS);

    ~Authority() override;

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.roproperty("Status", &getStatusManager(), "The status of this authority");
        r.property("HeartbeatInterval",
                   getter<Duration>(&DiagnosticsModule::getHeartbeatInterval,
                                    (DiagnosticsModule*)this),
                   setter<Duration>(&DiagnosticsModule::setHeartbeatInterval,
                                    (DiagnosticsModule*)this),
                   "The interval of the watchdog",
                   serialization::IgnoreMissing());
    }

public:

    void checkin(const ResourceName& fullname,
                 AuthorityRuntimePtr runtime = AuthorityRuntimePtr());

    void checkin(const ResourceName& ns, const std::string& name,
                 AuthorityRuntimePtr runtime = AuthorityRuntimePtr());

    void checkin(Authority& iParent, const ResourceName& ns,
                 const std::string& name);

    void checkin(Authority& iParent, const std::string& name);

    void checkout();

    bool isValid() const {
        return mValid;
    }

    void validate() const {
        if ( !isValid() )
            MIRA_THROW(XInvalidConfig, "Authority is not registered.");
    }

    void startWithParent(bool start) {
        mStartWithParent = start;
    }

    AuthorityRuntimePtr getRuntime() {
        return mRuntime;
    }

    std::set<Authority*> getChildren() {
        return mChildren;
    }

    void addImmediateHandler(DiagnosticRunnablePtr runnable);

    template <typename F>
    void addImmediateHandlerFunction(F&& fn);

    void addFinalizeHandler(DiagnosticRunnablePtr runnable);

    template <typename F>
    void addFinalizeHandlerFunction(F&& fn);

    TimerPtr createTimer(Duration period, TimerCallback callback,
                         bool oneshot=false);
    
    TimerPtr createTimer(Duration period, Duration tolerance,
                         TimerCallback callback, bool oneshot=false);

    template<typename Class>
    TimerPtr createTimer(Duration period,
                         void (Class::*f)(const Timer&), Class* obj,
                         bool oneshot=false);

    template<typename Class>
    TimerPtr createTimer(Duration period, void (Class::*f)(const Timer&),
                         bool oneshot=false);

    template<typename Class>
    TimerPtr createTimer(Duration period, Duration tolerance,
                         void (Class::*f)(const Timer&), Class* obj,
                         bool oneshot=false);

    template<typename Class>
    TimerPtr createTimer(Duration period, Duration tolerance,
                         void (Class::*f)(const Timer&), bool oneshot=false);

    TimerPtr createTimer(Time time, TimerCallback callback);

    template<typename Class>
    TimerPtr createTimer(Time time, void (Class::*f)(const Timer&), Class* obj);

    template<typename Class>
    TimerPtr createTimer(Time time, void (Class::*f)(const Timer&));

    void removeTimer(TimerPtr timer);

    void start() override;

    void stop() override;

    bool spin(const Duration& maxWait = Duration::infinity());

    [[nodiscard]] bool hasWork(Time horizon = Time::now()) const;

    bool hasUnrecoverableFailure() const override;

    bool isStarted() const override;

    bool isRunning() const override;


public:

    void enableSubscriberCallbacks(bool enable);

public:

    // Channel framework

    std::string resolveName(const std::string& name) const;

    std::string resolveServiceName(const std::string& name) const;

    template<typename T>
    Channel<T> publish(const std::string& channelID)
    {
        static_assert(std::is_void<T>::value!=true, 
                      "You must specify a typename for untyped channels");
        return publish<T>(channelID, typeName<T>());
    }

    template<typename T>
    Channel<T> publish(const std::string& channelID, const Typename& type);

    void unpublish(const std::string& channelID);

    template<typename T>
    Channel<T> subscribe(const std::string& channelID,
                         const Duration& storageDuration = Duration::seconds(0));

    template<typename T>
    Channel<T> subscribe(const std::string& channelID,
                         boost::function<void (ChannelRead<T>)> fn,
                         const Duration& storageDuration = Duration::seconds(0),
                         bool independentThread = false);

    template<typename T>
    Channel<T> subscribe(const std::string& channelID,
                         boost::function<void (ChannelRead<T>)> fn,
                         bool independentThread);

    template<typename T, typename Class>
    Channel<T> subscribe(const std::string& channelID,
                         void (Class::*f)(ChannelRead<T>),
                         Class* obj,
                         const Duration& storageDuration = Duration::seconds(0),
                         bool independentThread = false);

    template<typename T, typename Class>
    Channel<T> subscribe(const std::string& channelID,
                         void (Class::*f)(ChannelRead<T>),
                         Class* obj, bool independentThread);

    template<typename T, typename Class>
    Channel<T> subscribe(const std::string& channelID,
                         void (Class::*f)(ChannelRead<T>),
                         const Duration& storageDuration = Duration::seconds(0),
                         bool independentThread = false)
    {
        Class* obj = dynamic_cast<Class*>(this);
        assert(obj!=NULL &&
               "Called 'Authority::subscribe(..., Class::member, ...)' on an object that is not "
               "of type Class (you may need to provide a pointer to the correct object)!");
        return subscribe<T>(channelID, f, obj, storageDuration, independentThread);
    }

    template<typename T, typename Class>
    Channel<T> subscribe(const std::string& channelID,
                         void (Class::*f)(ChannelRead<T>), bool independentThread)
    {
        Class* obj = dynamic_cast<Class*>(this);
        assert(obj!=NULL &&
               "Called 'Authority::subscribe(..., Class::member, ...)' on an object that is not "
               "of type Class (you may need to provide a pointer to the correct object)!");
        return subscribe<T>(channelID, f, obj, independentThread);
    }

    template<typename T>
    Channel<T> publishAndSubscribe(const std::string& channelID,
                                      const Duration& storageDuration = Duration::seconds(0));

    template<typename T>
    Channel<T> publishAndSubscribe(const std::string& channelID,
                                      boost::function<void (ChannelRead<T>)> fn,
                                      const Duration& storageDuration = Duration::seconds(0),
                                      bool independentThread = false);

    template<typename T>
    Channel<T> publishAndSubscribe(const std::string& channelID,
                                      boost::function<void (ChannelRead<T>)> fn,
                                      bool independentThread);

    template<typename T, typename Class>
    Channel<T> publishAndSubscribe(const std::string& channelID,
                                      void (Class::*f)(ChannelRead<T>),
                                      Class* obj,
                                      const Duration& storageDuration = Duration::seconds(0),
                                      bool independentThread = false);

    template<typename T, typename Class>
    Channel<T> publishAndSubscribe(const std::string& channelID,
                                      void (Class::*f)(ChannelRead<T>),
                                      Class* obj, bool independentThread);

    template<typename T, typename Class>
    Channel<T> publishAndSubscribe(const std::string& channelID,
                                      void (Class::*f)(ChannelRead<T>),
                                      const Duration& storageDuration = Duration::seconds(0),
                                      bool independentThread = false)
    {
        Class* obj = dynamic_cast<Class*>(this);
        assert(obj!=NULL);
        return publishAndSubscribe<T>(channelID, f, obj, storageDuration, independentThread);
    }

    template<typename T, typename Class>
    Channel<T> publishAndSubscribe(const std::string& channelID,
                                      void (Class::*f)(ChannelRead<T>),
                                      bool independentThread)
    {
        Class* obj = dynamic_cast<Class*>(this);
        assert(obj!=NULL);
        return publishAndSubscribe<T>(channelID, f, obj, independentThread);
    }

    template<typename T>
    Channel<T> subscribeInterval(const std::string& channelID,
                         boost::function<void (ChannelReadInterval<T>)> fn,
                         const Duration& storageDuration,
                         bool independentThread = false,
                         const Time& startAfter = Time::unixEpoch());

    /*
     * Provided for simplified registration of a member function as callback
     * function. See the above subscribe methods for more details.
     */
    template<typename T, typename Class>
    Channel<T> subscribeInterval(const std::string& channelID,
                         void (Class::*f)(ChannelReadInterval<T>),
                         Class* obj,
                         const Duration& storageDuration,
                         bool independentThread = false,
                         const Time& startAfter = Time::unixEpoch());

    template<typename T, typename Class>
    Channel<T> subscribeInterval(const std::string& channelID,
                         void (Class::*f)(ChannelReadInterval<T>),
                         const Duration& storageDuration,
                         bool independentThread = false,
                         const Time& startAfter = Time::unixEpoch());

    template<typename T, typename Class>
    Channel<T> subscribeIntervalByElements(const std::string& channelID,
                         void (Class::*f)(ChannelRead<T>),
                         Class* obj,
                         const Duration& storageDuration = Duration::seconds(0),
                         bool independentThread = false,
                         const Time& startAfter = Time::unixEpoch());

    template <typename T>
    Channel<T> subscribeIntervalByElements(const std::string& channelID,
                         boost::function<void(ChannelRead<T>)>,
                         const Duration& storageDuration = Duration::seconds(0),
                         bool independentThread = false,
                         const Time& startAfter = Time::unixEpoch());

    template<typename T, typename Class>
    Channel<T> subscribeIntervalByElements(const std::string& channelID,
                         void (Class::*f)(ChannelRead<T>),
                         const Duration& storageDuration = Duration::seconds(0),
                         bool independentThread = false,
                         const Time& startAfter = Time::unixEpoch());

    template <typename T>
    ChannelRead<T> waitForData(Channel<T> channel,
                               Duration timeout = Duration::infinity()) const;

    void unsubscribe(const std::string& channelID);

    template <typename T>
    void unsubscribe(const std::string& channelID);

    template <typename T>
    Channel<T> getChannel(const std::string& channelID);

    bool isSubscribedOn(const std::string& channelID) const;

    MIRA_DEPRECATED("Please use isSubscribedOn() instead",
    bool hasSubscribedOn(const std::string& channelID) const) {
        return isSubscribedOn(channelID);
    }

    bool hasPublished(const std::string& channelID) const;

    bool doesChannelExist(const std::string& channelID) const;

    bool waitForChannel(const std::string& channelID,
                        const Duration& timeout = Duration::infinity()) const;


public:

    // Transform framework


    bool isTransformAvailable(const std::string& targetID, const std::string& sourceID) const;

    bool isTransformAvailable(const std::string& targetID, const std::string& sourceID,
                              const std::string& fixedID) const;

    bool waitForTransform(const std::string& targetID, const std::string& sourceID,
                          Duration timeout = Duration::invalid()) const;

    bool waitForTransform(const std::string& targetID, const std::string& sourceID,
                          const std::string& fixedID, Duration timeout = Duration::invalid()) const;


    template<typename Transform, typename Filter>
    void subscribeTransform(const std::string& targetID,
                            const std::string& sourceID,
                            boost::function<void (Transform, Time)> fn,
                            Filter&& filter);

    template<typename Transform, typename Filter, typename Class>
    void subscribeTransform(const std::string& targetID,
                            const std::string& sourceID,
                            void (Class::*f)(Transform, Time), Class* obj,
                            Filter&& filter);

    template<typename Transform, typename Filter, typename Class>
    void subscribeTransform(const std::string& targetID,
                            const std::string& sourceID,
                            void (Class::*f)(Transform, Time),
                             Filter&& filter);

    template<typename Transform>
    void subscribeTransform(const std::string& targetID,
                            const std::string& sourceID,
                            boost::function<void (Transform, Time)> fn);

    template<typename Transform, typename Class>
    void subscribeTransform(const std::string& targetID,
                            const std::string& sourceID,
                            void (Class::*f)(Transform, Time), Class* obj);

    template<typename Transform, typename Class>
    void subscribeTransform(const std::string& targetID,
                            const std::string& sourceID,
                            void (Class::*f)(Transform, Time));



    template<typename Transform, typename Filter>
    Transform getTransform(const std::string& targetID,
                           const std::string& sourceID,
                           const Time& time, Filter&& filter) const;

    template<typename Transform>
    Transform getTransform(const std::string& targetID,
                           const std::string& sourceID,
                           const Time& time = Time()) const
    {
        return getTransform<Transform>(targetID, sourceID, time,
                                       NearestNeighborInterpolator());
    }

    template<typename Transform, typename Filter>
    Transform getTransform(const std::string& targetID, const Time& targetTime,
                           const std::string& sourceID, const Time& sourceTime,
                           const std::string& fixedID,
                           Filter&& filter) const;

    template<typename Transform>
    Transform getTransform(const std::string& targetID, const Time& targetTime,
                           const std::string& sourceID, const Time& sourceTime,
                           const std::string& fixedID) const
    {
        return getTransform<Transform>(targetID, targetTime, sourceID,
                                       sourceTime, fixedID,
                                       NearestNeighborInterpolator());
    }




    FrameworkTransformerNode* getTransformNode(const std::string& frameID) const;

    TransformDesc prepareTransform(const std::string& targetID,
                                   const std::string& sourceID) const;

    TransformDesc prepareTransform(const std::string& targetID,
                                   const std::string& sourceID,
                                   const std::string& fixedID) const;

    template<typename Transform, typename Filter>
    Transform getTransform(const TransformDesc& desc, const Time& time,
                           Filter&& filter) const;

    template<typename Transform>
    Transform getTransform(const TransformDesc& desc, const Time& time = Time()) const
    {
        return getTransform<Transform>(desc, time, NearestNeighborInterpolator());
    }

    template<typename Transform, typename Filter>
    Transform getTransform(const TransformDesc& desc, const Time& targetTime,
                           const Time& sourceTime, Filter&& filter) const;

    template<typename Transform>
    Transform getTransform(const TransformDesc& desc, const Time& targetTime,
                           const Time& sourceTime) const
    {
        return getTransform<Transform>(desc, targetTime, sourceTime,
                                       NearestNeighborInterpolator());
    }

    template<typename Transform>
    void publishTransform(const std::string& frameID, const Transform& transform,
                          const Time& time) const;

    template<typename Transform, typename Filter = NearestNeighborInterpolator>
    void publishTransformIndirect(const std::string& frameID,
                                  const std::string& targetID,
                                  const std::string& sourceID,
                                  const Transform& transform, const Time& time,
                                  Filter&& filter = NearestNeighborInterpolator()) const;

    void addTransformLink(const std::string& childID, const std::string& parentID) const;

    void addTransformLink(const std::string& childID,
                          const std::string& parentID,
                          FrameworkTransformerNode::Type type) const;


public:

    // RPC Framework


    template<typename Service>
    void publishService(const std::string& name, Service& iService,
                        RPCHandlerPtr handler = RPCHandlerPtr());

    template<typename Service>
    void publishService(Service& iService,
                        RPCHandlerPtr handler = RPCHandlerPtr());

    void unpublishService();

    void unpublishService(const std::string& name);

    template<typename R, typename... ARGS>
    inline RPCFuture<R> callService(const std::string& iService, const std::string& method,
                                    ARGS&&... args) const;

    RPCFuture<JSONRPCResponse> callServiceJSON(const json::Value& jsonRequest) const;

    RPCFuture<JSONRPCResponse> callServiceJSON(const std::string& jsonString) const;

    RPCFuture<JSONRPCResponse> callServiceJSON(const std::string& service,
                                               const std::string& method,
                                               const json::Value& params = json::Value()) const;

    RPCFuture<JSONRPCResponse> callServiceJSON(const std::string& service,
                                               const std::string& method,
                                               const std::string& params) const;

    RPCFuture<JSONRPCResponse> callServiceJSON(const RPCCallDefinition& rpc) const;

    template<class F>
    [[nodiscard]] ServiceCall<F> createServiceCall(const std::string& service, std::string method,
                                                   bool waitTillExists = true, bool showBootupMsg = true);

    bool existsService(const std::string& name) const;

    bool implementsInterface(const std::string& name,
                             const std::string& interface) const;

    bool waitForService(const std::string& name, 
                        Duration timeout = Duration::invalid()) const;

    std::string waitForServiceInterface(const std::string& interface, 
                                        Duration timeout = Duration::invalid()) const;

    std::list<std::string> queryServicesForInterface(const std::string& interface) const;

    void registerCallbackForInterface(const std::string& interface,
                                      ServiceInterfaceHandler::Callback cb);

    template<typename Class>
    void registerCallbackForInterface(const std::string& interface,
                                      void (Class::*f)(const std::string&,
                                                       const std::string&),
                                      Class* obj);


public:

    StatusManager& getStatusManager();

    //{@

    StatusManager::StatusMap getStatusMap() override;

    boost::shared_ptr<PropertyNode> getProperties() override;

    std::set<std::string> getPublishedChannels() override;

    std::set<std::string> getSubscribedChannels() override;

    std::set<std::string> getServiceInterfaces() override;

    ChannelNameMapping getPublishedChannelNames() override;

    ChannelNameMapping getSubscribedChannelNames() override;

    ChannelStatusMap getSubscribedChannelStatus() override;


protected:

    template<typename T>
    Channel<T> toProxy(ConcreteChannel<T>* channel);

    void setChannelReadAccess(const std::string& channelID);
    void setChannelWriteAccess(const std::string& channelID);
    void removeChannelReadAccess(const std::string& channelID);
    void removeChannelWriteAccess(const std::string& channelID);
    void insertChannelNameMapping(AbstractAuthority::ChannelNameMapping& map,
                                  const std::string& local,
                                  const std::string& global);
    void eraseChannelNameMapping(AbstractAuthority::ChannelNameMapping& map,
                                 const std::string& local,
                                 const std::string& global);

    void remotePublishService(const std::string& service) const;

    friend struct AuthorityService;
    friend class AuthorityManager;

    PropertyTree getPropertiesRPC();

    void setProperty(const std::string& property, const std::string& value);
    std::string getProperty(const std::string& property) const;
    void setPropertyJSON(const std::string& property, const json::Value& value);
    json::Value getPropertyJSON(const std::string& property) const;

protected:

    struct RWAccessFlags {
        RWAccessFlags() : flags(CHANNEL_ACCESS_NONE) {}
        ChannelAccessFlags flags;
    };

    typedef std::map<std::string, RWAccessFlags> RWAccessMap;
    typedef std::list<AbstractChannelSubscriberPtr> SubscribePtrList;

private:

    void init();

private:

    AuthorityRuntimePtr mRuntime;
    bool mOwnsRuntime;
    bool mValid;
    bool mStarted;
    bool mIsInternal;
    bool mIsInvisiblePublisherSubscriber;

    Flags mFlags;
    RWAccessMap mAccessMap;
    Authority* mParent;
    bool mStartWithParent;
    std::set<Authority*> mChildren;

    bool mSubscriberEnabled;

    typedef boost::shared_ptr<DispatcherThread> DispatcherThreadPtr;

    typedef std::list<DispatcherThreadPtr> IndependentSubscribers;

    IndependentSubscribers mIndependentSubscriberDispatchers;

    DispatcherThreadPtr mSubscriberDispatcher;
    DispatcherThreadPtr mRPCDispatcher;

    RPCHandlerPtr mRPCHandler;
    ServiceInterfaceHandlerPtr mServiceInterfaceHandler;
    SubscribePtrList mSubscriberList;
    std::set<std::string> mPublishedServices;
    boost::mutex mNameMutex;
    ChannelNameMapping mPublishedChannelNames;
    ChannelNameMapping mSubscribedChannelNames;
    StatusManager mStatusManager;
};

typedef boost::shared_ptr<Authority> AuthorityPtr;


} // namespace

#endif