diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp index 397f4ab6d..c5fc4bfd2 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp @@ -73,8 +73,6 @@ class PointCloudXyzNode : public rclcpp::Node image_geometry::PinholeCameraModel model_; - void connectCb(); - void depthCb( const Image::ConstSharedPtr & depth_msg, const CameraInfo::ConstSharedPtr & info_msg); diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyz_radial.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyz_radial.hpp index 2cfc44e03..8cf5ae852 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyz_radial.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyz_radial.hpp @@ -73,8 +73,6 @@ class PointCloudXyzRadialNode : public rclcpp::Node cv::Mat transform_; - void connectCb(); - void depthCb( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg); diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp index cd2cbd6ea..1a9e754a2 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp @@ -77,8 +77,6 @@ class PointCloudXyziNode : public rclcpp::Node image_geometry::PinholeCameraModel model_; - void connectCb(); - void imageCb( const Image::ConstSharedPtr & depth_msg, const Image::ConstSharedPtr & intensity_msg, diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi_radial.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi_radial.hpp index 98398f227..d4838e120 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi_radial.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi_radial.hpp @@ -90,8 +90,6 @@ class PointCloudXyziRadialNode : public rclcpp::Node cv::Mat transform_; - void connectCb(); - void imageCb( const Image::ConstSharedPtr & depth_msg, const Image::ConstSharedPtr & intensity_msg_in, diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp index e18db0cfc..f2b99c165 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp @@ -81,8 +81,6 @@ class PointCloudXyzrgbNode : public rclcpp::Node image_geometry::PinholeCameraModel model_; - void connectCb(); - void imageCb( const Image::ConstSharedPtr & depth_msg, const Image::ConstSharedPtr & rgb_msg, diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp index ef07c31d5..6db3759f7 100644 --- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp +++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp @@ -93,8 +93,6 @@ class PointCloudXyzrgbRadialNode : public rclcpp::Node image_geometry::PinholeCameraModel model_; - void connectCb(); - void imageCb( const Image::ConstSharedPtr & depth_msg, const Image::ConstSharedPtr & rgb_msg, diff --git a/depth_image_proc/src/convert_metric.cpp b/depth_image_proc/src/convert_metric.cpp index f52f3e560..e77215abe 100644 --- a/depth_image_proc/src/convert_metric.cpp +++ b/depth_image_proc/src/convert_metric.cpp @@ -58,42 +58,32 @@ class ConvertMetricNode : public rclcpp::Node std::mutex connect_mutex_; image_transport::Publisher pub_depth_; - void connectCb(); - void depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg); }; ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options) : Node("ConvertMetricNode", options) { - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // image_transport::SubscriberStatusCallback connect_cb = - // std::bind(&ConvertMetricNode::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 ConvertMetricNode::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(&ConvertMetricNode::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(&ConvertMetricNode::depthCb, this, std::placeholders::_1), + hints.getTransport()); + } + }; + pub_depth_ = image_transport::create_publisher(this, "image", rmw_qos_profile_default, pub_options); } void ConvertMetricNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg) diff --git a/depth_image_proc/src/point_cloud_xyz.cpp b/depth_image_proc/src/point_cloud_xyz.cpp index 8da179831..ac3ea9a79 100644 --- a/depth_image_proc/src/point_cloud_xyz.cpp +++ b/depth_image_proc/src/point_cloud_xyz.cpp @@ -54,38 +54,32 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options) // Read parameters queue_size_ = this->declare_parameter("queue_size", 5); - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzNode::connectCb, this); - connectCb(); - - // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement when SubscriberStatusCallback is available - // pub_point_cloud_ = nh.advertise("points", 1, connect_cb, connect_cb); - pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS()); + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo& s) + { + std::lock_guard lock(connect_mutex_); + if (s.current_count == 0) + { + sub_depth_.shutdown(); + } + else if (!sub_depth_) + { + auto custom_qos = rmw_qos_profile_system_default; + custom_qos.depth = queue_size_; + + sub_depth_ = image_transport::create_camera_subscription( + this, + "image_rect", + std::bind(&PointCloudXyzNode::depthCb, this, std::placeholders::_1, std::placeholders::_2), + "raw", + custom_qos); + } + }; + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); } -// Handles (un)subscribing when clients (un)subscribe -void PointCloudXyzNode::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_.shutdown(); - } else if (!sub_depth_) { - auto custom_qos = rmw_qos_profile_system_default; - custom_qos.depth = queue_size_; - - sub_depth_ = image_transport::create_camera_subscription( - this, - "image_rect", - std::bind(&PointCloudXyzNode::depthCb, this, std::placeholders::_1, std::placeholders::_2), - "raw", - custom_qos); - } -} void PointCloudXyzNode::depthCb( const Image::ConstSharedPtr & depth_msg, diff --git a/depth_image_proc/src/point_cloud_xyz_radial.cpp b/depth_image_proc/src/point_cloud_xyz_radial.cpp index db5692709..e60fb532b 100644 --- a/depth_image_proc/src/point_cloud_xyz_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyz_radial.cpp @@ -53,40 +53,32 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt // Read parameters queue_size_ = this->declare_parameter("queue_size", 5); - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // ros::SubscriberStatusCallback connect_cb = - // boost::bind(&PointCloudXyzRadialNode::connectCb, this); - connectCb(); - // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement when SubscriberStatusCallback is available - // pub_point_cloud_ = nh.advertise("points", 1, connect_cb, connect_cb); - pub_point_cloud_ = create_publisher( - "points", rclcpp::SensorDataQoS()); -} - -// Handles (un)subscribing when clients (un)subscribe -void PointCloudXyzRadialNode::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_.shutdown(); - } else if (!sub_depth_) { - auto custom_qos = rmw_qos_profile_system_default; - custom_qos.depth = queue_size_; - - sub_depth_ = image_transport::create_camera_subscription( - this, - "image_raw", - std::bind( - &PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1, - std::placeholders::_2), - "raw", - custom_qos); - } + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo& s) + { + std::lock_guard lock(connect_mutex_); + if (s.current_count == 0) + { + sub_depth_.shutdown(); + } + else if (!sub_depth_) + { + auto custom_qos = rmw_qos_profile_system_default; + custom_qos.depth = queue_size_; + + sub_depth_ = image_transport::create_camera_subscription( + this, + "image_raw", + std::bind( + &PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1, + std::placeholders::_2), + "raw", + custom_qos); + } + }; + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); } void PointCloudXyzRadialNode::depthCb( diff --git a/depth_image_proc/src/point_cloud_xyzi.cpp b/depth_image_proc/src/point_cloud_xyzi.cpp index 8fe52c9f9..7a28f025b 100644 --- a/depth_image_proc/src/point_cloud_xyzi.cpp +++ b/depth_image_proc/src/point_cloud_xyzi.cpp @@ -71,40 +71,34 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options) std::placeholders::_2, std::placeholders::_3)); - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyziNode::connectCb, this); - connectCb(); - // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement when SubscriberStatusCallback is available - // pub_point_cloud_ = depth_nh.advertise("points", 1, connect_cb, connect_cb); - pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS()); -} - -// Handles (un)subscribing when clients (un)subscribe -void PointCloudXyziNode::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_.unsubscribe(); - sub_intensity_.unsubscribe(); - sub_info_.unsubscribe(); - } else if (!sub_depth_.getSubscriber()) { - // parameter for depth_image_transport hint - std::string depth_image_transport_param = "depth_image_transport"; - - // depth image can use different transport.(e.g. compressedDepth) - image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); - sub_depth_.subscribe(this, "depth/image_rect", depth_hints.getTransport()); - - // intensity uses normal ros transport hints. - image_transport::TransportHints hints(this, "raw"); - sub_intensity_.subscribe(this, "intensity/image_rect", hints.getTransport()); - sub_info_.subscribe(this, "intensity/camera_info"); - } + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo& s) + { + std::lock_guard lock(connect_mutex_); + if (s.current_count == 0) + { + sub_depth_.unsubscribe(); + sub_intensity_.unsubscribe(); + sub_info_.unsubscribe(); + } + else if (!sub_depth_.getSubscriber()) + { + // parameter for depth_image_transport hint + std::string depth_image_transport_param = "depth_image_transport"; + + // depth image can use different transport.(e.g. compressedDepth) + image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); + sub_depth_.subscribe(this, "depth/image_rect", depth_hints.getTransport()); + + // intensity uses normal ros transport hints. + image_transport::TransportHints hints(this, "raw"); + sub_intensity_.subscribe(this, "intensity/image_rect", hints.getTransport()); + sub_info_.subscribe(this, "intensity/camera_info"); + } + }; + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); } void PointCloudXyziNode::imageCb( diff --git a/depth_image_proc/src/point_cloud_xyzi_radial.cpp b/depth_image_proc/src/point_cloud_xyzi_radial.cpp index 313826bfd..f201231a3 100644 --- a/depth_image_proc/src/point_cloud_xyzi_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyzi_radial.cpp @@ -69,44 +69,34 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o std::placeholders::_2, std::placeholders::_3)); - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // ros::SubscriberStatusCallback connect_cb = - // boost::bind(&PointCloudXyziRadialNode::connectCb, this); - connectCb(); - - // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement when SubscriberStatusCallback is available - // pub_point_cloud_ = nh.advertise("points", 20, connect_cb, connect_cb); - pub_point_cloud_ = create_publisher( - "points", rclcpp::SensorDataQoS()); -} - -// Handles (un)subscribing when clients (un)subscribe -void PointCloudXyziRadialNode::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_.unsubscribe(); - sub_intensity_.unsubscribe(); - sub_info_.unsubscribe(); - } else if (!sub_depth_.getSubscriber()) { - // parameter for depth_image_transport hint - std::string depth_image_transport_param = "depth_image_transport"; - - // depth image can use different transport.(e.g. compressedDepth) - image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); - sub_depth_.subscribe(this, "depth/image_raw", depth_hints.getTransport()); - - // intensity uses normal ros transport hints. - image_transport::TransportHints hints(this, "raw"); - sub_intensity_.subscribe(this, "intensity/image_raw", hints.getTransport()); - sub_info_.subscribe(this, "intensity/camera_info"); - } + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo& s) + { + std::lock_guard lock(connect_mutex_); + if (s.current_count == 0) + { + sub_depth_.unsubscribe(); + sub_intensity_.unsubscribe(); + sub_info_.unsubscribe(); + } + else if (!sub_depth_.getSubscriber()) + { + // parameter for depth_image_transport hint + std::string depth_image_transport_param = "depth_image_transport"; + + // depth image can use different transport.(e.g. compressedDepth) + image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); + sub_depth_.subscribe(this, "depth/image_raw", depth_hints.getTransport()); + + // intensity uses normal ros transport hints. + image_transport::TransportHints hints(this, "raw"); + sub_intensity_.subscribe(this, "intensity/image_raw", hints.getTransport()); + sub_info_.subscribe(this, "intensity/camera_info"); + } + }; + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); } void PointCloudXyziRadialNode::imageCb( diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp index c77a1e680..d6341e46d 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb.cpp @@ -80,43 +80,34 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options) std::placeholders::_3)); } - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzrgbNode::connectCb, this); - connectCb(); - // TODO(ros2) Implement when SubscriberStatusCallback is available - // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available - // pub_point_cloud_ = depth_nh.advertise("points", 1, connect_cb, connect_cb); - pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS()); - // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available -} - -// Handles (un)subscribing when clients (un)subscribe -void PointCloudXyzrgbNode::connectCb() -{ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it - // if (pub_point_cloud_->getNumSubscribers() == 0) - if (0) { - // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it - sub_depth_.unsubscribe(); - sub_rgb_.unsubscribe(); - sub_info_.unsubscribe(); - } else if (!sub_depth_.getSubscriber()) { - // parameter for depth_image_transport hint - std::string depth_image_transport_param = "depth_image_transport"; - image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); - - // depth image can use different transport.(e.g. compressedDepth) - sub_depth_.subscribe(this, "depth_registered/image_rect", depth_hints.getTransport()); - - // rgb uses normal ros transport hints. - image_transport::TransportHints hints(this, "raw"); - sub_rgb_.subscribe(this, "rgb/image_rect_color", hints.getTransport()); - sub_info_.subscribe(this, "rgb/camera_info"); - } + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo& s) + { + std::lock_guard lock(connect_mutex_); + if (s.current_count == 0) + { + sub_depth_.unsubscribe(); + sub_rgb_.unsubscribe(); + sub_info_.unsubscribe(); + } + else if (!sub_depth_.getSubscriber()) + { + // parameter for depth_image_transport hint + std::string depth_image_transport_param = "depth_image_transport"; + image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); + + // depth image can use different transport.(e.g. compressedDepth) + sub_depth_.subscribe(this, "depth_registered/image_rect", depth_hints.getTransport()); + + // rgb uses normal ros transport hints. + image_transport::TransportHints hints(this, "raw"); + sub_rgb_.subscribe(this, "rgb/image_rect_color", hints.getTransport()); + sub_info_.subscribe(this, "rgb/camera_info"); + } + }; + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); } void PointCloudXyzrgbNode::imageCb( diff --git a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp index 56230eaf9..de53e17c1 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp @@ -84,44 +84,34 @@ PointCloudXyzrgbRadialNode::PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions std::placeholders::_3)); } - // Monitor whether anyone is subscribed to the output - // TODO(ros2) Implement when SubscriberStatusCallback is available - // ros::SubscriberStatusCallback connect_cb = - // boost::bind(&PointCloudXyzrgbRadialNode::connectCb, this); - connectCb(); - // TODO(ros2) Implement when SubscriberStatusCallback is available - // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ - // std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available - // pub_point_cloud_ = depth_nh.advertise("points", 1, connect_cb, connect_cb); - pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS()); - // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available -} - -// Handles (un)subscribing when clients (un)subscribe -void PointCloudXyzrgbRadialNode::connectCb() -{ - std::lock_guard lock(connect_mutex_); - // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it - // if (pub_point_cloud_->getNumSubscribers() == 0) - if (0) { - // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it - sub_depth_.unsubscribe(); - sub_rgb_.unsubscribe(); - sub_info_.unsubscribe(); - } else if (!sub_depth_.getSubscriber()) { - // parameter for depth_image_transport hint - std::string depth_image_transport_param = "depth_image_transport"; - image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo& s) + { + std::lock_guard lock(connect_mutex_); + if (s.current_count == 0) + { + sub_depth_.unsubscribe(); + sub_rgb_.unsubscribe(); + sub_info_.unsubscribe(); + } + else if (!sub_depth_.getSubscriber()) + { + // parameter for depth_image_transport hint + std::string depth_image_transport_param = "depth_image_transport"; + image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); - // depth image can use different transport.(e.g. compressedDepth) - sub_depth_.subscribe(this, "depth_registered/image_rect", depth_hints.getTransport()); + // depth image can use different transport.(e.g. compressedDepth) + sub_depth_.subscribe(this, "depth_registered/image_rect", depth_hints.getTransport()); - // rgb uses normal ros transport hints. - image_transport::TransportHints hints(this, "raw"); - sub_rgb_.subscribe(this, "rgb/image_rect_color", hints.getTransport()); - sub_info_.subscribe(this, "rgb/camera_info"); - } + // rgb uses normal ros transport hints. + image_transport::TransportHints hints(this, "raw"); + sub_rgb_.subscribe(this, "rgb/image_rect_color", hints.getTransport()); + sub_info_.subscribe(this, "rgb/camera_info"); + } + }; + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); } void PointCloudXyzrgbRadialNode::imageCb( diff --git a/image_proc/include/image_proc/rectify.hpp b/image_proc/include/image_proc/rectify.hpp index 4b80b25b2..c1f9915b8 100644 --- a/image_proc/include/image_proc/rectify.hpp +++ b/image_proc/include/image_proc/rectify.hpp @@ -56,13 +56,11 @@ class RectifyNode int queue_size_; int interpolation; - std::mutex connect_mutex_; image_transport::Publisher pub_rect_; // Processing state (note: only safe because we're using single-threaded NodeHandle!) image_geometry::PinholeCameraModel model_; - void subscribeToCamera(const rmw_qos_profile_t & qos_profile); void imageCb( const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg); diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp index c03bd3f75..bc678427c 100644 --- a/image_proc/src/crop_decimate.cpp +++ b/image_proc/src/crop_decimate.cpp @@ -127,11 +127,25 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options) int interpolation = this->declare_parameter("interpolation", 0); interpolation_ = static_cast(interpolation); - pub_ = image_transport::create_camera_publisher(this, "out/image_raw", qos_profile); - sub_ = image_transport::create_camera_subscription( - this, "in/image_raw", std::bind( - &CropDecimateNode::imageCb, this, - std::placeholders::_1, std::placeholders::_2), "raw", qos_profile); + // Create image pub with connection callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo&) + { + if (pub_.getNumSubscribers() == 0) + { + sub_.shutdown(); + } + else if (!sub_) + { + auto qos_profile = getTopicQosProfile(this, "in/image_raw"); + sub_ = image_transport::create_camera_subscription( + this, "in/image_raw", std::bind( + &CropDecimateNode::imageCb, this, + std::placeholders::_1, std::placeholders::_2), "raw", qos_profile); + } + }; + pub_ = image_transport::create_camera_publisher(this, "out/image_raw", qos_profile, pub_options); } void CropDecimateNode::imageCb( diff --git a/image_proc/src/debayer.cpp b/image_proc/src/debayer.cpp index 24cb168b4..52640fb2f 100644 --- a/image_proc/src/debayer.cpp +++ b/image_proc/src/debayer.cpp @@ -53,14 +53,27 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options) : Node("DebayerNode", options) { auto qos_profile = getTopicQosProfile(this, "image_raw"); - sub_raw_ = image_transport::create_subscription( - this, "image_raw", - std::bind( - &DebayerNode::imageCb, this, - std::placeholders::_1), "raw", qos_profile); - - pub_mono_ = image_transport::create_publisher(this, "image_mono", qos_profile); - pub_color_ = image_transport::create_publisher(this, "image_color", qos_profile); + // Create publisher options to setup callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo&) + { + if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0) + { + sub_raw_.shutdown(); + } + else if (!sub_raw_) + { + auto qos_profile = getTopicQosProfile(this, "image_raw"); + sub_raw_ = image_transport::create_subscription( + this, "image_raw", + std::bind( + &DebayerNode::imageCb, this, + std::placeholders::_1), "raw", qos_profile); + } + }; + pub_mono_ = image_transport::create_publisher(this, "image_mono", qos_profile, pub_options); + pub_color_ = image_transport::create_publisher(this, "image_color", qos_profile, pub_options); debayer_ = this->declare_parameter("debayer", 3); } diff --git a/image_proc/src/rectify.cpp b/image_proc/src/rectify.cpp index faeb738bb..92d60a270 100644 --- a/image_proc/src/rectify.cpp +++ b/image_proc/src/rectify.cpp @@ -54,28 +54,26 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options) auto qos_profile = getTopicQosProfile(this, "image"); queue_size_ = this->declare_parameter("queue_size", 5); interpolation = this->declare_parameter("interpolation", 1); - pub_rect_ = image_transport::create_publisher(this, "image_rect"); - subscribeToCamera(qos_profile); -} -// Handles (un)subscribing when clients (un)subscribe -void RectifyNode::subscribeToCamera(const rmw_qos_profile_t & qos_profile) -{ - std::lock_guard lock(connect_mutex_); - - /* - * SubscriberStatusCallback not yet implemented - * - if (pub_rect_.getNumSubscribers() == 0) - sub_camera_.shutdown(); - else if (!sub_camera_) - { - */ - sub_camera_ = image_transport::create_camera_subscription( - this, "image", std::bind( - &RectifyNode::imageCb, - this, std::placeholders::_1, std::placeholders::_2), "raw", qos_profile); - // } + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo&) + { + if (pub_rect_.getNumSubscribers() == 0) + { + sub_camera_.shutdown(); + } + else if (!sub_camera_) + { + auto qos_profile = getTopicQosProfile(this, "image"); + sub_camera_ = image_transport::create_camera_subscription( + this, "image", std::bind( + &RectifyNode::imageCb, + this, std::placeholders::_1, std::placeholders::_2), "raw", qos_profile); + } + }; + pub_rect_ = image_transport::create_publisher(this, "image_rect", qos_profile, pub_options); } void RectifyNode::imageCb( diff --git a/image_proc/src/resize.cpp b/image_proc/src/resize.cpp index fc488f787..f47b748de 100644 --- a/image_proc/src/resize.cpp +++ b/image_proc/src/resize.cpp @@ -52,15 +52,27 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options) : rclcpp::Node("ResizeNode", options) { auto qos_profile = getTopicQosProfile(this, "image/image_raw"); - // Create image pub - pub_image_ = image_transport::create_camera_publisher(this, "resize/image_raw", qos_profile); - // Create image sub - sub_image_ = image_transport::create_camera_subscription( - this, "image/image_raw", - std::bind( - &ResizeNode::imageCb, this, - std::placeholders::_1, - std::placeholders::_2), "raw", qos_profile); + // Create image pub with connection callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo&) + { + if (pub_image_.getNumSubscribers() == 0) + { + sub_image_.shutdown(); + } + else if (!sub_image_) + { + auto qos_profile = getTopicQosProfile(this, "image/image_raw"); + sub_image_ = image_transport::create_camera_subscription( + this, "image/image_raw", + std::bind( + &ResizeNode::imageCb, this, + std::placeholders::_1, + std::placeholders::_2), "raw", qos_profile); + } + }; + pub_image_ = image_transport::create_camera_publisher(this, "resize/image_raw", qos_profile, pub_options); interpolation_ = this->declare_parameter("interpolation", 1); use_scale_ = this->declare_parameter("use_scale", true); @@ -74,12 +86,6 @@ void ResizeNode::imageCb( sensor_msgs::msg::Image::ConstSharedPtr image_msg, sensor_msgs::msg::CameraInfo::ConstSharedPtr info_msg) { - // getNumSubscribers has a bug/doesn't work - // Eventually revisit and figure out how to make this work - // if (pub_image_.getNumSubscribers() < 1) { - // return; - //} - TRACEPOINT( image_proc_resize_init, static_cast(this),