[Jazzy] Allow disabling of matched_events to always run

image_proc/include/image_proc/crop_decimate.hpp

@@ -68,6 +68,7 @@ class CropDecimateNode : public rclcpp::Node
   int decimation_x_, decimation_y_, offset_x_, offset_y_, width_, height_;
   std::string image_topic_;
   CropDecimateModes interpolation_;
+  bool always_subscribe_;
 
   void imageCb(
     const sensor_msgs::msg::Image::ConstSharedPtr image_msg,

image_proc/include/image_proc/crop_non_zero.hpp

@@ -55,6 +55,7 @@ class CropNonZeroNode : public rclcpp::Node
   std::mutex connect_mutex_;
 
   image_transport::Publisher pub_;
+  bool always_subscribe_;
 
   void imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg);
 };

image_proc/include/image_proc/debayer.hpp

@@ -57,6 +57,7 @@ class DebayerNode
 
   image_transport::Publisher pub_mono_;
   image_transport::Publisher pub_color_;
+  bool always_subscribe_;
 
   void connectCb();
   void imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg);

image_proc/include/image_proc/rectify.hpp

@@ -59,6 +59,7 @@ class RectifyNode
 
   // Processing state (note: only safe because we're using single-threaded NodeHandle!)
   image_geometry::PinholeCameraModel model_;
+  bool always_subscribe_;
 
   void imageCb(
     const sensor_msgs::msg::Image::ConstSharedPtr & image_msg,

image_proc/include/image_proc/resize.hpp

@@ -62,6 +62,7 @@ class ResizeNode
   cv_bridge::CvImage scaled_cv_;
 
   std::mutex connect_mutex_;
+  bool always_subscribe_;
 
   void connectCb();
 

image_proc/include/image_proc/track_marker.hpp

@@ -60,6 +60,7 @@ class TrackMarkerNode : public rclcpp::Node
 
   cv::Ptr<cv::aruco::DetectorParameters> detector_params_;
   cv::Ptr<cv::aruco::Dictionary> dictionary_;
+  bool always_subscribe_;
 
   void imageCb(
     const sensor_msgs::msg::Image::ConstSharedPtr & image_msg,

image_proc/src/crop_decimate.cpp

@@ -130,12 +130,16 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
   // default: CropDecimate_NN
   int interpolation = this->declare_parameter("interpolation", 0);
   interpolation_ = static_cast<CropDecimateModes>(interpolation);
+  always_subscribe_ = this->declare_parameter("always_subscribe", false);
 
   // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
+      if (always_subscribe_) {
+        return;
+      }
       if (pub_.getNumSubscribers() == 0) {
         sub_.shutdown();
       } else if (!sub_) {
@@ -153,6 +157,16 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
   pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   pub_ = image_transport::create_camera_publisher(this, pub_topic, rmw_qos_profile_default,
       pub_options);
+
+  // If always subscribing, create the subscription immediately
+  if (always_subscribe_) {
+    auto qos_profile = getTopicQosProfile(this, image_topic_);
+    image_transport::TransportHints hints(this);
+    sub_ = image_transport::create_camera_subscription(
+      this, image_topic_, std::bind(
+        &CropDecimateNode::imageCb, this,
+        std::placeholders::_1, std::placeholders::_2), hints.getTransport(), qos_profile);
+  }
 }
 
 void CropDecimateNode::imageCb(

image_proc/src/crop_non_zero.cpp

@@ -58,12 +58,16 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)
   auto node_base = this->get_node_base_interface();
   image_topic_ = node_base->resolve_topic_or_service_name("image_raw", false);
   std::string pub_topic = node_base->resolve_topic_or_service_name("image", false);
+  always_subscribe_ = this->declare_parameter("always_subscribe", false);
 
   // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
+      if (always_subscribe_) {
+        return;
+      }
       if (pub_.getNumSubscribers() == 0) {
         sub_raw_.shutdown();
       } else if (!sub_raw_) {
@@ -80,6 +84,16 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)
   // Create publisher - allow overriding QoS settings (history, depth, reliability)
   pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   pub_ = image_transport::create_publisher(this, pub_topic, rmw_qos_profile_default, pub_options);
+
+  // If always subscribing, create the subscription immediately
+  if (always_subscribe_) {
+    auto qos_profile = getTopicQosProfile(this, image_topic_);
+    image_transport::TransportHints hints(this);
+    sub_raw_ = image_transport::create_subscription(
+      this, image_topic_, std::bind(
+        &CropNonZeroNode::imageCb, this,
+        std::placeholders::_1), hints.getTransport(), qos_profile);
+  }
 }
 
 void CropNonZeroNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)

image_proc/src/debayer.cpp

@@ -54,6 +54,7 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
   this->declare_parameter<std::string>("image_transport", "raw");
 
   debayer_ = this->declare_parameter("debayer", 3);
