Skip to content

[depth_image_proc] using sensor data qos profile everywhere #1081

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.

Sign up for GitHub

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

Open
LorenzoFerriniCodes wants to merge 2 commits into
base: humble
Choose a base branch
from
Open

[depth_image_proc] using sensor data qos profile everywhere #1081

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e8ffd83
[depth_image_proc] using sensor data qos profile everywhere
Feb 21, 2025
4256b86
making linters happy
Feb 21, 2025
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
5 changes: 3 additions & 2 deletions depth_image_proc/src/convert_metric.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)
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");
pub_depth_ = image_transport::create_publisher(this, "image", rmw_qos_profile_sensor_data);
Copy link
Member

@mikeferguson mikeferguson Feb 21, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This line should be reverted - the strategy throughout image_pipeline is supposed to be:

  • All publishers should be SystemDefaultQOS with allowable overrides
  • All subscribers should be SensorDataQOS with allowable overrides

This follows REP-2003 and deals with the fact that a DefaultQOS publisher will publish best effort when a SensorQOS subscribes to it - but is entirely incompatible with a DefaultQOS subscriber.

}

// Handles (un)subscribing when clients (un)subscribe
Expand All @@ -92,7 +92,8 @@ void ConvertMetricNode::connectCb()
sub_raw_ = image_transport::create_subscription(
this, "image_raw",
std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1),
hints.getTransport());
hints.getTransport(),
rmw_qos_profile_sensor_data);
}
}

Expand Down
5 changes: 3 additions & 2 deletions depth_image_proc/src/crop_foremost.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options)
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");
pub_depth_ = image_transport::create_publisher(this, "image", rmw_qos_profile_sensor_data);
Copy link
Member

@mikeferguson mikeferguson Feb 21, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

also revert

}

// Handles (un)subscribing when clients (un)subscribe
Expand All @@ -97,7 +97,8 @@ void CropForemostNode::connectCb()
sub_raw_ = image_transport::create_subscription(
this, "image_raw",
std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1),
hints.getTransport());
hints.getTransport(),
rmw_qos_profile_sensor_data);
}
}

Expand Down
6 changes: 4 additions & 2 deletions depth_image_proc/src/disparity.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,8 +123,10 @@ void DisparityNode::connectCb()
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");
sub_depth_image_.subscribe(
this, "left/image_rect",
hints.getTransport(), rmw_qos_profile_sensor_data);
sub_info_.subscribe(this, "right/camera_info", rmw_qos_profile_sensor_data);
}
}

Expand Down
2 changes: 1 addition & 1 deletion depth_image_proc/src/point_cloud_xyz.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ void PointCloudXyzNode::connectCb()
if (0) {
sub_depth_.shutdown();
} else if (!sub_depth_) {
auto custom_qos = rmw_qos_profile_system_default;
auto custom_qos = rmw_qos_profile_sensor_data;
custom_qos.depth = queue_size_;

sub_depth_ = image_transport::create_camera_subscription(
Expand Down
10 changes: 7 additions & 3 deletions depth_image_proc/src/point_cloud_xyzi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,12 +98,16 @@ void PointCloudXyziNode::connectCb()

// 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());
sub_depth_.subscribe(
this, "depth/image_rect",
depth_hints.getTransport(), rmw_qos_profile_sensor_data);

// 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");
sub_intensity_.subscribe(
this, "intensity/image_rect",
hints.getTransport(), rmw_qos_profile_sensor_data);
sub_info_.subscribe(this, "intensity/camera_info", rmw_qos_profile_sensor_data);
}
}

Expand Down
10 changes: 7 additions & 3 deletions depth_image_proc/src/point_cloud_xyzi_radial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,12 +100,16 @@ void PointCloudXyziRadialNode::connectCb()

// 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());
sub_depth_.subscribe(
this, "depth/image_raw",
depth_hints.getTransport(), rmw_qos_profile_sensor_data);

// 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");
sub_intensity_.subscribe(
this, "intensity/image_raw",
hints.getTransport(), rmw_qos_profile_sensor_data);
sub_info_.subscribe(this, "intensity/camera_info", rmw_qos_profile_sensor_data);
}
}

Expand Down
6 changes: 3 additions & 3 deletions depth_image_proc/src/point_cloud_xyzrgb.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,14 +123,14 @@ void PointCloudXyzrgbNode::connectCb()
// depth image can use different transport.(e.g. compressedDepth)
sub_depth_.subscribe(
this, "depth_registered/image_rect",
depth_hints.getTransport(), rmw_qos_profile_default, sub_opts);
depth_hints.getTransport(), rmw_qos_profile_sensor_data, sub_opts);

// rgb uses normal ros transport hints.
image_transport::TransportHints hints(this, "raw");
sub_rgb_.subscribe(
this, "rgb/image_rect_color",
hints.getTransport(), rmw_qos_profile_default, sub_opts);
sub_info_.subscribe(this, "rgb/camera_info");
hints.getTransport(), rmw_qos_profile_sensor_data, sub_opts);
sub_info_.subscribe(this, "rgb/camera_info", rmw_qos_profile_sensor_data);
}
}

Expand Down
10 changes: 7 additions & 3 deletions depth_image_proc/src/point_cloud_xyzrgb_radial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,12 +115,16 @@ void PointCloudXyzrgbRadialNode::connectCb()
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());
sub_depth_.subscribe(
this, "depth_registered/image_rect",
depth_hints.getTransport(), rmw_qos_profile_sensor_data);

// 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");
sub_rgb_.subscribe(
this, "rgb/image_rect_color",
hints.getTransport(), rmw_qos_profile_sensor_data);
sub_info_.subscribe(this, "rgb/camera_info", rmw_qos_profile_sensor_data);
}
}

Expand Down
13 changes: 9 additions & 4 deletions depth_image_proc/src/register.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,9 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options)
// 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");
pub_registered_ = image_transport::create_camera_publisher(
this, "depth_registered/image_rect",
rmw_qos_profile_sensor_data);
Copy link
Member

@mikeferguson mikeferguson Feb 21, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

revert

}

// Handles (un)subscribing when clients (un)subscribe
Expand All @@ -147,9 +149,11 @@ void RegisterNode::connectCb()
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");
sub_depth_image_.subscribe(
this, "depth/image_rect",
hints.getTransport(), rmw_qos_profile_sensor_data);
sub_depth_info_.subscribe(this, "depth/camera_info", rmw_qos_profile_sensor_data);
sub_rgb_info_.subscribe(this, "rgb/camera_info", rmw_qos_profile_sensor_data);
}
}

Expand Down Expand Up @@ -192,6 +196,7 @@ void RegisterNode::imageCb(
registered_msg->width = resolution.width;
// step and data set in convert(), depend on depth data type


Copy link
Member

@mikeferguson mikeferguson Feb 21, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we can revert this extra newline

if (depth_image_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
convert<uint16_t>(depth_image_msg, registered_msg, depth_to_rgb);
} else if (depth_image_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
Expand Down