SubscriptionHandler.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 GZ_TRANSPORT_SUBSCRIPTIONHANDLER_HH_
#define GZ_TRANSPORT_SUBSCRIPTIONHANDLER_HH_
 
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable: 4251)
#endif
#include <google/protobuf/message.h>
#include <google/protobuf/stubs/common.h>
#if GOOGLE_PROTOBUF_VERSION >= 3000000 && GOOGLE_PROTOBUF_VERSION < 4022000
#include <google/protobuf/stubs/casts.h>
#endif
#ifdef _MSC_VER
#pragma warning(pop)
#endif
 
#include <chrono>
#include <cstddef>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#include <utility>
 
#include <gz/msgs/Factory.hh>
 
#include "gz/transport/config.hh"
#include "gz/transport/Export.hh"
#include "gz/transport/MessageInfo.hh"
#include "gz/transport/SubscribeOptions.hh"
#include "gz/transport/TopicUtils.hh"
#include "gz/transport/TransportTypes.hh"
#include "gz/transport/Uuid.hh"
 
namespace zenoh
{
  // Forward declaration.
  class Session;
}
 
namespace gz::transport
{
  // Inline bracket to help doxygen filtering.
  inline namespace GZ_TRANSPORT_VERSION_NAMESPACE {
  //
    class SubscriptionHandlerBasePrivate;
 
  class GZ_TRANSPORT_VISIBLE SubscriptionHandlerBase
  {
    public: explicit SubscriptionHandlerBase(
      const std::string &_pUuid,
      const std::string &_nUuid,
      const SubscribeOptions &_opts = SubscribeOptions());
 
    public: virtual ~SubscriptionHandlerBase();
 
    public: virtual std::string TypeName() = 0;
 
    public: std::string ProcUuid() const;
 
    public: std::string NodeUuid() const;
 
    public: std::string HandlerUuid() const;
 
    public: bool IgnoreLocalMessages() const;
 
    protected: bool UpdateThrottling();
 
    protected: SubscribeOptions opts;
 
    protected: double periodNs;
 
#ifdef _WIN32
// Disable warning C4251 which is triggered by
// std::*
#pragma warning(push)
#pragma warning(disable: 4251)
#endif
    protected: std::unique_ptr<SubscriptionHandlerBasePrivate> dataPtr;
#ifdef _WIN32
#pragma warning(pop)
#endif
  };
 
  class GZ_TRANSPORT_VISIBLE ISubscriptionHandler
      : public SubscriptionHandlerBase
  {
    public: explicit ISubscriptionHandler(
      const std::string &_pUuid,
      const std::string &_nUuid,
      const SubscribeOptions &_opts = SubscribeOptions());
 
    public: virtual ~ISubscriptionHandler() = default;
 
    public: virtual bool RunLocalCallback(
      const ProtoMsg &_msg,
      const MessageInfo &_info) = 0;
 
    public: virtual const std::shared_ptr<ProtoMsg> CreateMsg(
      const std::string &_data,
      const std::string &_type) const = 0;
 
    public: using MsgFactory = std::function<std::shared_ptr<ProtoMsg>(
      const char *_data,
      std::size_t _size,
      const std::string &_type)>;
 
    public: const std::shared_ptr<ProtoMsg> CreateMsgFromBuffer(
      const char *_data,
      std::size_t _size,
      const std::string &_type) const;
 
    protected: void SetMsgFactory(MsgFactory _factory);
 
#ifdef HAVE_ZENOH
    public: void CreateLivelinessToken(
      std::shared_ptr<zenoh::Session> _session,
      const FullyQualifiedTopic &_fullyQualifiedTopic);
 
    public: void CreateGenericZenohSubscriber(
      std::shared_ptr<zenoh::Session> _session,
      const FullyQualifiedTopic &_fullyQualifiedTopic);
#endif
  };
 
  template <typename T> class SubscriptionHandler
    : public ISubscriptionHandler
  {
    // Documentation inherited.
    public: explicit SubscriptionHandler(const std::string &_pUuid,
      const std::string &_nUuid,
      const SubscribeOptions &_opts = SubscribeOptions())
      : ISubscriptionHandler(_pUuid, _nUuid, _opts)
    {
      // Parse straight from the buffer (no intermediate string copy).
      this->SetMsgFactory(
        [](const char *_data, std::size_t _size,
           const std::string &/*_type*/) -> std::shared_ptr<ProtoMsg>
        {
          auto msgPtr = std::make_shared<T>();
          if (!msgPtr->ParseFromArray(_data, static_cast<int>(_size)))
          {
            std::cerr << "SubscriptionHandler::CreateMsgFromBuffer() error: "
                      << "ParseFromArray failed" << std::endl;
          }
          return msgPtr;
        });
    }
 
    // Documentation inherited.
    public: const std::shared_ptr<ProtoMsg> CreateMsg(
      const std::string &_data,
      const std::string &_type) const
    {
      // Safe to delegate: the constructor registered a factory.
      return this->CreateMsgFromBuffer(_data.data(), _data.size(), _type);
    }
 
    // Documentation inherited.
    public: std::string TypeName()
    {
      return std::string(T().GetTypeName());
    }
 
    public: void SetCallback(const MsgCallback<T> &_cb)
    {
      this->cb = _cb;
    }
 
#ifdef HAVE_ZENOH
    public: void SetCallback(const MsgCallback<T> &_cb,
                             std::shared_ptr<zenoh::Session> _session,
                             const FullyQualifiedTopic &_fullyQualifiedTopic)
    {
      this->SetCallback(std::move(_cb));
      this->CreateLivelinessToken(_session, _fullyQualifiedTopic);
    }
#endif
 
    // Documentation inherited.
    public: bool RunLocalCallback(const ProtoMsg &_msg,
                                  const MessageInfo &_info)
    {
      // No callback stored.
      if (!this->cb)
      {
        std::cerr << "SubscriptionHandler::RunLocalCallback() error: "
                  << "Callback is NULL" << std::endl;
        return false;
      }
 
      // Check the subscription throttling option.
      if (!this->UpdateThrottling())
        return true;
 
#if GOOGLE_PROTOBUF_VERSION >= 5028000
      auto msgPtr = google::protobuf::DynamicCastMessage<T>(&_msg);
#elif GOOGLE_PROTOBUF_VERSION >= 4022000
      auto msgPtr = google::protobuf::internal::DownCast<const T*>(&_msg);
#elif GOOGLE_PROTOBUF_VERSION >= 3000000
      auto msgPtr = google::protobuf::down_cast<const T*>(&_msg);
#else
      auto msgPtr = google::protobuf::internal::down_cast<const T*>(&_msg);
#endif
 
      // Verify the dynamically casted message is valid
      if (msgPtr == nullptr)
      {
        if (_msg.GetDescriptor() != nullptr)
        {
          std::cerr << "SubscriptionHandler::RunLocalCallback() error: "
                    << "Failed to cast the message of the type "
                    << _msg.GetDescriptor()->full_name()
                    << " to the specified type" << '\n';
        }
        else
        {
          std::cerr << "SubscriptionHandler::RunLocalCallback() error: "
                    << "Failed to cast the message of an unknown type"
                    << " to the specified type" << '\n';
        }
        std::cerr.flush();
        return false;
      }
 
      this->cb(*msgPtr, _info);
      return true;
    }
 
    private: MsgCallback<T> cb;
  };
 
  template <> class SubscriptionHandler<ProtoMsg>
    : public ISubscriptionHandler
  {
    // Documentation inherited.
    public: explicit SubscriptionHandler(const std::string &_pUuid,
      const std::string &_nUuid,
      const SubscribeOptions &_opts = SubscribeOptions())
      : ISubscriptionHandler(_pUuid, _nUuid, _opts)
    {
      // Look the type up at runtime and parse straight from the buffer
      // (no intermediate string copy).
      this->SetMsgFactory(
        [](const char *_data, std::size_t _size,
           const std::string &_type) -> std::shared_ptr<ProtoMsg>
        {
          std::shared_ptr<google::protobuf::Message> msgPtr;
 
          const google::protobuf::Descriptor *desc =
            google::protobuf::DescriptorPool::generated_pool()
              ->FindMessageTypeByName(_type);
 
          // First, check if we have the descriptor from the generated
          // proto classes.
          if (desc)
          {
            msgPtr.reset(
              google::protobuf::MessageFactory::generated_factory()
                ->GetPrototype(desc)->New());
          }
          else
          {
            // Fallback on Gazebo Msgs if the message type is not found.
            msgPtr = gz::msgs::Factory::New(_type);
          }
 
          if (!msgPtr)
            return nullptr;
 
          if (!msgPtr->ParseFromArray(_data, static_cast<int>(_size)))
          {
            std::cerr << "CreateMsgFromBuffer() error: ParseFromArray failed"
                      << std::endl;
            return nullptr;
          }
 
          return msgPtr;
        });
    }
 
    // Documentation inherited.
    public: const std::shared_ptr<ProtoMsg> CreateMsg(
      const std::string &_data,
      const std::string &_type) const
    {
      // Safe to delegate: the constructor registered a factory.
      return this->CreateMsgFromBuffer(_data.data(), _data.size(), _type);
    }
 
    // Documentation inherited.
    public: std::string TypeName()
    {
      return kGenericMessageType;
    }
 
    public: void SetCallback(const MsgCallback<ProtoMsg> &_cb)
    {
      this->cb = _cb;
    }
 
#ifdef HAVE_ZENOH
    public: void SetCallback(const MsgCallback<ProtoMsg> &_cb,
                             std::shared_ptr<zenoh::Session> _session,
                             const FullyQualifiedTopic &_fullyQualifiedTopic)
    {
      this->SetCallback(std::move(_cb));
      this->CreateLivelinessToken(_session, _fullyQualifiedTopic);
    }
#endif
 
    // Documentation inherited.
    public: bool RunLocalCallback(const ProtoMsg &_msg,
                                  const MessageInfo &_info)
    {
      // No callback stored.
      if (!this->cb)
      {
        std::cerr << "SubscriptionHandler::RunLocalCallback() "
                  << "error: Callback is NULL" << std::endl;
        return false;
      }
 
      // Check the subscription throttling option.
      if (!this->UpdateThrottling())
        return true;
 
      this->cb(_msg, _info);
      return true;
    }
 
    private: MsgCallback<ProtoMsg> cb;
  };
 
  class RawSubscriptionHandler : public SubscriptionHandlerBase
  {
    public: explicit RawSubscriptionHandler(
      const std::string &_pUuid,
      const std::string &_nUuid,
      const std::string &_msgType = kGenericMessageType,
      const SubscribeOptions &_opts = SubscribeOptions());
 
    // Documentation inherited
    public: std::string TypeName() override;
 
    public: void SetCallback(const RawCallback &_callback);
 
#ifdef HAVE_ZENOH
    public: void SetCallback(const RawCallback &_cb,
                             std::shared_ptr<zenoh::Session> _session,
                             const FullyQualifiedTopic &_fullyQualifiedTopic);
 
    public: void CreateLivelinessToken(
      std::shared_ptr<zenoh::Session> _session,
      const FullyQualifiedTopic &_fullyQualifiedTopic);
#endif
 
    public: bool RunRawCallback(const char *_msgData, const size_t _size,
                                const MessageInfo &_info);
 
    public: ~RawSubscriptionHandler();
 
    private: class Implementation;
 
#ifdef _WIN32
// Disable warning C4251 which is triggered by
// std::unique_ptr
#pragma warning(push)
#pragma warning(disable: 4251)
#endif
    private: std::unique_ptr<Implementation> pimpl;
#ifdef _WIN32
#pragma warning(pop)
#endif
  };
  }
}
 
#endif