From e86007b83466395a041b57383ce47071ad70a82e Mon Sep 17 00:00:00 2001 From: Ilia Sevostianov Date: Sat, 5 Jul 2025 11:58:12 +0300 Subject: [PATCH 1/3] Add time threshold parameter and implement timestamp handling for pointclouds in concatenation process --- .../pointcloud_concatenate.hpp | 11 +- launch/concat.launch | 3 +- src/pointcloud_concatenate.cpp | 134 ++++++++---------- 3 files changed, 73 insertions(+), 75 deletions(-) diff --git a/include/pointcloud_concatenate/pointcloud_concatenate.hpp b/include/pointcloud_concatenate/pointcloud_concatenate.hpp index 7c3302e..695b449 100644 --- a/include/pointcloud_concatenate/pointcloud_concatenate.hpp +++ b/include/pointcloud_concatenate/pointcloud_concatenate.hpp @@ -33,7 +33,7 @@ class PointcloudConcatenate { double getHz(); // Public variables and objects - + private: // Private functions void subCallbackCloudIn1(sensor_msgs::PointCloud2 msg); @@ -41,7 +41,8 @@ class PointcloudConcatenate { void subCallbackCloudIn3(sensor_msgs::PointCloud2 msg); void subCallbackCloudIn4(sensor_msgs::PointCloud2 msg); void publishPointcloud(sensor_msgs::PointCloud2 cloud); - + bool checkTimeThreshold(ros::Time& reference_time, ros::Time& current_time, const int& cloud_index); + void concatenate_with_reference_cloud(sensor_msgs::PointCloud2& reference_cloud, const sensor_msgs::PointCloud2 cloud_to_concat, bool& success, bool& cloud_received_recent, ros::Time& reference_time, ros::Time& current_time, const int& cloud_index, const bool& update_reference_time = false); // Private variables and objects ros::NodeHandle nh_; std::string node_name_; @@ -50,7 +51,7 @@ class PointcloudConcatenate { std::string param_frame_target_; int param_clouds_; double param_hz_; - + double param_time_threshold_; // Publisher and subscribers ros::Subscriber sub_cloud_in1 = nh_.subscribe("cloud_in1", 1, &PointcloudConcatenate::subCallbackCloudIn1, this); ros::Subscriber sub_cloud_in2 = nh_.subscribe("cloud_in2", 1, &PointcloudConcatenate::subCallbackCloudIn2, this); @@ -73,6 +74,10 @@ class PointcloudConcatenate { bool cloud_in2_received_recent = false; bool cloud_in3_received_recent = false; bool cloud_in4_received_recent = false; + ros::Time cloud_in1_timestamp = ros::Time::now(); + ros::Time cloud_in2_timestamp = ros::Time::now(); + ros::Time cloud_in3_timestamp = ros::Time::now(); + ros::Time cloud_in4_timestamp = ros::Time::now(); // Initialization tf2 listener boost::shared_ptr tfBuffer; diff --git a/launch/concat.launch b/launch/concat.launch index eea22bc..8f47d09 100644 --- a/launch/concat.launch +++ b/launch/concat.launch @@ -9,9 +9,10 @@ - + + diff --git a/src/pointcloud_concatenate.cpp b/src/pointcloud_concatenate.cpp index 5f078a6..a0a6426 100644 --- a/src/pointcloud_concatenate.cpp +++ b/src/pointcloud_concatenate.cpp @@ -32,24 +32,32 @@ void PointcloudConcatenate::subCallbackCloudIn1(sensor_msgs::PointCloud2 msg) { cloud_in1 = msg; cloud_in1_received = true; cloud_in1_received_recent = true; + // Also, get the timestamp + cloud_in1_timestamp = msg.header.stamp; } void PointcloudConcatenate::subCallbackCloudIn2(sensor_msgs::PointCloud2 msg) { cloud_in2 = msg; cloud_in2_received = true; cloud_in2_received_recent = true; + // Also, get the timestamp + cloud_in2_timestamp = msg.header.stamp; } void PointcloudConcatenate::subCallbackCloudIn3(sensor_msgs::PointCloud2 msg) { cloud_in3 = msg; cloud_in3_received = true; cloud_in3_received_recent = true; + // Also, get the timestamp + cloud_in3_timestamp = msg.header.stamp; } void PointcloudConcatenate::subCallbackCloudIn4(sensor_msgs::PointCloud2 msg) { cloud_in4 = msg; cloud_in4_received = true; cloud_in4_received_recent = true; + // Also, get the timestamp + cloud_in4_timestamp = msg.header.stamp; } void PointcloudConcatenate::handleParams() { @@ -83,9 +91,56 @@ void PointcloudConcatenate::handleParams() { ROSPARAM_WARN(param_name, param_hz_); } + // Time threshold for skipping old pointclouds + param_name = node_name_ + "/time_threshold"; + if (!ros::param::get(param_name, param_time_threshold_)) { + param_time_threshold_ = 0.1; + ROSPARAM_WARN(param_name, param_time_threshold_); + } + ROS_INFO("Parameters loaded."); } +bool PointcloudConcatenate::checkTimeThreshold(ros::Time& reference_time, ros::Time& current_time, const int& cloud_index) { + double timestamp_diff = std::abs((current_time - reference_time).toSec()); + if (timestamp_diff > param_time_threshold_) { + ROS_WARN("Cloud %d is too old [%.3f s], skipping...", cloud_index, timestamp_diff); + return true; + } + return false; +} + +void PointcloudConcatenate::concatenate_with_reference_cloud(sensor_msgs::PointCloud2& reference_cloud, const sensor_msgs::PointCloud2 cloud_to_concat, bool& success, bool& cloud_received_recent, ros::Time& reference_time, ros::Time& current_time, const int& cloud_index, const bool& update_reference_time) { + if (param_clouds_ >= cloud_index && success && cloud_received_recent) { + // Warn if cloud was not received since last update + if (!cloud_received_recent) { + ROS_WARN("Cloud was not received since last update, reusing last received message..."); + } + cloud_received_recent = false; + bool success_time_threshold = false; + if (update_reference_time) { + success_time_threshold = true; + } else { + success_time_threshold = this->checkTimeThreshold(reference_time, current_time, cloud_index); + } + if (success_time_threshold) { + // Transform pointcloud to the target frame + success = pcl_ros::transformPointCloud(param_frame_target_, cloud_to_concat, reference_cloud, *tfBuffer); + if (!success) { + ROS_WARN("Transforming cloud %d from %s to %s failed!", cloud_index, cloud_to_concat.header.frame_id.c_str(), param_frame_target_.c_str()); + } + + // Concatenate the pointcloud + if (success) { + pcl::concatenatePointCloud(reference_cloud, cloud_to_concat, reference_cloud); + if (update_reference_time) { + reference_time = current_time; + } + } + } + } +} + double PointcloudConcatenate::getHz() { return param_hz_; } @@ -97,7 +152,7 @@ void PointcloudConcatenate::update() { // Initialise pointclouds sensor_msgs::PointCloud2 cloud_to_concat; cloud_out = cloud_to_concat; // Clear the output pointcloud - + // Track success of transforms bool success = true; @@ -114,83 +169,20 @@ void PointcloudConcatenate::update() { return; } - + // Set reference time to the current time + ros::Time reference_time = ros::Time::now(); + // Concatenate the first pointcloud - if (param_clouds_ >= 1 && success && cloud_in1_received) { - // Warn if cloud was not received since last update - if (!cloud_in1_received_recent) { - ROS_WARN("Cloud 1 was not received since last update, reusing last received message..."); - } - cloud_in1_received_recent = false; - - // Transform pointcloud to the target frame - // Here we just assign the pointcloud directly to the output to ensure the secondary - // data is inherited correctly. - success = pcl_ros::transformPointCloud(param_frame_target_, cloud_in1, cloud_out, *tfBuffer); - if (!success) { - ROS_WARN("Transforming cloud 1 from %s to %s failed!", cloud_in1.header.frame_id.c_str(), param_frame_target_.c_str()); - } - } + this->concatenate_with_reference_cloud(cloud_out, cloud_in1, success, cloud_in1_received_recent, reference_time, cloud_in1_timestamp, 1, true); // Concatenate the second pointcloud - if (param_clouds_ >= 2 && success && cloud_in2_received) { - // Warn if cloud was not received since last update - if (!cloud_in2_received_recent) { - ROS_WARN("Cloud 2 was not received since last update, reusing last received message..."); - } - cloud_in2_received_recent = false; - - // Transform pointcloud to the target frame - success = pcl_ros::transformPointCloud(param_frame_target_, cloud_in2, cloud_to_concat, *tfBuffer); - if (!success) { - ROS_WARN("Transforming cloud 2 from %s to %s failed!", cloud_in2.header.frame_id.c_str(), param_frame_target_.c_str()); - } - - // Concatenate the pointcloud - if (success) { - pcl::concatenatePointCloud(cloud_out, cloud_to_concat, cloud_out); - } - } + this->concatenate_with_reference_cloud(cloud_out, cloud_in2, success, cloud_in2_received_recent, reference_time, cloud_in2_timestamp, 2, false); // Concatenate the third pointcloud - if (param_clouds_ >= 3 && success && cloud_in3_received) { - // Warn if cloud was not received since last update - if (!cloud_in3_received_recent) { - ROS_WARN("Cloud 3 was not received since last update, reusing last received message..."); - } - cloud_in3_received_recent = false; - - // Transform pointcloud to the target frame - success = pcl_ros::transformPointCloud(param_frame_target_, cloud_in3, cloud_to_concat, *tfBuffer); - if (!success) { - ROS_WARN("Transforming cloud 3 from %s to %s failed!", cloud_in3.header.frame_id.c_str(), param_frame_target_.c_str()); - } - - // Concatenate the pointcloud - if (success) { - pcl::concatenatePointCloud(cloud_out, cloud_to_concat, cloud_out); - } - } + this->concatenate_with_reference_cloud(cloud_out, cloud_in3, success, cloud_in3_received_recent, reference_time, cloud_in3_timestamp, 3, false); // Concatenate the fourth pointcloud - if (param_clouds_ >= 4 && success && cloud_in4_received) { - // Warn if cloud was not received since last update - if (!cloud_in4_received_recent) { - ROS_WARN("Cloud 4 was not received since last update, reusing last received message..."); - } - cloud_in4_received_recent = false; - - // Transform pointcloud to the target frame - success = pcl_ros::transformPointCloud(param_frame_target_, cloud_in4, cloud_to_concat, *tfBuffer); - if (!success) { - ROS_WARN("Transforming cloud 4 from %s to %s failed!", cloud_in4.header.frame_id.c_str(), param_frame_target_.c_str()); - } - - // Concatenate the pointcloud - if (success) { - pcl::concatenatePointCloud(cloud_out, cloud_to_concat, cloud_out); - } - } + this->concatenate_with_reference_cloud(cloud_out, cloud_in4, success, cloud_in4_received_recent, reference_time, cloud_in4_timestamp, 4, false); // Publish the concatenated pointcloud if (success) { From 7309fe0b84e787df76a81c08f00370c3b98ca688 Mon Sep 17 00:00:00 2001 From: Ilia Sevostianov Date: Thu, 10 Jul 2025 21:40:57 +0300 Subject: [PATCH 2/3] Refactor time threshold check in pointcloud concatenation and improve logging for cloud transformations; Fix parent frame issue with output pcl; Resolve issue with ids (only first pcl was used) --- .../pointcloud_concatenate.hpp | 2 +- src/pointcloud_concatenate.cpp | 54 +++++++++++-------- 2 files changed, 32 insertions(+), 24 deletions(-) diff --git a/include/pointcloud_concatenate/pointcloud_concatenate.hpp b/include/pointcloud_concatenate/pointcloud_concatenate.hpp index 695b449..bdc7714 100644 --- a/include/pointcloud_concatenate/pointcloud_concatenate.hpp +++ b/include/pointcloud_concatenate/pointcloud_concatenate.hpp @@ -41,7 +41,7 @@ class PointcloudConcatenate { void subCallbackCloudIn3(sensor_msgs::PointCloud2 msg); void subCallbackCloudIn4(sensor_msgs::PointCloud2 msg); void publishPointcloud(sensor_msgs::PointCloud2 cloud); - bool checkTimeThreshold(ros::Time& reference_time, ros::Time& current_time, const int& cloud_index); + bool checkTimeThresholdOk(ros::Time& reference_time, ros::Time& current_time, const int& cloud_index); void concatenate_with_reference_cloud(sensor_msgs::PointCloud2& reference_cloud, const sensor_msgs::PointCloud2 cloud_to_concat, bool& success, bool& cloud_received_recent, ros::Time& reference_time, ros::Time& current_time, const int& cloud_index, const bool& update_reference_time = false); // Private variables and objects ros::NodeHandle nh_; diff --git a/src/pointcloud_concatenate.cpp b/src/pointcloud_concatenate.cpp index a0a6426..c215764 100644 --- a/src/pointcloud_concatenate.cpp +++ b/src/pointcloud_concatenate.cpp @@ -101,16 +101,18 @@ void PointcloudConcatenate::handleParams() { ROS_INFO("Parameters loaded."); } -bool PointcloudConcatenate::checkTimeThreshold(ros::Time& reference_time, ros::Time& current_time, const int& cloud_index) { +bool PointcloudConcatenate::checkTimeThresholdOk(ros::Time& reference_time, ros::Time& current_time, const int& cloud_index) { double timestamp_diff = std::abs((current_time - reference_time).toSec()); if (timestamp_diff > param_time_threshold_) { - ROS_WARN("Cloud %d is too old [%.3f s], skipping...", cloud_index, timestamp_diff); - return true; + ROS_INFO("Cloud %d is too old [%.3f s], skipping...", cloud_index, timestamp_diff); + return false; } - return false; + return true; } void PointcloudConcatenate::concatenate_with_reference_cloud(sensor_msgs::PointCloud2& reference_cloud, const sensor_msgs::PointCloud2 cloud_to_concat, bool& success, bool& cloud_received_recent, ros::Time& reference_time, ros::Time& current_time, const int& cloud_index, const bool& update_reference_time) { + // ROS_INFO cloud idx + ROS_INFO("Concatenating cloud %d", cloud_index); if (param_clouds_ >= cloud_index && success && cloud_received_recent) { // Warn if cloud was not received since last update if (!cloud_received_recent) { @@ -120,25 +122,35 @@ void PointcloudConcatenate::concatenate_with_reference_cloud(sensor_msgs::PointC bool success_time_threshold = false; if (update_reference_time) { success_time_threshold = true; + reference_time = cloud_to_concat.header.stamp; } else { - success_time_threshold = this->checkTimeThreshold(reference_time, current_time, cloud_index); + success_time_threshold = this->checkTimeThresholdOk(reference_time, current_time, cloud_index); } if (success_time_threshold) { - // Transform pointcloud to the target frame - success = pcl_ros::transformPointCloud(param_frame_target_, cloud_to_concat, reference_cloud, *tfBuffer); - if (!success) { - ROS_WARN("Transforming cloud %d from %s to %s failed!", cloud_index, cloud_to_concat.header.frame_id.c_str(), param_frame_target_.c_str()); + sensor_msgs::PointCloud2 transformed_cloud; + // Transform pointcloud to the target frame if needed + if (cloud_to_concat.header.frame_id != param_frame_target_) { + ROS_INFO("Transforming cloud %d from %s to %s", cloud_index, cloud_to_concat.header.frame_id.c_str(), param_frame_target_.c_str()); + success = pcl_ros::transformPointCloud(param_frame_target_, cloud_to_concat, transformed_cloud, *tfBuffer); + if (!success) { + ROS_WARN("Transforming cloud %d from %s to %s failed!", cloud_index, cloud_to_concat.header.frame_id.c_str(), param_frame_target_.c_str()); + return; + } + } else { + ROS_INFO("Cloud %d is already in the target frame", cloud_index); + transformed_cloud = cloud_to_concat; } - - // Concatenate the pointcloud - if (success) { - pcl::concatenatePointCloud(reference_cloud, cloud_to_concat, reference_cloud); + // For the first cloud, set the header and assign directly if (update_reference_time) { - reference_time = current_time; + reference_cloud = transformed_cloud; + ROS_INFO("Cloud %d is the first cloud. Taken as reference", cloud_index); + } else { + // Concatenate the pointclouds (all in the same frame now) + pcl::concatenatePointCloud(reference_cloud, transformed_cloud, reference_cloud); + ROS_INFO("Cloud %d is concatenated with the reference cloud wrt the target frame: %s", cloud_index, param_frame_target_.c_str()); } } } - } } double PointcloudConcatenate::getHz() { @@ -173,16 +185,16 @@ void PointcloudConcatenate::update() { ros::Time reference_time = ros::Time::now(); // Concatenate the first pointcloud - this->concatenate_with_reference_cloud(cloud_out, cloud_in1, success, cloud_in1_received_recent, reference_time, cloud_in1_timestamp, 1, true); + this->concatenate_with_reference_cloud(cloud_out, cloud_in1, success, cloud_in1_received_recent, reference_time, cloud_in1_timestamp, 0, true); // Concatenate the second pointcloud - this->concatenate_with_reference_cloud(cloud_out, cloud_in2, success, cloud_in2_received_recent, reference_time, cloud_in2_timestamp, 2, false); + this->concatenate_with_reference_cloud(cloud_out, cloud_in2, success, cloud_in2_received_recent, reference_time, cloud_in2_timestamp, 1, false); // Concatenate the third pointcloud - this->concatenate_with_reference_cloud(cloud_out, cloud_in3, success, cloud_in3_received_recent, reference_time, cloud_in3_timestamp, 3, false); + this->concatenate_with_reference_cloud(cloud_out, cloud_in3, success, cloud_in3_received_recent, reference_time, cloud_in3_timestamp, 2, false); // Concatenate the fourth pointcloud - this->concatenate_with_reference_cloud(cloud_out, cloud_in4, success, cloud_in4_received_recent, reference_time, cloud_in4_timestamp, 4, false); + this->concatenate_with_reference_cloud(cloud_out, cloud_in4, success, cloud_in4_received_recent, reference_time, cloud_in4_timestamp, 3, false); // Publish the concatenated pointcloud if (success) { @@ -193,9 +205,5 @@ void PointcloudConcatenate::update() { void PointcloudConcatenate::publishPointcloud(sensor_msgs::PointCloud2 cloud) { // Publishes the combined pointcloud - - // Update the timestamp - cloud.header.stamp = ros::Time::now(); - // Publish pub_cloud_out.publish(cloud); } \ No newline at end of file From 086b18f4f2028903c9c22e012a10816fea62575f Mon Sep 17 00:00:00 2001 From: Ilia Sevostianov Date: Thu, 10 Jul 2025 21:42:08 +0300 Subject: [PATCH 3/3] Comment out ROS_INFO logging statements in pointcloud concatenation to reduce verbosity during processing. --- src/pointcloud_concatenate.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/pointcloud_concatenate.cpp b/src/pointcloud_concatenate.cpp index c215764..b0478a4 100644 --- a/src/pointcloud_concatenate.cpp +++ b/src/pointcloud_concatenate.cpp @@ -112,7 +112,7 @@ bool PointcloudConcatenate::checkTimeThresholdOk(ros::Time& reference_time, ros: void PointcloudConcatenate::concatenate_with_reference_cloud(sensor_msgs::PointCloud2& reference_cloud, const sensor_msgs::PointCloud2 cloud_to_concat, bool& success, bool& cloud_received_recent, ros::Time& reference_time, ros::Time& current_time, const int& cloud_index, const bool& update_reference_time) { // ROS_INFO cloud idx - ROS_INFO("Concatenating cloud %d", cloud_index); + // ROS_INFO("Concatenating cloud %d", cloud_index); if (param_clouds_ >= cloud_index && success && cloud_received_recent) { // Warn if cloud was not received since last update if (!cloud_received_recent) { @@ -130,24 +130,24 @@ void PointcloudConcatenate::concatenate_with_reference_cloud(sensor_msgs::PointC sensor_msgs::PointCloud2 transformed_cloud; // Transform pointcloud to the target frame if needed if (cloud_to_concat.header.frame_id != param_frame_target_) { - ROS_INFO("Transforming cloud %d from %s to %s", cloud_index, cloud_to_concat.header.frame_id.c_str(), param_frame_target_.c_str()); + // ROS_INFO("Transforming cloud %d from %s to %s", cloud_index, cloud_to_concat.header.frame_id.c_str(), param_frame_target_.c_str()); success = pcl_ros::transformPointCloud(param_frame_target_, cloud_to_concat, transformed_cloud, *tfBuffer); if (!success) { ROS_WARN("Transforming cloud %d from %s to %s failed!", cloud_index, cloud_to_concat.header.frame_id.c_str(), param_frame_target_.c_str()); return; } } else { - ROS_INFO("Cloud %d is already in the target frame", cloud_index); + // ROS_INFO("Cloud %d is already in the target frame", cloud_index); transformed_cloud = cloud_to_concat; } // For the first cloud, set the header and assign directly if (update_reference_time) { reference_cloud = transformed_cloud; - ROS_INFO("Cloud %d is the first cloud. Taken as reference", cloud_index); + // ROS_INFO("Cloud %d is the first cloud. Taken as reference", cloud_index); } else { // Concatenate the pointclouds (all in the same frame now) pcl::concatenatePointCloud(reference_cloud, transformed_cloud, reference_cloud); - ROS_INFO("Cloud %d is concatenated with the reference cloud wrt the target frame: %s", cloud_index, param_frame_target_.c_str()); + // ROS_INFO("Cloud %d is concatenated with the reference cloud wrt the target frame: %s", cloud_index, param_frame_target_.c_str()); } } }