Add a warning when ROS2 publish fails

include/cactus_rt/ros2/publisher.h

@@ -7,6 +7,8 @@
 #include <rclcpp/rclcpp.hpp>
 #include <type_traits>
 
+#include "quill/Quill.h"
+
 namespace cactus_rt::ros2 {
 class Ros2Adapter;
 
@@ -29,6 +31,7 @@ class Publisher : public IPublisher {
   using NoConversion = std::is_same<RealtimeT, RosT>;
   using AdaptedRosType = typename std::conditional_t<NoConversion::value, RosT, rclcpp::TypeAdapter<RealtimeT, RosT>>;
 
+  quill::Logger*                                        logger_;
   typename rclcpp::Publisher<AdaptedRosType>::SharedPtr publisher_;
   moodycamel::ReaderWriterQueue<RealtimeT>              queue_;
 
@@ -74,12 +77,14 @@ class Publisher : public IPublisher {
   }
 
   static std::shared_ptr<Publisher<RealtimeT, RosT, CheckForTrivialRealtimeT>> Create(
+    quill::Logger*     logger,
     rclcpp::Node&      node,
     const std::string& topic_name,
     const rclcpp::QoS& qos,
     const size_t       rt_queue_size = 1000
   ) {
     return std::make_shared<Publisher<RealtimeT, RosT, CheckForTrivialRealtimeT>>(
+      logger,
       node.create_publisher<AdaptedRosType>(topic_name, qos),
       moodycamel::ReaderWriterQueue<RealtimeT>(rt_queue_size)
     );
@@ -92,14 +97,20 @@ class Publisher : public IPublisher {
    * @private
    */
   Publisher(
+    quill::Logger*                                        logger,
     typename rclcpp::Publisher<AdaptedRosType>::SharedPtr publisher,
     moodycamel::ReaderWriterQueue<RealtimeT>&&            queue
-  ) : publisher_(publisher), queue_(std::move(queue)) {}
+  ) : logger_(logger), publisher_(publisher), queue_(std::move(queue)) {}
 
   template <typename... Args>
   bool Publish(Args&&... args) noexcept {
     const bool success = queue_.try_emplace(std::forward<Args>(args)...);
     // TODO: Keep track of success/failed messages and expose that to be queried
+
+    if (!success) {
+      LOG_WARNING_LIMIT(std::chrono::seconds(5), logger_, "failed to publish to {} due to full queue", publisher_->get_topic_name());
+    }
+
     return success;
   }
 };

include/cactus_rt/ros2/ros2_adapter.h

@@ -11,6 +11,7 @@
 #include <vector>
 
 #include "publisher.h"
+#include "quill/Quill.h"
 #include "subscription.h"
 
 namespace cactus_rt::ros2 {
@@ -43,6 +44,8 @@ class Ros2Adapter {
   std::vector<std::shared_ptr<IPublisher>>    publishers_;
   std::vector<std::shared_ptr<ISubscription>> subscriptions_;
 
+  quill::Logger* logger_;
+
  public:
   Ros2Adapter(const std::string& name_, const Config& config);
 
@@ -56,7 +59,7 @@ class Ros2Adapter {
     const rclcpp::QoS& qos,
     size_t             rt_queue_size = 1000
   ) {
-    auto publisher = Publisher<RealtimeT, RosT, CheckForTrivialRealtimeT>::Create(*ros_node_, topic_name, qos, rt_queue_size);
+    auto publisher = Publisher<RealtimeT, RosT, CheckForTrivialRealtimeT>::Create(logger_, *ros_node_, topic_name, qos, rt_queue_size);
 
     const std::scoped_lock lock(mut_);
     publishers_.push_back(publisher);
@@ -68,7 +71,7 @@ class Ros2Adapter {
     const std::string& topic_name,
     const rclcpp::QoS& qos
   ) {
-    auto subscriber = SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>::Create(*this->ros_node_, topic_name, qos);
+    auto subscriber = SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>::Create(logger_, *this->ros_node_, topic_name, qos);
 
     const std::scoped_lock lock(mut_);
     subscriptions_.push_back(subscriber);

include/cactus_rt/ros2/subscription.h

@@ -6,6 +6,7 @@
 #include <type_traits>
 
 #include "../experimental/lockless/spsc/realtime_readable_value.h"
+#include "quill/Quill.h"
 
 // Note: ROS subscription dispatch is here: https://github.com/ros2/rclcpp/blob/e10728c/rclcpp/include/rclcpp/any_subscription_callback.hpp#L481
 // We are using the TypeAdapter method.
@@ -39,6 +40,7 @@ class SubscriptionLatest : public ISubscription {
 
   using RealtimeReadableValue = cactus_rt::experimental::lockless::spsc::RealtimeReadableValue<StampedValue<RealtimeT>>;
 
+  quill::Logger*                                           logger_;
   typename rclcpp::Subscription<AdaptedRosType>::SharedPtr ros_subscription_;
   int64_t                                                  current_msg_id_ = 0;
   RealtimeReadableValue                                    latest_value_;
@@ -55,11 +57,12 @@ class SubscriptionLatest : public ISubscription {
   }
 
   static std::shared_ptr<SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>> Create(
+    quill::Logger*     logger,
     rclcpp::Node&      node,
     const std::string& topic_name,
     const rclcpp::QoS& qos
   ) {
-    auto subscription = std::make_shared<SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>>();
+    auto subscription = std::make_shared<SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>>(logger);
 
     subscription->ros_subscription_ = node.create_subscription<AdaptedRosType>(
       topic_name,
@@ -79,7 +82,7 @@ class SubscriptionLatest : public ISubscription {
    *
    * @private
    */
-  SubscriptionLatest() = default;
+  explicit SubscriptionLatest(quill::Logger* logger) : logger_(logger) {}
 
   StampedValue<RealtimeT> ReadLatest() noexcept {
     return latest_value_.Read();

src/cactus_rt/ros2/ros2_adapter.cc

@@ -5,7 +5,8 @@
 namespace cactus_rt::ros2 {
 
 Ros2Adapter::Ros2Adapter(const std::string& name, const Ros2Adapter::Config& config)
-    : ros_node_(std::make_shared<rclcpp::Node>(name + "_ros_adapter")) {
+    : ros_node_(std::make_shared<rclcpp::Node>(name + "_ros_adapter")),
+      logger_(quill::create_logger("Ros2Adapter")) {
   timer_ = this->ros_node_->create_wall_timer(config.timer_interval, [this] { TimerCallback(); });
 }