Program Listing for File live_sync.hpp

↰ Return to documentation for file (include/flex_sync/live_sync.hpp)

// -*-c++-*---------------------------------------------------------------------------------------
// Copyright 2024 Bernd Pfrommer <bernd.pfrommer@gmail.com>
//
// 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 FLEX_SYNC__LIVE_SYNC_HPP_
#define FLEX_SYNC__LIVE_SYNC_HPP_

#include <flex_sync/msg_pack.hpp>
#include <rclcpp/rclcpp.hpp>

namespace flex_sync
{
template <typename SyncT, typename T>
class Subscriber
{
public:
  using TConstSharedPtr = typename T::ConstSharedPtr;
  Subscriber(
    const std::string & topic, rclcpp::Node * node, const rclcpp::QoS & qos,
    const std::shared_ptr<SyncT> & sync)
  : topic_(topic), sync_(sync)
  {
    sub_ = node->create_subscription<T>(
      topic, qos,
      std::bind(&Subscriber::callback, this, std::placeholders::_1));
  }
  void callback(TConstSharedPtr msg) { sync_->process(topic_, msg); }

private:
  std::string topic_;
  std::shared_ptr<SyncT> sync_;
  typename rclcpp::Subscription<T>::SharedPtr sub_;
};

/* ------------------- HERE STARTS THE MAIN CLASS ------- */

template <
  typename SyncT,
  template <typename, typename> typename SubscriberT = Subscriber,
  typename = typename SyncT::message_types>
class LiveSync;

template <
  typename SyncT, template <typename, typename> typename SubscriberT,
  typename... MsgTypes>
class LiveSync<SyncT, SubscriberT, MsgPack<MsgTypes...>>
{
public:
  using string = std::string;
  using TupleOfTopicVec =
    std::tuple<std::vector<std::shared_ptr<SubscriberT<SyncT, MsgTypes>>>...>;
  typedef typename SyncT::Callback Callback;

  LiveSync(
    rclcpp::Node * node, const std::vector<std::vector<string>> & topics,
    const Callback & callback, const rclcpp::QoS & qos)
  : node_(node)
  {
    sync_.reset(new SyncT(topics, callback, std::max(size_t(5), qos.depth())));
    // initialize topics
    TopicInitializer ti;
    (void)for_each(topics_, &ti);
  }

  std::shared_ptr<SyncT> getSync() { return (sync_); }

  rclcpp::Node * getNode() { return (node_); }

private:
  struct TopicInitializer
  {
    template <std::size_t I>
    int operate(LiveSync<SyncT, SubscriberT> * liveSync) const
    {
      std::shared_ptr<SyncT> sync = liveSync->getSync();
      auto & topics = sync->getTopics();
      auto & topic_vec = std::get<I>(liveSync->topics_);
      // std::cout << "creating topic for " << I
      //<< " num: " << topics[I].size() << std::endl;
      auto node = liveSync->getNode();
      const unsigned int qs = sync->getQueueSize();
      for (const std::string & topic : topics[I]) {
        // get vector type-> pointer type -> pointer element type
        typedef
          typename get_type<I, TupleOfTopicVec>::type::value_type::element_type
            SubscriberET;
        std::shared_ptr<SubscriberET> lt(
          new SubscriberET(topic, node, qs, sync));
        topic_vec.push_back(lt);
      }
      return (topic_vec.size());
    }
  };

  // some template tricks picked up here:
  // https://stackoverflow.com/questions/18063451/get-index-of-a-tuple-elements -type
  // This template terminates the recursion

  template <std::size_t I = 0, typename FuncT, typename... Tp>
  inline typename std::enable_if<I == sizeof...(Tp), int>::type for_each(
    std::tuple<Tp...> &, FuncT *)  // Unused arg needs no name
  {
    return 0;
  }  // do nothing

  // This template recursively calls itself, thereby iterating
  template <std::size_t I = 0, typename FuncT, typename... Tp>
    inline typename std::enable_if <
    I<sizeof...(Tp), int>::type for_each(std::tuple<Tp...> & t, FuncT * f)
  {
    const int rv = (*f).template operate<I>(this);
    return (rv + for_each<I + 1, FuncT, Tp...>(t, f));
  }

  // The following templates return the N'th type of a tuple.
  // source:
  // https://stackoverflow.com/questions/16928669/how-to-get-n-th-type-from-a-tuple
  template <int N, typename... Ts>
  struct get_type;
  template <int N, typename T, typename... Ts>
  struct get_type<N, std::tuple<T, Ts...>>
  {
    using type = typename get_type<N - 1, std::tuple<Ts...>>::type;
  };
  template <typename T, typename... Ts>
  struct get_type<0, std::tuple<T, Ts...>>
  {
    using type = T;
  };
  // -------------- variables -------------
  std::shared_ptr<SyncT> sync_;
  TupleOfTopicVec topics_;
  rclcpp::Node * node_{nullptr};
};
}  // namespace flex_sync

#endif  // FLEX_SYNC__LIVE_SYNC_HPP_