Node.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef IGN_TRANSPORT_NODE_HH_
#define IGN_TRANSPORT_NODE_HH_

#include <algorithm>
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <unordered_set>
#include <vector>

// ToDo: Remove after fixing the warnings
#ifdef _MSC_VER
#pragma warning(push, 0)
#endif
#include <ignition/msgs.hh>
#ifdef _MSC_VER
#pragma warning(pop)
#endif

#include "ignition/transport/AdvertiseOptions.hh"
#include "ignition/transport/Helpers.hh"
#include "ignition/transport/NodeOptions.hh"
#include "ignition/transport/NodeShared.hh"
#include "ignition/transport/Publisher.hh"
#include "ignition/transport/RepHandler.hh"
#include "ignition/transport/ReqHandler.hh"
#include "ignition/transport/SubscribeOptions.hh"
#include "ignition/transport/SubscriptionHandler.hh"
#include "ignition/transport/TopicUtils.hh"
#include "ignition/transport/TransportTypes.hh"

namespace ignition
{
  namespace transport
  {
    class NodePrivate;

    IGNITION_TRANSPORT_VISIBLE void waitForShutdown();

    class IGNITION_TRANSPORT_VISIBLE Node
    {
      class PublisherPrivate;

      public: class Publisher
      {
        public: Publisher();

        public: explicit Publisher(const MessagePublisher &_publisher);

        public: virtual ~Publisher();

        public: operator bool();

        public: bool Valid() const;

        public: bool Publish(const ProtoMsg &_msg);

        private: bool UpdateThrottling();

        public: bool HasConnections() const;

        private: std::shared_ptr<PublisherPrivate> dataPtr;
      };

      public: explicit Node(const NodeOptions &_options = NodeOptions());

      public: virtual ~Node();

      public: template<typename T> Node::Publisher Advertise(
                  const std::string &_topic,
            const AdvertiseMessageOptions &_options = AdvertiseMessageOptions())
      {
        return this->Advertise(_topic, T().GetTypeName(), _options);
      }

      public: Node::Publisher Advertise(const std::string &_topic,
                                        const std::string &_msgTypeName,
            const AdvertiseMessageOptions &_options = AdvertiseMessageOptions())
      {
        std::string fullyQualifiedTopic;
        if (!TopicUtils::FullyQualifiedName(this->Options().Partition(),
          this->Options().NameSpace(), _topic, fullyQualifiedTopic))
        {
          std::cerr << "Topic [" << _topic << "] is not valid." << std::endl;
          return Publisher();
        }

        auto currentTopics = this->AdvertisedTopics();

        if (std::find(currentTopics.begin(), currentTopics.end(),
              fullyQualifiedTopic) != currentTopics.end())
        {
          std::cerr << "Topic [" << _topic << "] already advertised. You cannot"
                    << " advertise the same topic twice on the same node."
                    << " If you want to advertise the same topic with different"
                    << " types, use separate nodes" << std::endl;
          return Publisher();
        }

        std::lock_guard<std::recursive_mutex> lk(this->Shared()->mutex);

        // Notify the discovery service to register and advertise my topic.
        MessagePublisher publisher(fullyQualifiedTopic,
          this->Shared()->myAddress,
          this->Shared()->myControlAddress,
          this->Shared()->pUuid, this->NodeUuid(), _msgTypeName, _options);

        if (!this->Shared()->msgDiscovery->Advertise(publisher))
        {
          std::cerr << "Node::Advertise(): Error advertising a topic. "
                    << "Did you forget to start the discovery service?"
                    << std::endl;
          return Publisher();
        }

        return Publisher(publisher);
      }

      public: std::vector<std::string> AdvertisedTopics() const;

