Skip to content

Support image_transport lifecycle #1099

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 3 commits into
base: rolling
Choose a base branch
from
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 5 additions & 3 deletions depth_image_proc/src/convert_metric.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -83,11 +83,12 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("image_raw", false);
// Get transport hints
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, topic);
sub_raw_ = image_transport::create_subscription(
this, topic,
*this,
topic,
std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1),
hints.getTransport(),
qos_profile);
Expand All @@ -100,7 +101,8 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)

// Create publisher - allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_depth_ = image_transport::create_publisher(this, topic, rmw_qos_profile_default,
pub_depth_ = image_transport::create_publisher(*this, topic,
rmw_qos_profile_default,
pub_options);
}

Expand Down
8 changes: 5 additions & 3 deletions depth_image_proc/src/crop_foremost.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -90,11 +90,12 @@ CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options)
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("image_raw", false);
// Get transport hints
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
// Create publisher with same QoS as subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, topic);
sub_raw_ = image_transport::create_subscription(
this, topic,
*this,
topic,
std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1),
hints.getTransport(),
qos_profile);
Expand All @@ -107,7 +108,8 @@ CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options)

// Create publisher - allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_depth_ = image_transport::create_publisher(this, topic, rmw_qos_profile_default,
pub_depth_ = image_transport::create_publisher(*this, topic,
rmw_qos_profile_default,
pub_options);
}

Expand Down
5 changes: 3 additions & 2 deletions depth_image_proc/src/disparity.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -114,8 +114,9 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("left/image_rect", false);
image_transport::TransportHints hints(this);
sub_depth_image_.subscribe(this, topic, hints.getTransport());
image_transport::TransportHints hints{*this};
sub_depth_image_.subscribe(*this, topic,
hints.getTransport());
sub_info_.subscribe(this, "right/camera_info", rclcpp::QoS(10));
}
};
Expand Down
5 changes: 3 additions & 2 deletions depth_image_proc/src/point_cloud_xyz.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -77,14 +77,15 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
std::string topic = node_base->resolve_topic_or_service_name("image_rect", false);

// Get transport hints
image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
image_transport::TransportHints depth_hints(*this,
"raw", "depth_image_transport");

// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, topic);
qos_profile.depth = queue_size_;

sub_depth_ = image_transport::create_camera_subscription(
this,
*this,
topic,
std::bind(
&PointCloudXyzNode::depthCb, this, std::placeholders::_1,
Expand Down
5 changes: 3 additions & 2 deletions depth_image_proc/src/point_cloud_xyz_radial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,13 +71,14 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("depth/image_raw", false);
// Get transport hints
image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
image_transport::TransportHints depth_hints(*this,
"raw", "depth_image_transport");
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, topic);
qos_profile.depth = queue_size_;
// Create subscriber
sub_depth_ = image_transport::create_camera_subscription(
this,
*this,
topic,
std::bind(
&PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1,
Expand Down
11 changes: 7 additions & 4 deletions depth_image_proc/src/point_cloud_xyzi.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,12 +102,15 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options)
image_transport::getCameraInfoTopic(intensity_topic), false);

// depth image can use different transport.(e.g. compressedDepth)
image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
sub_depth_.subscribe(this, depth_topic, depth_hints.getTransport());
image_transport::TransportHints depth_hints(*this,
"raw", "depth_image_transport");
sub_depth_.subscribe(*this, depth_topic,
depth_hints.getTransport());

// intensity uses normal ros transport hints.
image_transport::TransportHints hints(this, "raw");
sub_intensity_.subscribe(this, intensity_topic, hints.getTransport());
image_transport::TransportHints hints(*this, "raw");
sub_intensity_.subscribe(*this, intensity_topic,
hints.getTransport());
sub_info_.subscribe(this, intensity_info_topic, rclcpp::QoS(10));
}
};
Expand Down
11 changes: 7 additions & 4 deletions depth_image_proc/src/point_cloud_xyzi_radial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -98,16 +98,19 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o
image_transport::getCameraInfoTopic(intensity_topic), false);

// depth image can use different transport.(e.g. compressedDepth)
image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
image_transport::TransportHints depth_hints(*this,
"raw", "depth_image_transport");
// Create subscriber with QoS matched to subscribed topic publisher
auto depth_qos_profile = image_proc::getTopicQosProfile(this, depth_topic);
sub_depth_.subscribe(this, depth_topic, depth_hints.getTransport(), depth_qos_profile);
sub_depth_.subscribe(*this, depth_topic,
depth_hints.getTransport(), depth_qos_profile);