+  always_subscribe_ = this->declare_parameter("always_subscribe", false);
 
   // For compressed topics to remap appropriately, we need to pass a
   // fully expanded and remapped topic name to image_transport
@@ -67,6 +68,9 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
+      if (always_subscribe_) {
+        return;
+      }
       if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0) {
         sub_raw_.shutdown();
       } else if (!sub_raw_) {
@@ -87,6 +91,17 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
       pub_options);
   pub_color_ = image_transport::create_publisher(this, color_topic, rmw_qos_profile_default,
       pub_options);
+
+  // If always subscribing, create the subscription immediately
+  if (always_subscribe_) {
+    auto qos_profile = getTopicQosProfile(this, image_topic_);
+    image_transport::TransportHints hints(this);
+    sub_raw_ = image_transport::create_subscription(
+      this, image_topic_,
+      std::bind(
+        &DebayerNode::imageCb, this,
+        std::placeholders::_1), hints.getTransport(), qos_profile);
+  }
 }
 
 void DebayerNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)

image_proc/src/rectify.cpp

@@ -59,12 +59,16 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options)
 
   queue_size_ = this->declare_parameter("queue_size", 5);
   interpolation_ = this->declare_parameter("interpolation", 1);
+  always_subscribe_ = this->declare_parameter("always_subscribe", false);
 
   // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
+      if (always_subscribe_) {
+        return;
+      }
       if (pub_rect_.getNumSubscribers() == 0) {
         sub_camera_.shutdown();
       } else if (!sub_camera_) {
@@ -82,6 +86,16 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options)
   pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   pub_rect_ = image_transport::create_publisher(this, "image_rect", rmw_qos_profile_default,
       pub_options);
+
+  // If always subscribing, create the subscription immediately
+  if (always_subscribe_) {
+    auto qos_profile = getTopicQosProfile(this, image_topic_);
+    image_transport::TransportHints hints(this);
+    sub_camera_ = image_transport::create_camera_subscription(
+      this, image_topic_, std::bind(
+        &RectifyNode::imageCb,
+        this, std::placeholders::_1, std::placeholders::_2), hints.getTransport(), qos_profile);
+  }
 }
 
 void RectifyNode::imageCb(

image_proc/src/resize.cpp

@@ -65,12 +65,16 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)
   scale_width_ = this->declare_parameter("scale_width", 1.0);
   height_ = this->declare_parameter("height", -1);
   width_ = this->declare_parameter("width", -1);
+  always_subscribe_ = this->declare_parameter("always_subscribe", false);
 
   // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
+      if (always_subscribe_) {
+        return;
+      }
       if (pub_image_.getNumSubscribers() == 0) {
         sub_image_.shutdown();
       } else if (!sub_image_) {
@@ -90,6 +94,18 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)
   pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   pub_image_ =
     image_transport::create_camera_publisher(this, pub_topic, rmw_qos_profile_default, pub_options);
+
+  // If always subscribing, create the subscription immediately
+  if (always_subscribe_) {
+    auto qos_profile = getTopicQosProfile(this, image_topic_);
+    image_transport::TransportHints hints(this);
+    sub_image_ = image_transport::create_camera_subscription(
+      this, image_topic_,
+      std::bind(
+        &ResizeNode::imageCb, this,
+        std::placeholders::_1,
+        std::placeholders::_2), hints.getTransport(), qos_profile);
+  }
 }
 
 void ResizeNode::imageCb(

image_proc/src/track_marker.cpp

@@ -75,11 +75,16 @@ TrackMarkerNode::TrackMarkerNode(const rclcpp::NodeOptions & options)
   dictionary_ = cv::aruco::getPredefinedDictionary(dict_id);
   #endif
 
+  always_subscribe_ = this->declare_parameter("always_subscribe", false);
+
   // Setup lazy subscriber using publisher connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
     [this](rclcpp::MatchedInfo &)
     {
+      if (always_subscribe_) {
+        return;
+      }
       if (pub_->get_subscription_count() == 0) {
         sub_camera_.shutdown();
       } else if (!sub_camera_) {
@@ -98,6 +103,17 @@ TrackMarkerNode::TrackMarkerNode(const rclcpp::NodeOptions & options)
   pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
     "tracked_pose", 10, pub_options);
+
+  // If always subscribing, create the subscription immediately
+  if (always_subscribe_) {
+    auto qos_profile = getTopicQosProfile(this, image_topic_);
+    image_transport::TransportHints hints(this);
+    sub_camera_ = image_transport::create_camera_subscription(
+      this, image_topic_, std::bind(
+        &TrackMarkerNode::imageCb,
+        this, std::placeholders::_1, std::placeholders::_2),
+      hints.getTransport(), qos_profile);
+  }
 }
 
 void TrackMarkerNode::imageCb(