feat: Added lazy subscribe support for GZ image bridge

ros_gz_image/src/image_bridge.cpp

@@ -14,6 +14,7 @@
 
 #include <rmw/qos_profiles.h>
 
+#include <chrono>
 #include <iostream>
 #include <memory>
 #include <string>
@@ -33,45 +34,101 @@ class Handler
   /// \param[in] _topic Image base topic
   /// \param[in] _node Pointer to ROS node
   /// \param[in] _gz_node Pointer to Gazebo node
+  /// \param[in] _lazy Whether to lazily subscribe to Gazebo only when ROS subscribers are present
   Handler(
     const std::string & _topic,
     std::shared_ptr<rclcpp::Node> _node,
-    std::shared_ptr<gz::transport::Node> _gz_node)
+    std::shared_ptr<gz::transport::Node> _gz_node,
+    bool _lazy = false)
+  : topic_(_topic),
+    node_(_node),
+    gz_node_(_gz_node),
+    is_lazy_(_lazy)
   {
     // Get QoS profile from parameter
-    rclcpp::QoS qos_profile = rclcpp::QoS(10);
+    qos_profile_ = rclcpp::QoS(10);
     const auto qos_str =
       _node->get_parameter("qos").get_parameter_value().get<std::string>();
     if (qos_str == "system_default") {
-      qos_profile = rclcpp::SystemDefaultsQoS();
+      qos_profile_ = rclcpp::SystemDefaultsQoS();
     } else if (qos_str == "sensor_data") {
-      qos_profile = rclcpp::SensorDataQoS();
+      qos_profile_ = rclcpp::SensorDataQoS();
     } else if (qos_str != "default") {
       RCLCPP_ERROR(
         _node->get_logger(),
         "Invalid QoS profile %s specified. Using default profile.",
         qos_str.c_str());
     }
 
-    // Create publishers and subscribers
-    this->ros_pub = image_transport::create_publisher(
-      *_node, _topic, qos_profile);
+    // Always create the ROS publisher
+    this->ros_pub_ = image_transport::create_publisher(
+      *_node, _topic, qos_profile_);
 
-    _gz_node->Subscribe(_topic, &Handler::OnImage, this);
+    // Subscribe to Gazebo immediately unless lazy
+    if (!is_lazy_) {
+      StartSubscriber();
+    }
+  }
+
+  /// \brief Manage Gazebo subscription lifecycle based on ROS subscriber count.
+  /// Only active when lazy mode is enabled.
+  void Spin()
+  {
+    if (!is_lazy_) {
+      return;
+    }
+
+    const size_t num_subs = ros_pub_.getNumSubscribers();
+
+    if (has_subscriber_ && num_subs == 0) {
+      StopSubscriber();
+    } else if (!has_subscriber_ && num_subs > 0) {
+      StartSubscriber();
+    }
   }
 
 private:
+  void StartSubscriber()
+  {
+    gz_node_->Subscribe(topic_, &Handler::OnImage, this);
+    has_subscriber_ = true;
+  }
+
+  void StopSubscriber()
+  {
+    gz_node_->Unsubscribe(topic_);
+    has_subscriber_ = false;
+  }
+
   /// \brief Callback when Gazebo image is received
   /// \param[in] _gz_msg Gazebo message
   void OnImage(const gz::msgs::Image & _gz_msg)
   {
     sensor_msgs::msg::Image ros_msg;
     ros_gz_bridge::convert_gz_to_ros(_gz_msg, ros_msg);
-    this->ros_pub.publish(ros_msg);
+    this->ros_pub_.publish(ros_msg);
   }
 
+  /// \brief Image topic name
+  std::string topic_;
+
+  /// \brief ROS node
+  std::shared_ptr<rclcpp::Node> node_;
+
+  /// \brief Gazebo transport node
+  std::shared_ptr<gz::transport::Node> gz_node_;
+
+  /// \brief Whether to use lazy subscription
+  bool is_lazy_{false};
+
+  /// \brief Whether currently subscribed to Gazebo topic
+  bool has_subscriber_{false};
+
+  /// \brief QoS profile for ROS publisher
+  rclcpp::QoS qos_profile_{10};
+
   /// \brief ROS image publisher
-  image_transport::Publisher ros_pub;
+  image_transport::Publisher ros_pub_;
 };
 
 //////////////////////////////////////////////////
@@ -96,6 +153,11 @@ int main(int argc, char * argv[])
   // ROS node
   auto node_ = rclcpp::Node::make_shared("ros_gz_image");
   node_->declare_parameter("qos", "default");
+  node_->declare_parameter("lazy", false);
+  node_->declare_parameter("subscription_heartbeat", 1000);
+
+  const bool lazy = node_->get_parameter("lazy").as_bool();
+  const int heartbeat_ms = node_->get_parameter("subscription_heartbeat").as_int();
 
   // Gazebo node
   auto gz_node = std::make_shared<gz::transport::Node>();
@@ -107,9 +169,21 @@ int main(int argc, char * argv[])
   --argc;
   auto args = rclcpp::remove_ros_arguments(argc, argv);
 
-  // Create publishers and subscribers
+  // Create publishers (and subscribers if not lazy)
   for (auto topic : args) {
-    handlers.push_back(std::make_shared<Handler>(topic, node_, gz_node));
+    handlers.push_back(std::make_shared<Handler>(topic, node_, gz_node, lazy));
+  }
+
+  // When lazy, periodically check ROS subscriber count to start/stop Gz subscriptions
+  rclcpp::TimerBase::SharedPtr heartbeat_timer;
+  if (lazy) {
+    heartbeat_timer = node_->create_wall_timer(
+      std::chrono::milliseconds(heartbeat_ms),
+      [&handlers]() {
+        for (auto & handler : handlers) {
+          handler->Spin();
+        }
+      });
   }
 
   // Spin ROS and Gz until shutdown