// intensity uses normal ros transport hints.
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, intensity_topic);
sub_intensity_.subscribe(this, intensity_topic, hints.getTransport(), qos_profile);
sub_intensity_.subscribe(*this, intensity_topic,
hints.getTransport(), qos_profile);
sub_info_.subscribe(this, intensity_info_topic, rclcpp::QoS(10));
}
};
Expand Down
10 changes: 6 additions & 4 deletions depth_image_proc/src/point_cloud_xyzrgb.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,8 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)
image_transport::getCameraInfoTopic(rgb_topic), false);

// parameter for depth_image_transport hint
image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
image_transport::TransportHints depth_hints(*this,
"raw", "depth_image_transport");

rclcpp::SubscriptionOptions sub_opts;
// Update the subscription options to allow reconfigurable qos settings.
Expand All @@ -126,13 +127,14 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)

// depth image can use different transport.(e.g. compressedDepth)
sub_depth_.subscribe(
this, depth_topic,
*this, depth_topic,
depth_hints.getTransport(), rmw_qos_profile_default, sub_opts);

// rgb uses normal ros transport hints.
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
sub_rgb_.subscribe(
this, rgb_topic,
*this,
rgb_topic,
hints.getTransport(),
rmw_qos_profile_default, sub_opts);
sub_info_.subscribe(this, rgb_info_topic, rclcpp::QoS(10));
Expand Down
11 changes: 7 additions & 4 deletions depth_image_proc/src/point_cloud_xyzrgb_radial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,12 +112,15 @@ PointCloudXyzrgbRadialNode::PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions
image_transport::getCameraInfoTopic(rgb_topic), false);

// depth image can use different transport.(e.g. compressedDepth)
image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
sub_depth_.subscribe(this, depth_topic, depth_hints.getTransport());
image_transport::TransportHints depth_hints(*this,
"raw", "depth_image_transport");
sub_depth_.subscribe(*this, depth_topic,
depth_hints.getTransport());

// rgb uses normal ros transport hints.
image_transport::TransportHints hints(this, "raw");
sub_rgb_.subscribe(this, rgb_topic, hints.getTransport());
image_transport::TransportHints hints(*this, "raw");
sub_rgb_.subscribe(*this, rgb_topic,
hints.getTransport());
sub_info_.subscribe(this, rgb_info_topic, rclcpp::QoS(10));
}
};
Expand Down
8 changes: 5 additions & 3 deletions depth_image_proc/src/register.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -141,8 +141,10 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options)
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("depth/image_rect", false);
image_transport::TransportHints hints(this, "raw", "depth_image_transport");
sub_depth_image_.subscribe(this, topic, hints.getTransport(), rmw_qos_profile_default,
image_transport::TransportHints hints(*this, "raw",
"depth_image_transport");
sub_depth_image_.subscribe(*this, topic,
hints.getTransport(), rmw_qos_profile_default,
sub_options);
sub_depth_info_.subscribe(this, "depth/camera_info", rclcpp::QoS(10), sub_options);
sub_rgb_info_.subscribe(this, "rgb/camera_info", rclcpp::QoS(10), sub_options);
Expand All @@ -158,7 +160,7 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_registered_ =
image_transport::create_camera_publisher(
this, topic,
*this, topic,
rmw_qos_profile_default, pub_options);
}

Expand Down
7 changes: 4 additions & 3 deletions image_proc/src/crop_decimate.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -144,17 +144,18 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
} else if (!sub_) {
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
sub_ = image_transport::create_camera_subscription(
this, image_topic_, std::bind(
*this, image_topic_, std::bind(
&CropDecimateNode::imageCb, this,
std::placeholders::_1, std::placeholders::_2), hints.getTransport(), qos_profile);
}
};

// Create publisher - allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_ = image_transport::create_camera_publisher(this, pub_topic, rmw_qos_profile_default,
pub_ = image_transport::create_camera_publisher(*this,
pub_topic, rmw_qos_profile_default,
pub_options);
}

Expand Down
7 changes: 4 additions & 3 deletions image_proc/src/crop_non_zero.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -72,17 +72,18 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)
} else if (!sub_raw_) {
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
sub_raw_ = image_transport::create_subscription(
this, image_topic_, std::bind(
*this, image_topic_, std::bind(
&CropNonZeroNode::imageCb, this,
std::placeholders::_1), hints.getTransport(), qos_profile);
}
};

// 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);
pub_ = image_transport::create_publisher(*this, pub_topic,
rmw_qos_profile_default, pub_options);
}

void CropNonZeroNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)
Expand Down
10 changes: 6 additions & 4 deletions image_proc/src/debayer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -75,9 +75,9 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
} else if (!sub_raw_) {
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
sub_raw_ = image_transport::create_subscription(
this, image_topic_,
*this, image_topic_,
std::bind(
&DebayerNode::imageCb, this,
std::placeholders::_1), hints.getTransport(), qos_profile);
Expand All @@ -86,9 +86,11 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)

