From 553f5178a2c308f81aa355e3f623c0cf1917878b Mon Sep 17 00:00:00 2001 From: carma Date: Thu, 28 Mar 2024 09:28:15 -0400 Subject: [PATCH] updated point type in point_cloud_fusion and point_cloud_fusion_nodes include --- .../point_cloud_fusion/point_cloud_fusion.hpp | 4 ++-- .../point_cloud_fusion/src/point_cloud_fusion.cpp | 12 ++++++------ .../point_cloud_fusion_node.hpp | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp index 371ab867..9e2a6e63 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp @@ -32,7 +32,7 @@ namespace filters { namespace point_cloud_fusion { -using autoware::common::types::PointXYZI; +using autoware::common::types::PointXYZIRing; class POINT_CLOUD_FUSION_PUBLIC PointCloudFusion { @@ -67,7 +67,7 @@ class POINT_CLOUD_FUSION_PUBLIC PointCloudFusion void concatenate_pointcloud( const PointCloudMsgT & pc_in, uint32_t & concat_idx, - point_cloud_msg_wrapper::PointCloud2Modifier & modifier) const; + point_cloud_msg_wrapper::PointCloud2Modifier & modifier) const; uint32_t m_cloud_capacity; size_t m_input_topics_size; diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp index ae1545ee..e019610e 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp @@ -41,8 +41,8 @@ uint32_t PointCloudFusion::fuse_pc_msgs( { uint32_t pc_concat_idx = 0; - using autoware::common::types::PointXYZI; - point_cloud_msg_wrapper::PointCloud2Modifier modifier{cloud_concatenated}; + using autoware::common::types::PointXYZIRing; + point_cloud_msg_wrapper::PointCloud2Modifier modifier{cloud_concatenated}; for (size_t i = 0; i < m_input_topics_size; ++i) { concatenate_pointcloud(*msgs[i], pc_concat_idx, modifier); @@ -54,18 +54,18 @@ uint32_t PointCloudFusion::fuse_pc_msgs( void PointCloudFusion::concatenate_pointcloud( const sensor_msgs::msg::PointCloud2 & pc_in, uint32_t & concat_idx, - point_cloud_msg_wrapper::PointCloud2Modifier & modifier) const + point_cloud_msg_wrapper::PointCloud2Modifier & modifier) const { if ((pc_in.width + concat_idx) > m_cloud_capacity) { throw Error::TOO_LARGE; } - using autoware::common::types::PointXYZI; - point_cloud_msg_wrapper::PointCloud2View view{pc_in}; + using autoware::common::types::PointXYZIRing; + point_cloud_msg_wrapper::PointCloud2View view{pc_in}; auto view_it = view.cbegin(); while (view_it != view.cend()) { - common::types::PointXYZI pt; + common::types::PointXYZIRing pt; pt.x = (*view_it).x; pt.y = (*view_it).y; pt.z = (*view_it).z; diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp index 587f0388..bf8fe794 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp @@ -52,7 +52,7 @@ class POINT_CLOUD_FUSION_NODES_PUBLIC PointCloudFusionNode : public rclcpp::Node const rclcpp::NodeOptions & node_options); private: - using PointT = common::types::PointXYZIF; + using PointT = common::types::PointXYZIRing; using PointCloudMsgT = sensor_msgs::msg::PointCloud2; using PointCloudT = sensor_msgs::msg::PointCloud2; using SyncPolicyT = message_filters::sync_policies::ApproximateTime