feat: add point_types for wrapper (autowarefoundation#784) (autowaref…

…oundation#215)

* add point_types

* Revert "add point_types"

This reverts commit 5810000cd1cbd876bc22372e2bb74ccaca06187b.

* create autoware_point_types pkg

* add include

* add cmath

* fix author

* fix bug

* define epsilon as argument

* add test

* remove unnamed namespace

* update test

* fix test name

* use std::max

* change comparison method

* remove unnencessary line

* fix test

* fix comparison method name

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>

common/autoware_point_types/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_point_types)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_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()

common/autoware_point_types/include/autoware_point_types/types.hpp

@@ -0,0 +1,80 @@
+// 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__AUTOWARE_POINT_TYPES_HPP_
+#define AUTOWARE_POINT_TYPES__AUTOWARE_POINT_TYPES_HPP_
+
+#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
+
+#include <cmath>
+#include <tuple>
+
+namespace autoware_point_types
+{
+template <class T>
+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<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
+           float_eq<float>(p1.z, p2.z) && float_eq<float>(p1.intensity, p2.intensity);
+  }
+};
+
+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<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
+           float_eq<float>(p1.z, p2.z) && float_eq<float>(p1.intensity, p2.intensity) &&
+           p1.ring == p2.ring && float_eq<float>(p1.azimuth, p2.azimuth) &&
+           float_eq<float>(p1.distance, p2.distance) && p1.return_type == p2.return_type &&
+           float_eq<float>(p1.time_stamp, p2.time_stamp);
+  }
+};
+
+enum class PointIndex { X, Y, Z, Intensity, Ring, Azimuth, Distance, ReturnType, TimeStamp };
+
+LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth);
+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);
+
+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>;
+
+}  // namespace autoware_point_types
+
+#endif  // AUTOWARE_POINT_TYPES__AUTOWARE_POINT_TYPES_HPP_

common/autoware_point_types/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_point_types</name>
+  <version>0.1.0</version>
+  <description>The point types definition to use point_cloud_msg_wrapper</description>
+  <maintainer email="taichi.higashide@tier4.jp">Taichi Higashide</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_core</buildtool_depend>
+  <buildtool_depend>ament_cmake_export_dependencies</buildtool_depend>
+  <buildtool_depend>ament_cmake_test</buildtool_depend>
+
+  <buildtool_export_depend>ament_cmake_core</buildtool_export_depend>
+  <buildtool_export_depend>ament_cmake_test</buildtool_export_depend>
+
+  <depend>ament_cmake_copyright</depend>
+  <depend>ament_cmake_cppcheck</depend>
+  <depend>ament_cmake_lint_cmake</depend>
+  <depend>ament_cmake_xmllint</depend>
+  <depend>point_cloud_msg_wrapper</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>point_cloud_msg_wrapper</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/autoware_point_types/test/test_point_types.cpp

@@ -0,0 +1,57 @@
+// 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 <gtest/gtest.h>
+
+#include <limits>
+
+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, FloatEq)
+{
+  // test template
+  EXPECT_TRUE(autoware_point_types::float_eq<float>(1, 1));
+  EXPECT_TRUE(autoware_point_types::float_eq<double>(1, 1));
+
+  // test floating point error
+  EXPECT_TRUE(autoware_point_types::float_eq<float>(1, 1 + std::numeric_limits<float>::epsilon()));
+
+  // test difference of sign
+  EXPECT_FALSE(autoware_point_types::float_eq<float>(2, -2));
+  EXPECT_FALSE(autoware_point_types::float_eq<float>(-2, 2));
+
+  // small value difference
+  EXPECT_FALSE(autoware_point_types::float_eq<float>(2, 2 + 10e-6));
+
+  // expect same value if epsilon is larger than difference
+  EXPECT_TRUE(autoware_point_types::float_eq<float>(2, 2 + 10e-6, 10e-5));
+}