Skip to content

Commit

Permalink
make remaining components lazy (ros-perception#853)
Browse files Browse the repository at this point in the history 
missed a few components in ros-perception#815
  • Loading branch information
@mikeferguson
mikeferguson authored and Krzysztof Wojciechowski committed May 27, 2024
1 parent d1517db commit 8d12179
Show file tree
Hide file tree
Showing 3 changed files with 55 additions and 90 deletions.
47 changes: 18 additions & 29 deletions depth_image_proc/src/crop_foremost.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand All @@ -72,33 +70,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<std::mutex> 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<std::mutex> 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<std::mutex> 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)
Expand Down
45 changes: 16 additions & 29 deletions depth_image_proc/src/disparity.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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<std::mutex> lock(connect_mutex_);
// TODO(ros2) Implement when SubscriberStatusCallback is available
// pub_disparity_ =
// left_nh.advertise<stereo_msgs::DisparityImage>("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<std::mutex> 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<stereo_msgs::msg::DisparityImage>(
"left/disparity", rclcpp::SensorDataQoS());
}

// Handles (un)subscribing when clients (un)subscribe
void DisparityNode::connectCb()
{
std::lock_guard<std::mutex> 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(
Expand Down
53 changes: 21 additions & 32 deletions depth_image_proc/src/register.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,8 +84,6 @@ class RegisterNode : public rclcpp::Node
bool fill_upsampling_holes_;
bool use_rgb_timestamp_; // use source time stamp from RGB camera

void connectCb();

void imageCb(
const Image::ConstSharedPtr & depth_image_msg,
const CameraInfo::ConstSharedPtr & depth_info_msg,
Expand Down Expand Up @@ -121,36 +119,27 @@ 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<std::mutex> 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<std::mutex> 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<std::mutex> 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(
Expand Down

0 comments on commit 8d12179

Please sign in to comment.