From 3dbd2b4782f4b15296b559f2a7a9c41fd566dd2a Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Fri, 19 Jan 2024 17:50:01 -0500 Subject: [PATCH] stereo_image_proc: consistent image_transport (#903) * make image_transport work * make remap work as expected with image_transport * make subscribers lazy --- .../src/stereo_image_proc/disparity_node.cpp | 81 ++++++++++++------- .../stereo_image_proc/point_cloud_node.cpp | 75 +++++++++++------ 2 files changed, 102 insertions(+), 54 deletions(-) diff --git a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp index d092fcc05..ed7384017 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -48,6 +48,7 @@ #include +#include #include #include #include @@ -107,8 +108,6 @@ class DisparityNode : public rclcpp::Node // contains scratch buffers for block matching stereo_image_proc::StereoProcessor block_matcher_; - void connectCb(); - void imageCb( const sensor_msgs::msg::Image::ConstSharedPtr & l_image_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & l_info_msg, @@ -163,6 +162,9 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) { using namespace std::placeholders; + // TransportHints does not actually declare the parameter + this->declare_parameter("image_transport", "raw"); + // Declare/read parameters int queue_size = this->declare_parameter("queue_size", 5); bool approx = this->declare_parameter("approximate_sync", false); @@ -287,35 +289,58 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) this->declare_parameters("", double_params); this->declare_parameters("", bool_params); - // Update the publisher options to allow reconfigurable qos settings. + // Publisher options to allow reconfigurable qos settings and connect callback rclcpp::PublisherOptions pub_opts; pub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - pub_disparity_ = create_publisher("disparity", 1, pub_opts); - - // TODO(jacobperron): Replace this with a graph event. - // Only subscribe if there's a subscription listening to our publisher. - connectCb(); -} + pub_opts.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo & s) + { + if (s.current_count == 0) { + sub_l_image_.unsubscribe(); + sub_l_info_.unsubscribe(); + sub_r_image_.unsubscribe(); + sub_r_info_.unsubscribe(); + } else if (!sub_l_image_.getSubscriber()) { + // Optionally switch between system/sensor defaults + // TODO(fergs): remove and conform to REP-2003? + const bool use_system_default_qos = + this->get_parameter("use_system_default_qos").as_bool(); + rclcpp::QoS image_sub_qos = rclcpp::SensorDataQoS(); + if (use_system_default_qos) { + image_sub_qos = rclcpp::SystemDefaultsQoS(); + } + const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); + auto sub_opts = rclcpp::SubscriptionOptions(); + + // For compressed topics to remap appropriately, we need to pass a + // fully expanded and remapped topic name to image_transport + auto node_base = this->get_node_base_interface(); + std::string left_topic = + node_base->resolve_topic_or_service_name("left/image_rect", false); + std::string right_topic = + node_base->resolve_topic_or_service_name("right/image_rect", false); + // Allow also remapping camera_info to something different than default + std::string left_info_topic = + node_base->resolve_topic_or_service_name( + image_transport::getCameraInfoTopic(left_topic), false); + std::string right_info_topic = + node_base->resolve_topic_or_service_name( + image_transport::getCameraInfoTopic(right_topic), false); + + // Setup hints and QoS overrides + image_transport::TransportHints hints(this); + sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + + sub_l_image_.subscribe( + this, left_topic, hints.getTransport(), image_sub_rmw_qos, sub_opts); + sub_l_info_.subscribe(this, left_info_topic, image_sub_rmw_qos, sub_opts); + sub_r_image_.subscribe( + this, right_topic, hints.getTransport(), image_sub_rmw_qos, sub_opts); + sub_r_info_.subscribe(this, right_info_topic, image_sub_rmw_qos, sub_opts); + } + }; -// Handles (un)subscribing when clients (un)subscribe -void DisparityNode::connectCb() -{ - // TODO(jacobperron): Add unsubscribe logic when we use graph events - image_transport::TransportHints hints(this, "raw"); - const bool use_system_default_qos = this->get_parameter("use_system_default_qos").as_bool(); - rclcpp::QoS image_sub_qos = rclcpp::SensorDataQoS(); - if (use_system_default_qos) { - image_sub_qos = rclcpp::SystemDefaultsQoS(); - } - const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); - auto sub_opts = rclcpp::SubscriptionOptions(); - sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - sub_l_image_.subscribe( - this, "left/image_rect", hints.getTransport(), image_sub_rmw_qos, sub_opts); - sub_l_info_.subscribe(this, "left/camera_info", image_sub_rmw_qos, sub_opts); - sub_r_image_.subscribe( - this, "right/image_rect", hints.getTransport(), image_sub_rmw_qos, sub_opts); - sub_r_info_.subscribe(this, "right/camera_info", image_sub_rmw_qos, sub_opts); + pub_disparity_ = create_publisher("disparity", 1, pub_opts); } void DisparityNode::imageCb( diff --git a/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp b/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp index ddbf752a0..1d7d0f232 100644 --- a/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp @@ -42,6 +42,7 @@ #include "message_filters/sync_policies/exact_time.h" #include "rcutils/logging_macros.h" +#include #include #include #include @@ -93,8 +94,6 @@ class PointCloudNode : public rclcpp::Node image_geometry::StereoCameraModel model_; cv::Mat_ points_mat_; // scratch buffer - void connectCb(); - void imageCb( const sensor_msgs::msg::Image::ConstSharedPtr & l_image_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & l_info_msg, @@ -107,6 +106,9 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options) { using namespace std::placeholders; + // TransportHints does not actually declare the parameter + this->declare_parameter("image_transport", "raw"); + // Declare/read parameters int queue_size = this->declare_parameter("queue_size", 5); bool approx = this->declare_parameter("approximate_sync", false); @@ -152,34 +154,55 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options) std::bind(&PointCloudNode::imageCb, this, _1, _2, _3, _4)); } - // Update the publisher options to allow reconfigurable qos settings. + // Publisher options to allow reconfigurable qos settings and connect callback rclcpp::PublisherOptions pub_opts; pub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - pub_points2_ = create_publisher("points2", 1, pub_opts); + pub_opts.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo & s) + { + if (s.current_count == 0) { + sub_l_image_.unsubscribe(); + sub_l_info_.unsubscribe(); + sub_r_info_.unsubscribe(); + sub_disparity_.unsubscribe(); + } else if (!sub_l_image_.getSubscriber()) { + // Optionally switch between system/sensor defaults + // TODO(fergs): remove and conform to REP-2003? + const bool use_system_default_qos = + this->get_parameter("use_system_default_qos").as_bool(); + rclcpp::QoS image_sub_qos = rclcpp::SensorDataQoS(); + if (use_system_default_qos) { + image_sub_qos = rclcpp::SystemDefaultsQoS(); + } + const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); - // TODO(jacobperron): Replace this with a graph event. - // Only subscribe if there's a subscription listening to our publisher. - connectCb(); -} + // For compressed topics to remap appropriately, we need to pass a + // fully expanded and remapped topic name to image_transport + auto node_base = this->get_node_base_interface(); + std::string left_topic = + node_base->resolve_topic_or_service_name("left/image_rect_color", false); + std::string right_topic = + node_base->resolve_topic_or_service_name("right/camera_info", false); + std::string disparity_topic = + node_base->resolve_topic_or_service_name("disparity", false); + // Allow also remapping camera_info to something different than default + std::string left_info_topic = + node_base->resolve_topic_or_service_name( + image_transport::getCameraInfoTopic(left_topic), false); -// Handles (un)subscribing when clients (un)subscribe -void PointCloudNode::connectCb() -{ - // TODO(jacobperron): Add unsubscribe logic when we use graph events - image_transport::TransportHints hints(this, "raw"); - const bool use_system_default_qos = this->get_parameter("use_system_default_qos").as_bool(); - rclcpp::QoS image_sub_qos = rclcpp::SensorDataQoS(); - if (use_system_default_qos) { - image_sub_qos = rclcpp::SystemDefaultsQoS(); - } - const auto image_sub_rmw_qos = image_sub_qos.get_rmw_qos_profile(); - auto sub_opts = rclcpp::SubscriptionOptions(); - sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - sub_l_image_.subscribe( - this, "left/image_rect_color", hints.getTransport(), image_sub_rmw_qos, sub_opts); - sub_l_info_.subscribe(this, "left/camera_info", image_sub_rmw_qos, sub_opts); - sub_r_info_.subscribe(this, "right/camera_info", image_sub_rmw_qos, sub_opts); - sub_disparity_.subscribe(this, "disparity", image_sub_rmw_qos, sub_opts); + // Setup hints and QoS overrides + image_transport::TransportHints hints(this); + auto sub_opts = rclcpp::SubscriptionOptions(); + sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + + sub_l_image_.subscribe( + this, left_topic, hints.getTransport(), image_sub_rmw_qos, sub_opts); + sub_l_info_.subscribe(this, left_info_topic, image_sub_rmw_qos, sub_opts); + sub_r_info_.subscribe(this, right_topic, image_sub_rmw_qos, sub_opts); + sub_disparity_.subscribe(this, disparity_topic, image_sub_rmw_qos, sub_opts); + } + }; + pub_points2_ = create_publisher("points2", 1, pub_opts); } inline bool isValidPoint(const cv::Vec3f & pt)