diff --git a/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml index 0838cc60a284b..d99849b532f3e 100644 --- a/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml +++ b/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + has_static_tf_only: false maximum_queue_size: 5 timeout_sec: 0.2 is_motion_compensated: true @@ -16,4 +17,3 @@ output_frame: base_link lidar_timestamp_offsets: [0.0, 0.04, 0.08] lidar_timestamp_noise_window: [0.01, 0.01, 0.01] - has_static_tf_only: false diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp index 32882b8ce6a6b..7d5e26f388ce6 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp @@ -92,12 +92,11 @@ class CombineCloudHandler { private: rclcpp::Node * node_; - std::unique_ptr managed_tf_buffer_{nullptr}; - std::vector input_topics_; std::string output_frame_; bool is_motion_compensated_; bool keep_input_frame_in_synchronized_pointcloud_; + std::unique_ptr managed_tf_buffer_{nullptr}; struct RclcppTimeHash_ { @@ -112,7 +111,8 @@ class CombineCloudHandler CombineCloudHandler( rclcpp::Node * node, std::vector input_topics, std::string output_frame, - bool is_motion_compensated, bool keep_input_frame_in_synchronized_pointcloud); + bool is_motion_compensated, bool keep_input_frame_in_synchronized_pointcloud, + bool has_static_tf_only); void processTwist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & input); void processOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr & input); diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp index 5d09c02dda874..c1dc2b736954b 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp @@ -74,8 +74,8 @@ CombineCloudHandler::CombineCloudHandler( output_frame_(output_frame), is_motion_compensated_(is_motion_compensated), keep_input_frame_in_synchronized_pointcloud_(keep_input_frame_in_synchronized_pointcloud), - managed_tf_buffer_ = - std::make_unique(this, has_static_tf_only) + managed_tf_buffer_( + std::make_unique(node_, has_static_tf_only)) { } @@ -156,8 +156,7 @@ CombineCloudHandler::combinePointClouds( sensor_msgs::msg::PointCloud2::SharedPtr cloud = pair.second; auto transformed_cloud_ptr = std::make_shared(); - managed_tf_buffer_->transformPointcloud( - output_frame_, cloud->header.frame_id, *transformed_cloud_ptr); + managed_tf_buffer_->transformPointcloud(output_frame_, *cloud, *transformed_cloud_ptr); topic_to_original_stamp_map[topic] = rclcpp::Time(cloud->header.stamp).seconds(); diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_cloud_collector.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_cloud_collector.cpp index f3f0626364f88..1a73bcede8ab4 100644 --- a/sensing/autoware_pointcloud_preprocessor/test/test_cloud_collector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/test/test_cloud_collector.cpp @@ -41,7 +41,8 @@ class ConcatenateCloudTest : public ::testing::Test // Instead of "input_topics", other parameters are not unsed. // They just helps to setup the concatenate node node_options.parameter_overrides( - {{"maximum_queue_size", 5}, + {{"has_static_tf_only", false}, + {"maximum_queue_size", 5}, {"timeout_sec", 0.2}, {"is_motion_compensated", true}, {"publish_synchronized_pointcloud", true}, @@ -60,7 +61,7 @@ class ConcatenateCloudTest : public ::testing::Test combine_cloud_handler_ = std::make_shared( concatenate_node_.get(), std::vector{"lidar_top", "lidar_left", "lidar_right"}, - "base_link", true, true); + "base_link", true, true, false); collector_ = std::make_shared( std::dynamic_pointer_cast< diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node.py b/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node.py index ac1a63b9ef07a..0923a7fb375a4 100644 --- a/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node.py +++ b/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node.py @@ -83,6 +83,7 @@ def generate_test_description(): ], parameters=[ { + "has_static_tf_only": False, "maximum_queue_size": 5, "timeout_sec": TIMEOUT_SEC, "is_motion_compensated": True, @@ -470,7 +471,7 @@ def test_2_normal_inputs_with_noise(self): cloud_arr = get_output_points(self.msg_buffer[0]) print("cloud_arr: ", cloud_arr) self.assertTrue( - np.allclose(cloud_arr, expected_pointcloud, atol=1e-2), + np.allclose(cloud_arr, expected_pointcloud, atol=2e-2), "The concatenation node have wierd output", )