      public: template<typename T> bool Subscribe(
          const std::string &_topic,
          void(*_cb)(const T &_msg),
          const SubscribeOptions &_opts = SubscribeOptions())
      {
        std::function<void(const T &, const MessageInfo &)> f =
          [_cb](const T & _internalMsg, const MessageInfo &/*_internalInfo*/)
        {
          (*_cb)(_internalMsg);
        };

        return this->Subscribe<T>(_topic, f, _opts);
      }

      public: template<typename T> bool Subscribe(
          const std::string &_topic,
          std::function<void(const T &_msg)> &_cb,
          const SubscribeOptions &_opts = SubscribeOptions())
      {
        std::function<void(const T &, const MessageInfo &)> f =
          [_cb](const T & _internalMsg, const MessageInfo &/*_internalInfo*/)
        {
          _cb(_internalMsg);
        };

        return this->Subscribe<T>(_topic, f, _opts);
      }

      public: template<typename C, typename T> bool Subscribe(
          const std::string &_topic,
          void(C::*_cb)(const T &_msg),
          C *_obj,
          const SubscribeOptions &_opts = SubscribeOptions())
      {
        std::function<void(const T &, const MessageInfo &)> f =
          [_cb, _obj](const T & _internalMsg,
                      const MessageInfo &/*_internalInfo*/)
        {
          auto cb = std::bind(_cb, _obj, std::placeholders::_1);
          cb(_internalMsg);
        };

        return this->Subscribe<T>(_topic, f, _opts);
      }

      public: template<typename T> bool Subscribe(
          const std::string &_topic,
          void(*_cb)(const T &_msg, const MessageInfo &_info),
          const SubscribeOptions &_opts = SubscribeOptions())
      {
        std::function<void(const T &, const MessageInfo &)> f =
          [_cb](const T & _internalMsg, const MessageInfo &_internalInfo)
        {
          (*_cb)(_internalMsg, _internalInfo);
        };

        return this->Subscribe<T>(_topic, f, _opts);
      }

      public: template<typename T> bool Subscribe(
          const std::string &_topic,
          std::function<void(const T &_msg, const MessageInfo &_info)> &_cb,
          const SubscribeOptions &_opts = SubscribeOptions())
      {
        std::string fullyQualifiedTopic;
        if (!TopicUtils::FullyQualifiedName(this->Options().Partition(),
          this->Options().NameSpace(), _topic, fullyQualifiedTopic))
        {
          std::cerr << "Topic [" << _topic << "] is not valid." << std::endl;
          return false;
        }

        // Create a new subscription handler.
        std::shared_ptr<SubscriptionHandler<T>> subscrHandlerPtr(
            new SubscriptionHandler<T>(this->NodeUuid(), _opts));

        // Insert the callback into the handler.
        subscrHandlerPtr->SetCallback(_cb);

        std::lock_guard<std::recursive_mutex> lk(this->Shared()->mutex);

        // Store the subscription handler. Each subscription handler is
        // associated with a topic. When the receiving thread gets new data,
        // it will recover the subscription handler associated to the topic and
        // will invoke the callback.
        this->Shared()->localSubscriptions.AddHandler(
          fullyQualifiedTopic, this->NodeUuid(), subscrHandlerPtr);

        // Add the topic to the list of subscribed topics (if it was not before)
        this->TopicsSubscribed().insert(fullyQualifiedTopic);

        // Discover the list of nodes that publish on the topic.
        if (!this->Shared()->msgDiscovery->Discover(fullyQualifiedTopic))
        {
          std::cerr << "Node::Subscribe(): Error discovering a topic. "
                    << "Did you forget to start the discovery service?"
                    << std::endl;
          return false;
        }

        return true;
      }

      public: template<typename C, typename T> bool Subscribe(
          const std::string &_topic,
          void(C::*_cb)(const T &_msg, const MessageInfo &_info),
          C *_obj,
          const SubscribeOptions &_opts = SubscribeOptions())
      {
        std::function<void(const T &, const MessageInfo &)> f =
          [_cb, _obj](const T & _internalMsg, const MessageInfo &_internalInfo)
        {
          auto cb = std::bind(_cb, _obj, std::placeholders::_1,
            std::placeholders::_2);
          cb(_internalMsg, _internalInfo);
        };

        return this->Subscribe<T>(_topic, f, _opts);
      }

