Skip to content

Commit

Permalink
stereo_image_proc: consistent image_transport (#903)
Browse files Browse the repository at this point in the history 
* make image_transport work
* make remap work as expected with image_transport
* make subscribers lazy
  • Loading branch information
@mikeferguson
mikeferguson authored Jan 19, 2024
1 parent 135e07f commit 3dbd2b4
Show file tree
Hide file tree
Showing 2 changed files with 102 additions and 54 deletions.
81 changes: 53 additions & 28 deletions stereo_image_proc/src/stereo_image_proc/disparity_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@

#include <stereo_image_proc/stereo_processor.hpp>

#include <image_transport/camera_common.hpp>
#include <image_transport/image_transport.hpp>
#include <image_transport/subscriber_filter.hpp>
#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -163,6 +162,9 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
{
using namespace std::placeholders;

// TransportHints does not actually declare the parameter
this->declare_parameter<std::string>("image_transport", "raw");

// Declare/read parameters
int queue_size = this->declare_parameter("queue_size", 5);
bool approx = this->declare_parameter("approximate_sync", false);
Expand Down Expand Up @@ -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<stereo_msgs::msg::DisparityImage>("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<stereo_msgs::msg::DisparityImage>("disparity", 1, pub_opts);
}

void DisparityNode::imageCb(
Expand Down
75 changes: 49 additions & 26 deletions stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include "message_filters/sync_policies/exact_time.h"
#include "rcutils/logging_macros.h"

#include <image_transport/camera_common.hpp>
#include <image_transport/image_transport.hpp>
#include <image_transport/subscriber_filter.hpp>
#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -93,8 +94,6 @@ class PointCloudNode : public rclcpp::Node
image_geometry::StereoCameraModel model_;
cv::Mat_<cv::Vec3f> 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,
Expand All @@ -107,6 +106,9 @@ PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options)
{
using namespace std::placeholders;

// TransportHints does not actually declare the parameter
this->declare_parameter<std::string>("image_transport", "raw");

// Declare/read parameters
int queue_size = this->declare_parameter("queue_size", 5);
bool approx = this->declare_parameter("approximate_sync", false);
Expand Down Expand Up @@ -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<sensor_msgs::msg::PointCloud2>("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<sensor_msgs::msg::PointCloud2>("points2", 1, pub_opts);
}

inline bool isValidPoint(const cv::Vec3f & pt)
Expand Down

0 comments on commit 3dbd2b4

Please sign in to comment.