Skip to content

Commit

Permalink
feat(pointcloud_preprocessor): runtime configurable output topic qos (a…
Browse files Browse the repository at this point in the history
…utowarefoundation#6658)

* feat(pointcloud_preprocessor): runtime configurable output topic qos

Signed-off-by: Grzegorz Głowacki <[email protected]>

* configurable qos in livox_tag_filter_node

Signed-off-by: Grzegorz Głowacki <[email protected]>

* configurable qos in radar_scan_to_pointcloud2

Signed-off-by: Grzegorz Głowacki <[email protected]>

---------

Signed-off-by: Grzegorz Głowacki <[email protected]>
  • Loading branch information
ralwing authored Jul 3, 2024
1 parent ced8e37 commit f1e4a9e
Show file tree
Hide file tree
Showing 11 changed files with 58 additions and 23 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -47,9 +47,13 @@ LivoxTagFilterNode::LivoxTagFilterNode(const rclcpp::NodeOptions & node_options)
sub_pointcloud_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"input", rclcpp::SensorDataQoS(), std::bind(&LivoxTagFilterNode::onPointCloud, this, _1));

// Publisher
pub_pointcloud_ =
this->create_publisher<sensor_msgs::msg::PointCloud2>("output", rclcpp::SensorDataQoS());
{
// Publisher
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_pointcloud_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
"output", rclcpp::SensorDataQoS(), pub_options);
}
}

void LivoxTagFilterNode::onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -141,8 +141,10 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro

// Output Publishers
{
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_output_ = this->create_publisher<PointCloud2>(
"output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_));
"output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options);
}

// Subscribers
Expand Down Expand Up @@ -192,10 +194,13 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro

// Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud
if (publish_synchronized_pointcloud_) {
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();

for (auto & topic : input_topics_) {
std::string new_topic = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix_);
auto publisher = this->create_publisher<sensor_msgs::msg::PointCloud2>(
new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_));
new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options);
transformed_raw_pc_publisher_map_.insert({topic, publisher});
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -97,8 +97,10 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent(

// Output Publishers
{
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_output_ = this->create_publisher<PointCloud2>(
"output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_));
"output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options);
}

// Subscribers
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -87,8 +87,10 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio

// set additional publishers
{
crop_box_polygon_pub_ =
this->create_publisher<geometry_msgs::msg::PolygonStamped>("~/crop_box_polygon", 10);
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
crop_box_polygon_pub_ = this->create_publisher<geometry_msgs::msg::PolygonStamped>(
"~/crop_box_polygon", 10, pub_options);
}

// set parameter service callback
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,13 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp");
use_imu_ = declare_parameter("use_imu", true);

// Publisher
undistorted_points_pub_ =
this->create_publisher<PointCloud2>("~/output/pointcloud", rclcpp::SensorDataQoS());

{
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
// Publisher
undistorted_points_pub_ = this->create_publisher<PointCloud2>(
"~/output/pointcloud", rclcpp::SensorDataQoS(), pub_options);
}
// Subscriber
twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
"~/input/twist", 10,
Expand Down
4 changes: 3 additions & 1 deletion sensing/pointcloud_preprocessor/src/filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -89,8 +89,10 @@ pointcloud_preprocessor::Filter::Filter(

// Set publisher
{
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_output_ = this->create_publisher<PointCloud2>(
"output", rclcpp::SensorDataQoS().keep_last(max_queue_size_));
"output", rclcpp::SensorDataQoS().keep_last(max_queue_size_), pub_options);
}

subscribe(filter_name);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -72,9 +72,12 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent(
image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image");
visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS());
noise_cloud_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>(
"dual_return_outlier_filter/debug/pointcloud_noise", rclcpp::SensorDataQoS());

{
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
noise_cloud_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>(
"dual_return_outlier_filter/debug/pointcloud_noise", rclcpp::SensorDataQoS(), pub_options);
}
using std::placeholders::_1;
set_param_res_ = this->add_on_set_parameters_callback(
std::bind(&DualReturnOutlierFilterComponent::paramCallback, this, _1));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,12 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
using autoware::universe_utils::StopWatch;
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
outlier_pointcloud_publisher_ =
this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1);
{
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
outlier_pointcloud_publisher_ =
this->create_publisher<PointCloud2>("debug/ring_outlier_filter", 1, pub_options);
}
visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS());
stop_watch_ptr_->tic("cyclic_time");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -160,10 +160,12 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(

// Transformed Raw PointCloud2 Publisher
{
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
for (auto & topic : input_topics_) {
std::string new_topic = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix);
auto publisher = this->create_publisher<sensor_msgs::msg::PointCloud2>(
new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_));
new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options);
transformed_raw_pc_publisher_map_.insert({topic, publisher});
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,10 @@ Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions

// Set publisher
{
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
filtered_pointcloud_pub_ =
this->create_publisher<PointCloud2>("output", rclcpp::SensorDataQoS());
this->create_publisher<PointCloud2>("output", rclcpp::SensorDataQoS(), pub_options);
}

// Set subscriber
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -111,9 +111,15 @@ RadarScanToPointcloud2Node::RadarScanToPointcloud2Node(const rclcpp::NodeOptions
sub_radar_ = create_subscription<RadarScan>(
"~/input/radar", rclcpp::QoS{1}, std::bind(&RadarScanToPointcloud2Node::onData, this, _1));

// Publisher
pub_amplitude_pointcloud_ = create_publisher<PointCloud2>("~/output/amplitude_pointcloud", 1);
pub_doppler_pointcloud_ = create_publisher<PointCloud2>("~/output/doppler_pointcloud", 1);
{
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
// Publisher
pub_amplitude_pointcloud_ =
create_publisher<PointCloud2>("~/output/amplitude_pointcloud", 1, pub_options);
pub_doppler_pointcloud_ =
create_publisher<PointCloud2>("~/output/doppler_pointcloud", 1, pub_options);
}
}

rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam(
Expand Down

0 comments on commit f1e4a9e

Please sign in to comment.