      public: std::vector<std::string> SubscribedTopics() const;

      public: bool Unsubscribe(const std::string &_topic);

      public: template<typename T1, typename T2> bool Advertise(
        const std::string &_topic,
        void(*_cb)(const T1 &_req, T2 &_rep, bool &_result),
        const AdvertiseServiceOptions &_options = AdvertiseServiceOptions())
      {
        std::function<void(const T1 &, T2 &, bool &)> f =
          [_cb](const T1 &_internalReq, T2 &_internalRep, bool &_internalResult)
        {
          (*_cb)(_internalReq, _internalRep, _internalResult);
        };

        return this->Advertise<T1, T2>(_topic, f, _options);
      }

      public: template<typename T> bool Advertise(
        const std::string &_topic,
        void(*_cb)(T &_rep, bool &_result),
        const AdvertiseServiceOptions &_options = AdvertiseServiceOptions())
      {
        std::function<void(const msgs::Empty &, T &, bool &)> f =
          [_cb](const msgs::Empty &/*_internalReq*/, T &_internalRep,
                bool &_internalResult)
        {
          (*_cb)(_internalRep, _internalResult);
        };
        return this->Advertise<msgs::Empty, T>(_topic, f, _options);
      }

      public: template<typename T> bool Advertise(
        const std::string &_topic,
        void(*_cb)(const T &_req),
        const AdvertiseServiceOptions &_options = AdvertiseServiceOptions())
      {
        std::function<void(const T &, ignition::msgs::Empty &, bool &)> f =
          [_cb](const T &_internalReq, ignition::msgs::Empty &/*_internalRep*/,
                bool &/*_internalResult*/)
        {
          (*_cb)(_internalReq);
        };

        return this->Advertise<T, ignition::msgs::Empty>(_topic, f, _options);
      }

      public: template<typename T1, typename T2> bool Advertise(
        const std::string &_topic,
        std::function<void(const T1 &_req, T2 &_rep, bool &_result)> &_cb,
        const AdvertiseServiceOptions &_options = AdvertiseServiceOptions())
      {
        std::string fullyQualifiedTopic;
        if (!TopicUtils::FullyQualifiedName(this->Options().Partition(),
          this->Options().NameSpace(), _topic, fullyQualifiedTopic))
        {
          std::cerr << "Service [" << _topic << "] is not valid." << std::endl;
          return false;
        }

        // Create a new service reply handler.
        std::shared_ptr<RepHandler<T1, T2>> repHandlerPtr(
          new RepHandler<T1, T2>());

        // Insert the callback into the handler.
        repHandlerPtr->SetCallback(_cb);

        std::lock_guard<std::recursive_mutex> lk(this->Shared()->mutex);

        // Add the topic to the list of advertised services.
        this->SrvsAdvertised().insert(fullyQualifiedTopic);

        // Store the replier handler. Each replier handler is
        // associated with a topic. When the receiving thread gets new requests,
        // it will recover the replier handler associated to the topic and
        // will invoke the service call.
        this->Shared()->repliers.AddHandler(
          fullyQualifiedTopic, this->NodeUuid(), repHandlerPtr);

        // Notify the discovery service to register and advertise my responser.
        ServicePublisher publisher(fullyQualifiedTopic,
          this->Shared()->myReplierAddress,
          this->Shared()->replierId.ToString(),
          this->Shared()->pUuid, this->NodeUuid(),
          T1().GetTypeName(), T2().GetTypeName(), _options);

        if (!this->Shared()->srvDiscovery->Advertise(publisher))
        {
          std::cerr << "Node::Advertise(): Error advertising a service. "
                    << "Did you forget to start the discovery service?"
                    << std::endl;
          return false;
        }

        return true;
      }

