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_

#include <tbb/task.h>
#include <boost/bind.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <map>
#include <list>
#include <string>
#include <vector>

#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/transport/TopicManager.hh"
#include "gazebo/util/system.hh"

namespace gazebo
{
  namespace transport
  {
    class GZ_TRANSPORT_VISIBLE PublishTask : public tbb::task
    {
      public: PublishTask(transport::PublisherPtr _pub,
                  const google::protobuf::Message &_message)
              : pub(_pub)
      {
        this->msg = _message.New();
        this->msg->CopyFrom(_message);
      }

      public: tbb::task *execute()
              {
                this->pub->WaitForConnection();
                this->pub->Publish(*this->msg, true);
                this->pub->SendMessage();
                delete this->msg;
                this->pub.reset();
                return NULL;
              }

      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);
                PublishTask *task = new(tbb::task::allocate_root())
                  PublishTask(pub, _message);

                tbb::task::enqueue(*task);
                return;
              }

      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);

        {
          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);

        {
          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;

      private: boost::mutex publisherMutex;
      private: boost::mutex publisherDeleteMutex;
      private: boost::recursive_mutex incomingMutex;

      private: boost::recursive_mutex processIncomingMutex;

      private: bool initialized;
    };
  }
}
#endif