subscription.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <px4_ros2/common/context.hpp>
#include <px4_ros2/common/exception.hpp>
#include <px4_ros2/components/shared_subscription.hpp>
 
using namespace std::chrono_literals;  // NOLINT
 
namespace px4_ros2 {
template <typename RosMessageType>
class Subscription {
  using UpdateCallback = std::function<void(const RosMessageType&)>;
 
 public:
  Subscription(Context& context, const std::string& topic) : _node(context.node())
  {
    const std::string namespaced_topic = context.topicNamespacePrefix() + topic;
    _callback_instance = SharedSubscription<RosMessageType>::create(
        _node, namespaced_topic, [this](const typename RosMessageType::UniquePtr& msg) {
          _last = *msg;
          _last_message_time = _node.get_clock()->now();
          for (const auto& callback : _callbacks) {
            callback(_last);
          }
        });
  }
 
  void onUpdate(const UpdateCallback& callback) { _callbacks.push_back(callback); }
 
  const RosMessageType& last() const
  {
    if (!hasReceivedMessages()) {
      throw Exception("No messages received.");
    }
    return _last;
  }
 
  const rclcpp::Time& lastTime() const { return _last_message_time; }
 
  template <typename DurationT = std::milli>
  bool lastValid(const std::chrono::duration<int64_t, DurationT> max_delay = 500ms) const
  {
    return hasReceivedMessages() && _node.get_clock()->now() - _last_message_time < max_delay;
  }
 
 protected:
  rclcpp::Node& _node;
 
 private:
  SharedSubscriptionCallbackInstance _callback_instance;
 
  RosMessageType _last;
  rclcpp::Time _last_message_time;
 
  std::vector<std::function<void(const RosMessageType&)>> _callbacks{};
 
  bool hasReceivedMessages() const { return _last_message_time.seconds() != 0; }
};
 
}  // namespace px4_ros2