      public: template<typename T> bool Advertise(
        const std::string &_topic,
        std::function<void(T &_rep, bool &_result)> &_cb,
        const AdvertiseServiceOptions &_options = AdvertiseServiceOptions())
      {
        std::function<void(const msgs::Empty &, T &, bool &)> f =
          [_cb](const msgs::Empty &/*_internalReq*/, T &_internalRep,
                bool &_internalResult)
        {
          (_cb)(_internalRep, _internalResult);
        };
        return this->Advertise<msgs::Empty, T>(_topic, f, _options);
      }

      public: template<typename T> bool Advertise(
        const std::string &_topic,
        std::function<void(const T &_req)> &_cb,
        const AdvertiseServiceOptions &_options = AdvertiseServiceOptions())
      {
        std::function<void(const T &, ignition::msgs::Empty &, bool &)> f =
          [_cb](const T &_internalReq, ignition::msgs::Empty &/*_internalRep*/,
                bool &/*_internalResult*/)
        {
          (_cb)(_internalReq);
        };

        return this->Advertise<T, ignition::msgs::Empty>(_topic, f, _options);
      }

      public: template<typename C, typename T1, typename T2> bool Advertise(
        const std::string &_topic,
        void(C::*_cb)(const T1 &_req, T2 &_rep, bool &_result),
        C *_obj,
        const AdvertiseServiceOptions &_options = AdvertiseServiceOptions())
      {
        std::function<void(const T1 &, T2 &, bool &)> f =
          [_cb, _obj](const T1 &_internalReq,
                      T2 &_internalRep,
                      bool &_internalResult)
        {
          auto cb = std::bind(_cb, _obj, std::placeholders::_1,
            std::placeholders::_2, std::placeholders::_3);
          cb(_internalReq, _internalRep, _internalResult);
        };

        return this->Advertise<T1, T2>(_topic, f, _options);
      }

      public: template<typename C, typename T> bool Advertise(
        const std::string &_topic,
        void(C::*_cb)(T &_rep, bool &_result),
        C *_obj,
        const AdvertiseServiceOptions &_options = AdvertiseServiceOptions())
      {
        std::function<void(const msgs::Empty &, T &, bool &)> f =
          [_cb, _obj](const msgs::Empty &/*_internalReq*/, T &_internalRep,
                      bool &_internalResult)
        {
          auto cb = std::bind(_cb, _obj, std::placeholders::_1,
            std::placeholders::_2);
          cb(_internalRep, _internalResult);
        };

        return this->Advertise<msgs::Empty, T>(_topic, f, _options);
      }

      public: template<typename C, typename T> bool Advertise(
        const std::string &_topic,
        void(C::*_cb)(const T &_req),
        C *_obj,
        const AdvertiseServiceOptions &_options = AdvertiseServiceOptions())
      {
        std::function<void(const T &, ignition::msgs::Empty &, bool &)> f =
          [_cb, _obj](const T &_internalReq,
             ignition::msgs::Empty &/*_internalRep*/,
             bool &/*_internalResult*/)
        {
          auto cb = std::bind(_cb, _obj, std::placeholders::_1);
          cb(_internalReq);
        };

        return this->Advertise<T, ignition::msgs::Empty>(_topic, f, _options);
      }

      public: std::vector<std::string> AdvertisedServices() const;

      public: template<typename T1, typename T2> bool Request(
        const std::string &_topic,
        const T1 &_req,
        void(*_cb)(const T2 &_rep, const bool _result))
      {
        std::function<void(const T2 &, const bool)> f =
          [_cb](const T2 &_internalRep, const bool _internalResult)
        {
          (*_cb)(_internalRep, _internalResult);
        };

        return this->Request<T1, T2>(_topic, _req, f);
      }