// Create publisher - allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_mono_ = image_transport::create_publisher(this, mono_topic, rmw_qos_profile_default,
pub_mono_ = image_transport::create_publisher(*this,
mono_topic, rmw_qos_profile_default,
pub_options);
pub_color_ = image_transport::create_publisher(this, color_topic, rmw_qos_profile_default,
pub_color_ = image_transport::create_publisher(*this,
color_topic, rmw_qos_profile_default,
pub_options);
}

Expand Down
7 changes: 4 additions & 3 deletions image_proc/src/rectify.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -73,17 +73,18 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options)
} else if (!sub_camera_) {
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
sub_camera_ = image_transport::create_camera_subscription(
this, image_topic_, std::bind(
*this, image_topic_, std::bind(
&RectifyNode::imageCb,
this, std::placeholders::_1, std::placeholders::_2), hints.getTransport(), qos_profile);
}
};

// Create publisher - allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_rect_ = image_transport::create_publisher(this, "image_rect", rmw_qos_profile_default,
pub_rect_ = image_transport::create_publisher(*this,
"image_rect", rmw_qos_profile_default,
pub_options);
}

Expand Down
7 changes: 4 additions & 3 deletions image_proc/src/resize.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,9 +79,9 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)
} else if (!sub_image_) {
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
sub_image_ = image_transport::create_camera_subscription(
this, image_topic_,
*this, image_topic_,
std::bind(
&ResizeNode::imageCb, this,
std::placeholders::_1,
Expand All @@ -92,7 +92,8 @@ ResizeNode::ResizeNode(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_ =
image_transport::create_camera_publisher(this, pub_topic, rmw_qos_profile_default, pub_options);
image_transport::create_camera_publisher(*this, pub_topic,
rmw_qos_profile_default, pub_options);
}

void ResizeNode::imageCb(
Expand Down
4 changes: 2 additions & 2 deletions image_proc/src/track_marker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -87,9 +87,9 @@ TrackMarkerNode::TrackMarkerNode(const rclcpp::NodeOptions & options)
} else if (!sub_camera_) {
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
image_transport::TransportHints hints(this);
image_transport::TransportHints hints{*this};
sub_camera_ = image_transport::create_camera_subscription(
this, image_topic_, std::bind(
*this, image_topic_, std::bind(
&TrackMarkerNode::imageCb,
this, std::placeholders::_1, std::placeholders::_2),
hints.getTransport(), qos_profile);
Expand Down
2 changes: 1 addition & 1 deletion image_proc/test/rostest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ class ImageProcTest
}

// Create raw camera publisher
image_transport::ImageTransport it(this->node);
image_transport::ImageTransport it{*node};
cam_pub = it.advertiseCamera(topic_raw, 1);

while (true) {
Expand Down
4 changes: 2 additions & 2 deletions image_proc/test/test_rectify.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ class ImageProcRectifyTest
});

// Create raw camera subscriber and publisher
image_transport::ImageTransport it(node);
image_transport::ImageTransport it{*node};
cam_pub_ = it.advertiseCamera(topic_raw_, 1);
}

Expand Down Expand Up @@ -194,7 +194,7 @@ class ImageProcRectifyTest
TEST_F(ImageProcRectifyTest, rectifyTest)
{
RCLCPP_INFO(node->get_logger(), "In test. Subscribing.");
image_transport::ImageTransport it(node);
image_transport::ImageTransport it{*node};
cam_sub_ = it.subscribe(
topic_rect_, rclcpp::SensorDataQoS().get_rmw_qos_profile(),
&ImageProcRectifyTest::imageCallback,
Expand Down
12 changes: 9 additions & 3 deletions image_publisher/src/image_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,8 +66,11 @@ ImagePublisher::ImagePublisher(
std::string topic_name = node_base->resolve_topic_or_service_name("image_raw", false);
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_ = image_transport::create_camera_publisher(this, topic_name, rmw_qos_profile_default,
pub_options);
pub_ = image_transport::create_camera_publisher(
*this,
topic_name,
rmw_qos_profile_default,
pub_options);

field_of_view_ = this->declare_parameter("field_of_view", static_cast<double>(0));
flip_horizontal_ = this->declare_parameter("flip_horizontal", false);
Expand Down Expand Up @@ -137,7 +140,10 @@ void ImagePublisher::reconfigureCallback()
std::chrono::milliseconds(static_cast<int>(1000 / publish_rate_)),
std::bind(&ImagePublisher::doWork, this));

camera_info_manager::CameraInfoManager c(this);
camera_info_manager::CameraInfoManager c(
this->get_node_base_interface(),
this->node_services_interface(),
this->get_logger_logging_interfac());
if (!camera_info_url_.empty()) {
RCLCPP_INFO(get_logger(), "camera_info_url: %s", camera_info_url_.c_str());
try {
Expand Down
Loading