From 3c3d5b57aa8f168a054ec7ba53bd3376d53bd7b4 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Thu, 18 Jan 2024 10:58:49 -0500 Subject: [PATCH 1/3] make remaining components lazy missed a few components in #815 --- depth_image_proc/src/crop_foremost.cpp | 45 +++++++++------------- depth_image_proc/src/disparity.cpp | 45 ++++++++-------------- depth_image_proc/src/register.cpp | 52 ++++++++++---------------- 3 files changed, 54 insertions(+), 88 deletions(-) diff --git a/depth_image_proc/src/crop_foremost.cpp b/depth_image_proc/src/crop_foremost.cpp index e44d4ca34..f0c9738ac 100644 --- a/depth_image_proc/src/crop_foremost.cpp +++ b/depth_image_proc/src/crop_foremost.cpp @@ -72,33 +72,24 @@ CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options) { distance_ = this->declare_parameter("distance", 0.0); - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // image_transport::SubscriberStatusCallback connect_cb = - // std::bind(&CropForemostNode::connectCb, this); - connectCb(); - // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement when SubscriberStatusCallback is available - // pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb); - pub_depth_ = image_transport::create_publisher(this, "image"); -} - -// Handles (un)subscribing when clients (un)subscribe -void CropForemostNode::connectCb() -{ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it - // if (pub_depth_.getNumSubscribers() == 0) - if (0) { - sub_raw_.shutdown(); - } else if (!sub_raw_) { - image_transport::TransportHints hints(this, "raw"); - sub_raw_ = image_transport::create_subscription( - this, "image_raw", - std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1), - hints.getTransport()); - } + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo &) + { + std::lock_guard lock(connect_mutex_); + if (pub_depth_.getNumSubscribers() == 0) { + sub_raw_.shutdown(); + } else if (!sub_raw_) { + image_transport::TransportHints hints(this, "raw"); + sub_raw_ = image_transport::create_subscription( + this, "image_raw", + std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1), + hints.getTransport()); + } + }; + pub_depth_ = + image_transport::create_publisher(this, "image", rmw_qos_profile_default, pub_options); } void CropForemostNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg) diff --git a/depth_image_proc/src/disparity.cpp b/depth_image_proc/src/disparity.cpp index 51a97c4da..367395228 100644 --- a/depth_image_proc/src/disparity.cpp +++ b/depth_image_proc/src/disparity.cpp @@ -69,8 +69,6 @@ class DisparityNode : public rclcpp::Node double max_range_; double delta_d_; - void connectCb(); - void depthCb( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg); @@ -98,34 +96,23 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) std::bind( &DisparityNode::depthCb, this, std::placeholders::_1, std::placeholders::_2)); - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // ros::SubscriberStatusCallback connect_cb = std::bind(&DisparityNode::connectCb, this); - connectCb(); - - // Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement when SubscriberStatusCallback is available - // pub_disparity_ = - // left_nh.advertise("disparity", 1, connect_cb, connect_cb); + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo &) + { + std::lock_guard lock(connect_mutex_); + if (pub_disparity_->get_subscription_count() == 0) { + sub_depth_image_.unsubscribe(); + sub_info_.unsubscribe(); + } else if (!sub_depth_image_.getSubscriber()) { + image_transport::TransportHints hints(this, "raw"); + sub_depth_image_.subscribe(this, "left/image_rect", hints.getTransport()); + sub_info_.subscribe(this, "right/camera_info"); + } + }; pub_disparity_ = create_publisher( - "left/disparity", rclcpp::SensorDataQoS()); -} - -// Handles (un)subscribing when clients (un)subscribe -void DisparityNode::connectCb() -{ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it - // if (pub_disparity_.getNumSubscribers() == 0) - if (0) { - sub_depth_image_.unsubscribe(); - sub_info_.unsubscribe(); - } else if (!sub_depth_image_.getSubscriber()) { - image_transport::TransportHints hints(this, "raw"); - sub_depth_image_.subscribe(this, "left/image_rect", hints.getTransport()); - sub_info_.subscribe(this, "right/camera_info"); - } + "left/disparity", rclcpp::SensorDataQoS(), pub_options); } void DisparityNode::depthCb( diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index acff4b0c9..eaa6ce468 100644 --- a/depth_image_proc/src/register.cpp +++ b/depth_image_proc/src/register.cpp @@ -83,8 +83,6 @@ class RegisterNode : public rclcpp::Node // Parameters bool fill_upsampling_holes_; - void connectCb(); - void imageCb( const Image::ConstSharedPtr & depth_image_msg, const CameraInfo::ConstSharedPtr & depth_info_msg, @@ -119,36 +117,26 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options) &RegisterNode::imageCb, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // image_transport::SubscriberStatusCallback image_connect_cb - // boost::bind(&RegisterNode::connectCb, this); - // ros::SubscriberStatusCallback info_connect_cb = boost::bind(&RegisterNode::connectCb, this); - connectCb(); - // Make sure we don't enter connectCb() between advertising and assigning to pub_registered_ - std::lock_guard lock(connect_mutex_); - // pub_registered_ = it_depth_reg.advertiseCamera("image_rect", 1, - // image_connect_cb, image_connect_cb, - // info_connect_cb, info_connect_cb); - pub_registered_ = image_transport::create_camera_publisher(this, "depth_registered/image_rect"); -} - -// Handles (un)subscribing when clients (un)subscribe -void RegisterNode::connectCb() -{ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it - // if (pub_point_cloud_->getNumSubscribers() == 0) - if (0) { - sub_depth_image_.unsubscribe(); - sub_depth_info_.unsubscribe(); - sub_rgb_info_.unsubscribe(); - } else if (!sub_depth_image_.getSubscriber()) { - image_transport::TransportHints hints(this, "raw"); - sub_depth_image_.subscribe(this, "depth/image_rect", hints.getTransport()); - sub_depth_info_.subscribe(this, "depth/camera_info"); - sub_rgb_info_.subscribe(this, "rgb/camera_info"); - } + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo &) + { + std::lock_guard lock(connect_mutex_); + if (pub_registered_.getNumSubscribers() == 0) { + sub_depth_image_.unsubscribe(); + sub_depth_info_.unsubscribe(); + sub_rgb_info_.unsubscribe(); + } else if (!sub_depth_image_.getSubscriber()) { + image_transport::TransportHints hints(this, "raw"); + sub_depth_image_.subscribe(this, "depth/image_rect", hints.getTransport()); + sub_depth_info_.subscribe(this, "depth/camera_info"); + sub_rgb_info_.subscribe(this, "rgb/camera_info"); + } + }; + pub_registered_ = + image_transport::create_camera_publisher(this, "depth_registered/image_rect", + rmw_qos_profile_default, pub_options); } void RegisterNode::imageCb( From f7abed1c3c05be504130256b9ca314284db70a58 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Thu, 18 Jan 2024 11:06:25 -0500 Subject: [PATCH 2/3] linting --- depth_image_proc/src/register.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index eaa6ce468..9e68dc6fb 100644 --- a/depth_image_proc/src/register.cpp +++ b/depth_image_proc/src/register.cpp @@ -135,8 +135,9 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options) } }; pub_registered_ = - image_transport::create_camera_publisher(this, "depth_registered/image_rect", - rmw_qos_profile_default, pub_options); + image_transport::create_camera_publisher( + this, "depth_registered/image_rect", + rmw_qos_profile_default, pub_options); } void RegisterNode::imageCb( From 89a20dcb5a6e6371228305b0f3a7657d952b5bdb Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Thu, 18 Jan 2024 11:24:30 -0500 Subject: [PATCH 3/3] remove connectCb --- depth_image_proc/src/crop_foremost.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/depth_image_proc/src/crop_foremost.cpp b/depth_image_proc/src/crop_foremost.cpp index f0c9738ac..7581bb1d1 100644 --- a/depth_image_proc/src/crop_foremost.cpp +++ b/depth_image_proc/src/crop_foremost.cpp @@ -58,8 +58,6 @@ class CropForemostNode : public rclcpp::Node std::mutex connect_mutex_; image_transport::Publisher pub_depth_; - void connectCb(); - void depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg); double distance_;