      public: template<typename T> bool Request(
        const std::string &_topic,
        void(*_cb)(const T &_rep, const bool _result))
      {
        msgs::Empty req;
        return this->Request(_topic, req, _cb);
      }

      public: template<typename T1, typename T2> bool Request(
        const std::string &_topic,
        const T1 &_req,
        std::function<void(const T2 &_rep, const bool _result)> &_cb)
      {
        std::string fullyQualifiedTopic;
        if (!TopicUtils::FullyQualifiedName(this->Options().Partition(),
          this->Options().NameSpace(), _topic, fullyQualifiedTopic))
        {
          std::cerr << "Service [" << _topic << "] is not valid." << std::endl;
          return false;
        }

        bool localResponserFound;
        IRepHandlerPtr repHandler;
        {
          std::lock_guard<std::recursive_mutex> lk(this->Shared()->mutex);
          localResponserFound = this->Shared()->repliers.FirstHandler(
            fullyQualifiedTopic, T1().GetTypeName(), T2().GetTypeName(),
              repHandler);
        }

        // If the responser is within my process.
        if (localResponserFound)
        {
          // There is a responser in my process, let's use it.
          T2 rep;
          bool result;
          repHandler->RunLocalCallback(_req, rep, result);

          _cb(rep, result);
          return true;
        }

        // Create a new request handler.
        std::shared_ptr<ReqHandler<T1, T2>> reqHandlerPtr(
          new ReqHandler<T1, T2>(this->NodeUuid()));

        // Insert the request's parameters.
        reqHandlerPtr->SetMessage(&_req);

        // Insert the callback into the handler.
        reqHandlerPtr->SetCallback(_cb);

        {
          std::lock_guard<std::recursive_mutex> lk(this->Shared()->mutex);

          // Store the request handler.
          this->Shared()->requests.AddHandler(
            fullyQualifiedTopic, this->NodeUuid(), reqHandlerPtr);

          // If the responser's address is known, make the request.
          SrvAddresses_M addresses;
          if (this->Shared()->srvDiscovery->Publishers(
            fullyQualifiedTopic, addresses))
          {
            this->Shared()->SendPendingRemoteReqs(fullyQualifiedTopic,
              T1().GetTypeName(), T2().GetTypeName());
          }
          else
          {
            // Discover the service responser.
            if (!this->Shared()->srvDiscovery->Discover(fullyQualifiedTopic))
            {
              std::cerr << "Node::Request(): Error discovering a service. "
                        << "Did you forget to start the discovery service?"
                        << std::endl;
              return false;
            }
          }
        }

        return true;
      }

      public: template<typename T> bool Request(
        const std::string &_topic,
        std::function<void(const T &_rep, const bool _result)> &_cb)
      {
        msgs::Empty req;
        return this->Request(_topic, req, _cb);
      }

      public: template<typename C, typename T1, typename T2> bool Request(
        const std::string &_topic,
        const T1 &_req,
        void(C::*_cb)(const T2 &_rep, const bool _result),
        C *_obj)
      {
        std::function<void(const T2 &, const bool)> f =
          [_cb, _obj](const T2 &_internalRep, const bool _internalResult)
        {
          auto cb = std::bind(_cb, _obj, std::placeholders::_1,
            std::placeholders::_2);
          cb(_internalRep, _internalResult);
        };

        return this->Request<T1, T2>(_topic, _req, f);
      }

      public: template<typename C, typename T> bool Request(
        const std::string &_topic,
        void(C::*_cb)(const T &_rep, const bool _result),
        C *_obj)
      {
        msgs::Empty req;
        return this->Request(_topic, req, _cb, _obj);
      }

