diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/common/types.hpp index 3f3e444c1aa8c..b8afd90746d49 100644 --- a/common/autoware_auto_common/include/common/types.hpp +++ b/common/autoware_auto_common/include/common/types.hpp @@ -109,6 +109,54 @@ struct COMMON_PUBLIC PointXYZI } }; +struct COMMON_PUBLIC PointXYZ +{ + float32_t x{0.0F}; + float32_t y{0.0F}; + float32_t z{0.0F}; + friend bool operator==(const PointXYZ & p1, const PointXYZ & p2) noexcept + { + return helper_functions::comparisons::rel_eq( + p1.x, p2.x, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.y, p2.y, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.z, p2.z, std::numeric_limits::epsilon()); + } +}; + +struct COMMON_PUBLIC PointXYZRGBA +{ + float32_t x{0.0F}; + float32_t y{0.0F}; + float32_t z{0.0F}; + uint8_t r{0}; + uint8_t g{0}; + uint8_t b{0}; + uint8_t a{0}; + friend bool operator==(const PointXYZRGBA & p1, const PointXYZRGBA & p2) noexcept + { + return helper_functions::comparisons::rel_eq( + p1.x, p2.x, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.y, p2.y, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.z, p2.z, std::numeric_limits::epsilon()) && + + (p1.r == p2.r) && + + (p1.g == p2.g) && + + (p1.b == p2.b) && + + (p1.a == p2.a); + } +}; + using PointBlock = std::vector; using PointPtrBlock = std::vector; /// \brief Stores basic configuration information, does some simple validity checking diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp index 6ff563c157d00..4580363c8affb 100644 --- a/perception/euclidean_cluster/lib/utils.cpp +++ b/perception/euclidean_cluster/lib/utils.cpp @@ -13,6 +13,9 @@ // limitations under the License. #include "euclidean_cluster/utils.hpp" +#include +#include + #include #include #include @@ -27,12 +30,11 @@ geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & poin centroid.x = 0.0f; centroid.y = 0.0f; centroid.z = 0.0f; - for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), - iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - centroid.x += *iter_x; - centroid.y += *iter_y; - centroid.z += *iter_z; + point_cloud_msg_wrapper::PointCloud2View view{pointcloud}; + for (const auto & point : view) { + centroid.x += point.x; + centroid.y += point.y; + centroid.z += point.z; } const size_t size = pointcloud.width * pointcloud.height; centroid.x = centroid.x / static_cast(size); @@ -73,40 +75,20 @@ void convertObjectMsg2SensorMsg( pointcloud_size += feature_object.feature.cluster.width * feature_object.feature.cluster.height; } - sensor_msgs::PointCloud2Modifier modifier(output); - modifier.setPointCloud2Fields( - 4, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, - "z", 1, sensor_msgs::msg::PointField::FLOAT32, "rgb", 1, sensor_msgs::msg::PointField::FLOAT32); - modifier.resize(pointcloud_size); - - sensor_msgs::PointCloud2Iterator iter_out_x(output, "x"); - sensor_msgs::PointCloud2Iterator iter_out_y(output, "y"); - sensor_msgs::PointCloud2Iterator iter_out_z(output, "z"); - sensor_msgs::PointCloud2Iterator iter_out_r(output, "r"); - sensor_msgs::PointCloud2Iterator iter_out_g(output, "g"); - sensor_msgs::PointCloud2Iterator iter_out_b(output, "b"); + point_cloud_msg_wrapper::PointCloud2Modifier modifier{ + output, "euclidean_cluster_cloud"}; constexpr uint8_t color_data[] = {200, 0, 0, 0, 200, 0, 0, 0, 200, 200, 200, 0, 200, 0, 200, 0, 200, 200}; // 6 pattern for (size_t i = 0; i < input.feature_objects.size(); ++i) { const auto & feature_object = input.feature_objects.at(i); - sensor_msgs::PointCloud2ConstIterator iter_in_x(feature_object.feature.cluster, "x"); - sensor_msgs::PointCloud2ConstIterator iter_in_y(feature_object.feature.cluster, "y"); - sensor_msgs::PointCloud2ConstIterator iter_in_z(feature_object.feature.cluster, "z"); - for (; iter_in_x != iter_in_x.end(); ++iter_in_x, ++iter_in_y, ++iter_in_z, ++iter_out_x, - ++iter_out_y, ++iter_out_z, ++iter_out_r, ++iter_out_g, - ++iter_out_b) { - *iter_out_x = *iter_in_x; - *iter_out_y = *iter_in_y; - *iter_out_z = *iter_in_z; - *iter_out_r = color_data[3 * (i % 6) + 0]; - *iter_out_g = color_data[3 * (i % 6) + 1]; - *iter_out_b = color_data[3 * (i % 6) + 2]; + point_cloud_msg_wrapper::PointCloud2View view{ + feature_object.feature.cluster}; + for (const auto & point : view) { + modifier.push_back( + {point.x, point.y, point.z, (color_data[3 * (i % 6) + 0]), (color_data[3 * (i % 6) + 1]), + (color_data[3 * (i % 6) + 2]), 0}); } } - - output.width = pointcloud_size; - output.height = 1; - output.is_dense = false; } } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index d99c923d91928..1b5c0ee04e218 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -11,6 +11,7 @@ autoware_cmake + autoware_auto_common autoware_auto_perception_msgs compare_map_segmentation geometry_msgs