git cherry-pick 44e4be7 # feat(pointcloud_preprocessor): enable to ch…

…ange synchronized pointcloud topic name (autowarefoundation#6525) fix : fix : fix conflict occured by cherry pick Signed-off-by: N-Eiki <[email protected]>
tier4 · Jun 10, 2024 · 23d6755 · 23d6755
1 parent 39f09fd
commit 23d6755
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 21 deletions.
diff --git a/...or/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/...or/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
@@ -126,7 +126,6 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   double timeout_sec_ = 0.1;
 
   bool publish_synchronized_pointcloud_;
-  bool keep_input_frame_in_synchronized_pointcloud_;
   std::string synchronized_pointcloud_postfix_;
 
   std::set<std::string> not_subscribed_topic_names_;

diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
@@ -112,9 +112,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
     }
 
     // Check if publish synchronized pointcloud
-    publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", true);
-    keep_input_frame_in_synchronized_pointcloud_ =
-      declare_parameter("keep_input_frame_in_synchronized_pointcloud", true);
+    publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false);
     synchronized_pointcloud_postfix_ =
       declare_parameter("synchronized_pointcloud_postfix", "pointcloud");
   }
@@ -399,23 +397,10 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
         pcl::concatenatePointCloud(
           *concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr);
       }
-      // convert to original sensor frame if necessary
-      bool need_transform_to_sensor_frame = (e.second->header.frame_id != output_frame_);
-      if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) {
-        sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr_in_sensor_frame(
-          new sensor_msgs::msg::PointCloud2());
-        pcl_ros::transformPointCloud(
-          (std::string)e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr,
-          *transformed_cloud_ptr_in_sensor_frame, *tf2_buffer_);
-        transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp;
-        transformed_cloud_ptr_in_sensor_frame->header.frame_id = e.second->header.frame_id;
-        transformed_clouds[e.first] = transformed_cloud_ptr_in_sensor_frame;
-      } else {
-        transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp;
-        transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;
-        transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr;
-      }
-
+      // gather transformed clouds
+      transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp;
+      transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;
+      transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr;
     } else {
       not_subscribed_topic_names_.insert(e.first);
     }