diff --git a/image_proc/include/image_proc/crop_decimate.hpp b/image_proc/include/image_proc/crop_decimate.hpp index b38f79540..41c200480 100644 --- a/image_proc/include/image_proc/crop_decimate.hpp +++ b/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, diff --git a/image_proc/include/image_proc/crop_non_zero.hpp b/image_proc/include/image_proc/crop_non_zero.hpp index c8e3edcd8..69734d255 100644 --- a/image_proc/include/image_proc/crop_non_zero.hpp +++ b/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); }; diff --git a/image_proc/include/image_proc/debayer.hpp b/image_proc/include/image_proc/debayer.hpp index 1f3d3d134..72ca56ee0 100644 --- a/image_proc/include/image_proc/debayer.hpp +++ b/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); diff --git a/image_proc/include/image_proc/rectify.hpp b/image_proc/include/image_proc/rectify.hpp index aec7e5d2e..d4b654140 100644 --- a/image_proc/include/image_proc/rectify.hpp +++ b/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, diff --git a/image_proc/include/image_proc/resize.hpp b/image_proc/include/image_proc/resize.hpp index 4d78958ee..180135564 100644 --- a/image_proc/include/image_proc/resize.hpp +++ b/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(); diff --git a/image_proc/include/image_proc/track_marker.hpp b/image_proc/include/image_proc/track_marker.hpp index c70bf92c8..ff375e1a8 100644 --- a/image_proc/include/image_proc/track_marker.hpp +++ b/image_proc/include/image_proc/track_marker.hpp @@ -60,6 +60,7 @@ class TrackMarkerNode : public rclcpp::Node cv::Ptr detector_params_; cv::Ptr dictionary_; + bool always_subscribe_; void imageCb( const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp index a424abde6..a8d223fab 100644 --- a/image_proc/src/crop_decimate.cpp +++ b/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(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( diff --git a/image_proc/src/crop_non_zero.cpp b/image_proc/src/crop_non_zero.cpp index 8316b4776..c5eb93af7 100644 --- a/image_proc/src/crop_non_zero.cpp +++ b/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) diff --git a/image_proc/src/debayer.cpp b/image_proc/src/debayer.cpp index b2c326798..2a639eedf 100644 --- a/image_proc/src/debayer.cpp +++ b/image_proc/src/debayer.cpp @@ -54,6 +54,7 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options) this->declare_parameter("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) diff --git a/image_proc/src/rectify.cpp b/image_proc/src/rectify.cpp index e7cbd9b1e..972d5907b 100644 --- a/image_proc/src/rectify.cpp +++ b/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( diff --git a/image_proc/src/resize.cpp b/image_proc/src/resize.cpp index 0b30983cf..028b2a6e9 100644 --- a/image_proc/src/resize.cpp +++ b/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( diff --git a/image_proc/src/track_marker.cpp b/image_proc/src/track_marker.cpp index 5d249c058..ca2117e8c 100644 --- a/image_proc/src/track_marker.cpp +++ b/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( "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(