Node.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012 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 GAZEBO_TRANSPORT_NODE_HH_
#define GAZEBO_TRANSPORT_NODE_HH_
 
#undef emit
#include <tbb/task.h>
#define emit
#ifndef TBB_VERSION_MAJOR
#include <tbb/version.h>
#endif
 
// This fixes compiler warnings, see #3147 and #3160
#ifndef BOOST_BIND_GLOBAL_PLACEHOLDERS
#define BOOST_BIND_GLOBAL_PLACEHOLDERS
#endif
 
#include <boost/bind.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <map>
#include <list>
#include <string>
#include <vector>
#if TBB_VERSION_MAJOR >= 2021
#include "gazebo/transport/TaskGroup.hh"
#endif
#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/transport/TopicManager.hh"
#include "gazebo/util/system.hh"
 
namespace gazebo
{
  namespace transport
  {
#if TBB_VERSION_MAJOR < 2021
    class GZ_TRANSPORT_VISIBLE PublishTask : public tbb::task
#else
    class GZ_TRANSPORT_VISIBLE PublishTask
#endif
    {
      public: PublishTask(transport::PublisherPtr _pub,
                  const google::protobuf::Message &_message)
              : pub(_pub)
      {
        this->msg = _message.New();
        this->msg->CopyFrom(_message);
      }
 
#if TBB_VERSION_MAJOR < 2021
      public: tbb::task *execute()
#else
      public: void operator()() const
#endif
              {
                this->pub->WaitForConnection();
                this->pub->Publish(*this->msg, true);
                this->pub->SendMessage();
                delete this->msg;
#if TBB_VERSION_MAJOR < 2021
                this->pub.reset();
                return NULL;
#endif
              }
 
      private: transport::PublisherPtr pub;
 
      private: google::protobuf::Message *msg;
    };
 
 
    class GZ_TRANSPORT_VISIBLE Node :
      public boost::enable_shared_from_this<Node>
    {
      public: Node();
 
      public: virtual ~Node();
 
      public: void Init(const std::string &_space ="");
 
      public: bool TryInit(
        const common::Time &_maxWait = common::Time(1, 0));
 
      public: bool IsInitialized() const;
 
      public: void Fini();
 
      public: std::string GetTopicNamespace() const;
 
      public: std::string DecodeTopicName(const std::string &_topic);
 
      public: std::string EncodeTopicName(const std::string &_topic);
 
      public: unsigned int GetId() const;
 
      public: void ProcessPublishers();
 
      public: void ProcessIncoming();
 
      public: bool HasLatchedSubscriber(const std::string &_topic) const;
 
 
      public: template<typename M>
              void Publish(const std::string &_topic,
                  const google::protobuf::Message &_message)
              {
                transport::PublisherPtr pub = this->Advertise<M>(_topic);
#if TBB_VERSION_MAJOR < 2021
                PublishTask *task = new(tbb::task::allocate_root())
                  PublishTask(pub, _message);
 
                tbb::task::enqueue(*task);
                return;
#else
                this->taskGroup.run<PublishTask>(pub, _message);
#endif
              }
 
      public: template<typename M>
      transport::PublisherPtr Advertise(const std::string &_topic,
                                        unsigned int _queueLimit = 1000,
                                        double _hzRate = 0)
      {
        std::string decodedTopic = this->DecodeTopicName(_topic);
        PublisherPtr publisher =
          transport::TopicManager::Instance()->Advertise<M>(
              decodedTopic, _queueLimit, _hzRate);
 
        boost::mutex::scoped_lock lock(this->publisherMutex);
        publisher->SetNode(shared_from_this());
        this->publishers.push_back(publisher);
 
        return publisher;
      }
 
      public: void Publish(const std::string &_topic,
                  const google::protobuf::Message &_message)
              {
                transport::PublisherPtr pub = this->Advertise(_topic,
                    _message.GetTypeName());
                pub->WaitForConnection();
 
                pub->Publish(_message, true);
              }
 
      public: transport::PublisherPtr Advertise(const std::string &_topic,
                                        const std::string &_msgTypeName,
                                        unsigned int _queueLimit = 1000,
                                        double _hzRate = 0)
      {
        std::string decodedTopic = this->DecodeTopicName(_topic);
        PublisherPtr publisher =
          transport::TopicManager::Instance()->Advertise(
              decodedTopic, _msgTypeName, _queueLimit, _hzRate);
 
        boost::mutex::scoped_lock lock(this->publisherMutex);
        publisher->SetNode(shared_from_this());
        this->publishers.push_back(publisher);
 
        return publisher;
      }
 
      public: template<typename M, typename T>
      SubscriberPtr Subscribe(const std::string &_topic,
          void(T::*_fp)(const boost::shared_ptr<M const> &), T *_obj,
          bool _latching = false)
      {
        SubscribeOptions ops;
        std::string decodedTopic = this->DecodeTopicName(_topic);
        ops.template Init<M>(decodedTopic, shared_from_this(), _latching);
 
        {
          using namespace boost::placeholders;
          boost::recursive_mutex::scoped_lock lock(this->incomingMutex);
          this->callbacks[decodedTopic].push_back(CallbackHelperPtr(
                new CallbackHelperT<M>(boost::bind(_fp, _obj, _1), _latching)));
        }
 
        SubscriberPtr result =
          transport::TopicManager::Instance()->Subscribe(ops);
 
        result->SetCallbackId(this->callbacks[decodedTopic].back()->GetId());
 
        return result;
      }
 
      public: template<typename M>
      SubscriberPtr Subscribe(const std::string &_topic,
          void(*_fp)(const boost::shared_ptr<M const> &),
                     bool _latching = false)
      {
        SubscribeOptions ops;
        std::string decodedTopic = this->DecodeTopicName(_topic);
        ops.template Init<M>(decodedTopic, shared_from_this(), _latching);
 
        {
          boost::recursive_mutex::scoped_lock lock(this->incomingMutex);
          this->callbacks[decodedTopic].push_back(
              CallbackHelperPtr(new CallbackHelperT<M>(_fp, _latching)));
        }
 
        SubscriberPtr result =
          transport::TopicManager::Instance()->Subscribe(ops);
 
        result->SetCallbackId(this->callbacks[decodedTopic].back()->GetId());
 
        return result;
      }
 
      template<typename T>
      SubscriberPtr Subscribe(const std::string &_topic,
          void(T::*_fp)(const std::string &), T *_obj,
          bool _latching = false)
      {
        SubscribeOptions ops;
        std::string decodedTopic = this->DecodeTopicName(_topic);
        ops.Init(decodedTopic, shared_from_this(), _latching);
 
        {
          using namespace boost::placeholders;
          boost::recursive_mutex::scoped_lock lock(this->incomingMutex);
          this->callbacks[decodedTopic].push_back(CallbackHelperPtr(
                new RawCallbackHelper(boost::bind(_fp, _obj, _1))));
        }
 
        SubscriberPtr result =
          transport::TopicManager::Instance()->Subscribe(ops);
 
        result->SetCallbackId(this->callbacks[decodedTopic].back()->GetId());
 
        return result;
      }
 
 
      SubscriberPtr Subscribe(const std::string &_topic,
          void(*_fp)(const std::string &), bool _latching = false)
      {
        SubscribeOptions ops;
        std::string decodedTopic = this->DecodeTopicName(_topic);
        ops.Init(decodedTopic, shared_from_this(), _latching);
 
        {
          boost::recursive_mutex::scoped_lock lock(this->incomingMutex);
          this->callbacks[decodedTopic].push_back(
              CallbackHelperPtr(new RawCallbackHelper(_fp)));
        }
 
        SubscriberPtr result =
          transport::TopicManager::Instance()->Subscribe(ops);
 
        result->SetCallbackId(this->callbacks[decodedTopic].back()->GetId());
 
        return result;
      }
 
      public: bool HandleData(const std::string &_topic,
                              const std::string &_msg);
 
      public: bool HandleMessage(const std::string &_topic, MessagePtr _msg);
 
      public: void InsertLatchedMsg(const std::string &_topic,
                                    const std::string &_msg);
 
      public: void InsertLatchedMsg(const std::string &_topic,
                                    MessagePtr _msg);
 
      public: std::string GetMsgType(const std::string &_topic) const;
 
      public: void RemoveCallback(const std::string &_topic, unsigned int _id);
 
      private: bool PrivateInit(const std::string &_space,
                                const common::Time &_maxWait,
                                const bool _fallbackToDefault);
 
      private: std::string topicNamespace;
      private: std::vector<PublisherPtr> publishers;
      private: std::vector<PublisherPtr>::iterator publishersIter;
      private: static unsigned int idCounter;
      private: unsigned int id;
 
      private: typedef std::list<CallbackHelperPtr> Callback_L;
      private: typedef std::map<std::string, Callback_L> Callback_M;
      private: Callback_M callbacks;
      private: std::map<std::string, std::list<std::string> > incomingMsgs;
 
      private: std::map<std::string, std::list<MessagePtr> > incomingMsgsLocal;
 
#if TBB_VERSION_MAJOR >= 2021
      private: TaskGroup taskGroup;
#endif
 
      private: boost::mutex publisherMutex;
      private: boost::mutex publisherDeleteMutex;
      private: boost::recursive_mutex incomingMutex;
 
      private: boost::recursive_mutex processIncomingMutex;
 
      private: bool initialized;
    };
  }
}
#endif