diff --git a/common/autoware_point_types/CMakeLists.txt b/common/autoware_point_types/CMakeLists.txt new file mode 100644 index 00000000..c149f79d --- /dev/null +++ b/common/autoware_point_types/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_point_types) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +include_directories( + include + SYSTEM + ${PCL_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_autoware_point_types + test/test_point_types.cpp + ) + target_include_directories(test_autoware_point_types + PRIVATE include + ) + ament_target_dependencies(test_autoware_point_types + point_cloud_msg_wrapper + ) +endif() + +ament_auto_package() diff --git a/common/autoware_point_types/README.md b/common/autoware_point_types/README.md new file mode 100644 index 00000000..5a14d68f --- /dev/null +++ b/common/autoware_point_types/README.md @@ -0,0 +1,68 @@ +# Autoware Point Types + +## Overview + +This package provides a variety of structures to represent different types of point cloud data, mainly used for point cloud processing and analysis. + +## Design + +### Point cloud data type definition + +`autoware_point_types` defines multiple structures (such as PointXYZI, PointXYZIRC, PointXYZIRADRT, PointXYZIRCAEDT), each structure contains different attributes to adapt to different application scenarios. + +- `autoware::point_types::PointXYZI`: Point type with intensity information. +- `autoware::point_types::PointXYZIRC`: Extended PointXYZI, adds return_type and channel information. +- `autoware::point_types::PointXYZIRADRT`: Extended PointXYZI, adds ring, azimuth, distance, return_type and time_stamp information. +- `autoware::point_types::PointXYZIRCAEDT`: Similar to PointXYZIRADRT, but adds elevation information and uses `std::uint32_t` as the data type for time_stamp. + +### Operator overload + +Each structure overloads the `==` operator, allowing users to easily compare whether two points are equal, which is very useful for deduplication and matching of point cloud data. + +### Field generators + +The field generator is implemented using macro definitions and std::tuple, which simplifies the serialization and deserialization process of point cloud messages and improves the reusability and readability of the code. + +### Registration mechanism + +Register custom point cloud structures into the PCL library through the macro `POINT_CLOUD_REGISTER_POINT_STRUCT`, so that these structures can be directly integrated with other functions of the PCL library. + +## Usage + +- Create a point cloud object of PointXYZIRC type + +```cpp +#include "autoware/point_types/types.hpp" + +int main(){ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + + for (int i = 0; i < 5; ++i) { + autoware::point_types::PointXYZIRC point; + point.x = static_cast(i * 0.1); + point.y = static_cast(i * 0.2); + point.z = static_cast(i * 0.3); + point.intensity = static_cast(i * 10); + point.return_type = autoware::point_types::ReturnType::SINGLE_STRONGEST; + point.channel = static_cast(i); + + cloud->points.push_back(point); + } + cloud->width = cloud->points.size(); + cloud->height = 1; + + return 0; +} +``` + +- Convert ROS message to point cloud of PointXYZIRC type + +```cpp +ExampleNode::points_callback(const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + pcl::PointCloud::Ptr points_ptr( + new pcl::PointCloud); + + pcl::fromROSMsg(*points_msg_ptr, *points_ptr); +} +``` diff --git a/common/autoware_point_types/include/autoware/point_types/types.hpp b/common/autoware_point_types/include/autoware/point_types/types.hpp new file mode 100644 index 00000000..0fde6222 --- /dev/null +++ b/common/autoware_point_types/include/autoware/point_types/types.hpp @@ -0,0 +1,188 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__POINT_TYPES__TYPES_HPP_ +#define AUTOWARE__POINT_TYPES__TYPES_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::point_types +{ +template +bool float_eq(const T a, const T b, const T eps = 10e-6) +{ + return std::fabs(a - b) < eps; +} + +struct PointXYZI +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + float intensity{0.0F}; + friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity); + } +}; + +enum ReturnType : uint8_t { + INVALID = 0, + SINGLE_STRONGEST, + SINGLE_LAST, + DUAL_STRONGEST_FIRST, + DUAL_STRONGEST_LAST, + DUAL_WEAK_FIRST, + DUAL_WEAK_LAST, + DUAL_ONLY, +}; + +struct PointXYZIRC +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + + friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel; + } +}; + +struct PointXYZIRADRT +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + float intensity{0.0F}; + uint16_t ring{0U}; + float azimuth{0.0F}; + float distance{0.0F}; + uint8_t return_type{0U}; + double time_stamp{0.0}; + friend bool operator==(const PointXYZIRADRT & p1, const PointXYZIRADRT & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity) && + p1.ring == p2.ring && float_eq(p1.azimuth, p2.azimuth) && + float_eq(p1.distance, p2.distance) && p1.return_type == p2.return_type && + float_eq(p1.time_stamp, p2.time_stamp); + } +}; + +struct PointXYZIRCAEDT +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + float azimuth{0.0F}; + float elevation{0.0F}; + float distance{0.0F}; + std::uint32_t time_stamp{0U}; + + friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel && + float_eq(p1.azimuth, p2.azimuth) && float_eq(p1.distance, p2.distance) && + p1.time_stamp == p2.time_stamp; + } +}; + +enum class PointXYZIIndex { X, Y, Z, Intensity }; +enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel }; +enum class PointXYZIRADRTIndex { + X, + Y, + Z, + Intensity, + Ring, + Azimuth, + Distance, + ReturnType, + TimeStamp +}; +enum class PointXYZIRCAEDTIndex { + X, + Y, + Z, + Intensity, + ReturnType, + Channel, + Azimuth, + Elevation, + Distance, + TimeStamp +}; + +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp); + +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); + +using PointXYZIRCGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator>; + +using PointXYZIRADRTGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator, + field_return_type_generator, field_time_stamp_generator>; + +using PointXYZIRCAEDTGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator, field_azimuth_generator, + field_elevation_generator, field_distance_generator, field_time_stamp_generator>; + +} // namespace autoware::point_types + +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware::point_types::PointXYZIRC, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware::point_types::PointXYZIRADRT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( + float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( + double, time_stamp, time_stamp)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware::point_types::PointXYZIRCAEDT, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, + return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( + float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) +#endif // AUTOWARE__POINT_TYPES__TYPES_HPP_ diff --git a/common/autoware_point_types/package.xml b/common/autoware_point_types/package.xml new file mode 100644 index 00000000..73db1829 --- /dev/null +++ b/common/autoware_point_types/package.xml @@ -0,0 +1,35 @@ + + + + autoware_point_types + 0.0.0 + The point types definition to use point_cloud_msg_wrapper + David Wong + Max Schmeller + Cynthia Liu + Apache License 2.0 + + ament_cmake_core + ament_cmake_export_dependencies + ament_cmake_test + autoware_cmake + + ament_cmake_core + ament_cmake_test + + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_lint_cmake + ament_cmake_xmllint + pcl_ros + point_cloud_msg_wrapper + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + point_cloud_msg_wrapper + + + ament_cmake + + diff --git a/common/autoware_point_types/test/test_point_types.cpp b/common/autoware_point_types/test/test_point_types.cpp new file mode 100644 index 00000000..08696a99 --- /dev/null +++ b/common/autoware_point_types/test/test_point_types.cpp @@ -0,0 +1,66 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/point_types/types.hpp" + +#include + +#include + +TEST(PointEquality, PointXYZI) +{ + using autoware::point_types::PointXYZI; + + PointXYZI pt0{0, 1, 2, 3}; + PointXYZI pt1{0, 1, 2, 3}; + EXPECT_EQ(pt0, pt1); +} + +TEST(PointEquality, PointXYZIRADRT) +{ + using autoware::point_types::PointXYZIRADRT; + + PointXYZIRADRT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8}; + PointXYZIRADRT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8}; + EXPECT_EQ(pt0, pt1); +} + +TEST(PointEquality, PointXYZIRCAEDT) +{ + using autoware::point_types::PointXYZIRCAEDT; + + PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + EXPECT_EQ(pt0, pt1); +} + +TEST(PointEquality, FloatEq) +{ + // test template + EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); + EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); + + // test floating point error + EXPECT_TRUE(autoware::point_types::float_eq(1, 1 + std::numeric_limits::epsilon())); + + // test difference of sign + EXPECT_FALSE(autoware::point_types::float_eq(2, -2)); + EXPECT_FALSE(autoware::point_types::float_eq(-2, 2)); + + // small value difference + EXPECT_FALSE(autoware::point_types::float_eq(2, 2 + 10e-6)); + + // expect same value if epsilon is larger than difference + EXPECT_TRUE(autoware::point_types::float_eq(2, 2 + 10e-6, 10e-5)); +}