Skip to content

Commit 523b084

Browse files
authored
feat(pointcloud_preprocessor): make concatenate node to publish pointclouds in sensor frame (#6586)
* feat: make concatenate node to publish pointclouds in sensor frame Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * chore: disable frame transform if not necessary Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
1 parent 7e09808 commit 523b084

File tree

4 files changed

+50
-21
lines changed

4 files changed

+50
-21
lines changed

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -126,6 +126,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
126126
double timeout_sec_ = 0.1;
127127

128128
bool publish_synchronized_pointcloud_;
129+
bool keep_input_frame_in_synchronized_pointcloud_;
129130
std::string synchronized_pointcloud_postfix_;
130131

131132
std::set<std::string> not_subscribed_topic_names_;

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node
138138

139139
/** \brief Output TF frame the concatenated points should be transformed to. */
140140
std::string output_frame_;
141+
bool keep_input_frame_in_synchronized_pointcloud_;
141142

142143
/** \brief Input point cloud topics. */
143144
// XmlRpc::XmlRpcValue input_topics_;

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp

+33-19
Original file line numberDiff line numberDiff line change
@@ -112,7 +112,9 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
112112
}
113113

114114
// Check if publish synchronized pointcloud
115-
publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false);
115+
publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", true);
116+
keep_input_frame_in_synchronized_pointcloud_ =
117+
declare_parameter("keep_input_frame_in_synchronized_pointcloud", true);
116118
synchronized_pointcloud_postfix_ =
117119
declare_parameter("synchronized_pointcloud_postfix", "pointcloud");
118120
}
@@ -397,10 +399,23 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
397399
pcl::concatenatePointCloud(
398400
*concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr);
399401
}
400-
// gather transformed clouds
401-
transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp;
402-
transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;
403-
transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr;
402+
// convert to original sensor frame if necessary
403+
bool need_transform_to_sensor_frame = (e.second->header.frame_id != output_frame_);
404+
if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) {
405+
sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr_in_sensor_frame(
406+
new sensor_msgs::msg::PointCloud2());
407+
pcl_ros::transformPointCloud(
408+
(std::string)e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr,
409+
*transformed_cloud_ptr_in_sensor_frame, *tf2_buffer_);
410+
transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp;
411+
transformed_cloud_ptr_in_sensor_frame->header.frame_id = e.second->header.frame_id;
412+
transformed_clouds[e.first] = transformed_cloud_ptr_in_sensor_frame;
413+
} else {
414+
transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp;
415+
transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;
416+
transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr;
417+
}
418+
404419
} else {
405420
not_subscribed_topic_names_.insert(e.first);
406421
}
@@ -418,20 +433,6 @@ void PointCloudConcatenateDataSynchronizerComponent::publish()
418433
const auto & transformed_raw_points =
419434
PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr);
420435

421-
for (const auto & e : cloud_stdmap_) {
422-
if (e.second != nullptr) {
423-
if (debug_publisher_) {
424-
const auto pipeline_latency_ms =
425-
std::chrono::duration<double, std::milli>(
426-
std::chrono::nanoseconds(
427-
(this->get_clock()->now() - e.second->header.stamp).nanoseconds()))
428-
.count();
429-
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
430-
"debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms);
431-
}
432-
}
433-
}
434-
435436
// publish concatenated pointcloud
436437
if (concat_cloud_ptr) {
437438
auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*concat_cloud_ptr);
@@ -469,6 +470,19 @@ void PointCloudConcatenateDataSynchronizerComponent::publish()
469470
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
470471
"debug/processing_time_ms", processing_time_ms);
471472
}
473+
for (const auto & e : cloud_stdmap_) {
474+
if (e.second != nullptr) {
475+
if (debug_publisher_) {
476+
const auto pipeline_latency_ms =
477+
std::chrono::duration<double, std::milli>(
478+
std::chrono::nanoseconds(
479+
(this->get_clock()->now() - e.second->header.stamp).nanoseconds()))
480+
.count();
481+
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
482+
"debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms);
483+
}
484+
}
485+
}
472486
}
473487

474488
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp

+15-2
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,9 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
5858
std::string synchronized_pointcloud_postfix;
5959
{
6060
output_frame_ = static_cast<std::string>(declare_parameter("output_frame", ""));
61-
if (output_frame_.empty()) {
61+
keep_input_frame_in_synchronized_pointcloud_ =
62+
static_cast<bool>(declare_parameter("keep_input_frame_in_synchronized_pointcloud", false));
63+
if (output_frame_.empty() && !keep_input_frame_in_synchronized_pointcloud_) {
6264
RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!");
6365
return;
6466
}
@@ -351,7 +353,18 @@ PointCloudDataSynchronizerComponent::synchronizeClouds()
351353
pcl_ros::transformPointCloud(
352354
adjust_to_old_data_transform, *transformed_cloud_ptr,
353355
*transformed_delay_compensated_cloud_ptr);
354-
356+
// transform to sensor frame if needed
357+
bool need_transform_to_sensor_frame = (e.second->header.frame_id != output_frame_);
358+
if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) {
359+
sensor_msgs::msg::PointCloud2::SharedPtr
360+
transformed_delay_compensated_cloud_ptr_in_input_frame(
361+
new sensor_msgs::msg::PointCloud2());
362+
transformPointCloud(
363+
transformed_delay_compensated_cloud_ptr,
364+
transformed_delay_compensated_cloud_ptr_in_input_frame, e.second->header.frame_id);
365+
transformed_delay_compensated_cloud_ptr =
366+
transformed_delay_compensated_cloud_ptr_in_input_frame;
367+
}
355368
// gather transformed clouds
356369
transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp;
357370
transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;

0 commit comments

Comments
 (0)