      public: template<typename T1, typename T2> bool Request(
        const std::string &_topic,
        const T1 &_req,
        const unsigned int &_timeout,
        T2 &_rep,
        bool &_result)
      {
        std::string fullyQualifiedTopic;
        if (!TopicUtils::FullyQualifiedName(this->Options().Partition(),
          this->Options().NameSpace(), _topic, fullyQualifiedTopic))
        {
          std::cerr << "Service [" << _topic << "] is not valid." << std::endl;
          return false;
        }

        // Create a new request handler.
        std::shared_ptr<ReqHandler<T1, T2>> reqHandlerPtr(
          new ReqHandler<T1, T2>(this->NodeUuid()));

        // Insert the request's parameters.
        reqHandlerPtr->SetMessage(&_req);
        reqHandlerPtr->SetResponse(&_rep);

        std::unique_lock<std::recursive_mutex> lk(this->Shared()->mutex);

        // If the responser is within my process.
        IRepHandlerPtr repHandler;
        if (this->Shared()->repliers.FirstHandler(fullyQualifiedTopic,
          _req.GetTypeName(), _rep.GetTypeName(), repHandler))
        {
          // There is a responser in my process, let's use it.
          repHandler->RunLocalCallback(_req, _rep, _result);
          return true;
        }

        // Store the request handler.
        this->Shared()->requests.AddHandler(
          fullyQualifiedTopic, this->NodeUuid(), reqHandlerPtr);

        // If the responser's address is known, make the request.
        SrvAddresses_M addresses;
        if (this->Shared()->srvDiscovery->Publishers(
          fullyQualifiedTopic, addresses))
        {
          this->Shared()->SendPendingRemoteReqs(fullyQualifiedTopic,
            _req.GetTypeName(), _rep.GetTypeName());
        }
        else
        {
          // Discover the service responser.
          if (!this->Shared()->srvDiscovery->Discover(fullyQualifiedTopic))
          {
            std::cerr << "Node::Request(): Error discovering a service. "
                      << "Did you forget to start the discovery service?"
                      << std::endl;
            return false;
          }
        }

        // Wait until the REP is available.
        bool executed = reqHandlerPtr->WaitUntil(lk, _timeout);

        // The request was not executed.
        if (!executed)
          return false;

        // The request was executed but did not succeed.
        if (!reqHandlerPtr->Result())
        {
          _result = false;
          return true;
        }

        // Parse the response.
        if (!_rep.ParseFromString(reqHandlerPtr->Response()))
        {
          std::cerr << "Node::Request(): Error Parsing the response"
                    << std::endl;
          _result = false;
          return true;
        }

        _result = true;
        return true;
      }

      public: template<typename T> bool Request(
        const std::string &_topic,
        const unsigned int &_timeout,
        T &_rep,
        bool &_result)
      {
        msgs::Empty req;
        return this->Request(_topic, req, _timeout, _rep, _result);
      }

      public: template<typename T> bool Request(const std::string &_topic,
                                                const T &_req)
        {
          // This callback is here for reusing the regular Request() call with
          // input and output parameters.
          std::function<void(const ignition::msgs::Empty &, const bool)> f =
            [](const ignition::msgs::Empty &, const bool)
          {
          };

          return this->Request<T, ignition::msgs::Empty>(_topic, _req, f);
        }

      public: bool UnadvertiseSrv(const std::string &_topic);

      public: void TopicList(std::vector<std::string> &_topics) const;

      public: bool TopicInfo(const std::string &_topic,
                             std::vector<MessagePublisher> &_publishers) const;

      public: void ServiceList(std::vector<std::string> &_services) const;

      public: bool ServiceInfo(const std::string &_service,
                              std::vector<ServicePublisher> &_publishers) const;

      private: const std::string &Partition() const;

      private: const std::string &NameSpace() const;

      private: NodeShared *Shared() const;

      private: const std::string &NodeUuid() const;

      private: std::unordered_set<std::string> &TopicsSubscribed() const;

      private: std::unordered_set<std::string> &SrvsAdvertised() const;

      private: NodeOptions &Options() const;

      private: std::unique_ptr<transport::NodePrivate> dataPtr;
    };
  }
}
#endif