Feat/autoware universe utils

common/autoware_universe_utils/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_autoware_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Boost REQUIRED)
+
+ament_auto_add_library(tier4_autoware_utils SHARED
+  src/geometry/geometry.cpp
+  src/geometry/pose_deviation.cpp
+  src/geometry/boost_polygon_utils.cpp
+  src/math/sin_table.cpp
+  src/math/trigonometry.cpp
+  src/ros/msg_operation.cpp
+  src/ros/marker_helper.cpp
+  src/ros/logger_level_configure.cpp
+  src/system/backtrace.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+
+  file(GLOB_RECURSE test_files test/**/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_tier4_autoware_utils ${test_files})
+
+  target_link_libraries(test_tier4_autoware_utils
+    tier4_autoware_utils
+  )
+endif()
+
+ament_auto_package()

common/autoware_universe_utils/README.md

@@ -0,0 +1,9 @@
+# tier4_autoware_utils
+
+## Purpose
+
+This package contains many common functions used by other packages, so please refer to them as needed.
+
+## For developers
+
+`tier4_autoware_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing.

common/autoware_universe_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp

@@ -0,0 +1,104 @@
+// Copyright 2020 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 TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_
+#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_
+
+#include <boost/geometry/core/cs.hpp>
+#include <boost/geometry/geometries/geometries.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/geometries/register/ring.hpp>
+
+#define EIGEN_MPL2_ONLY
+#include <Eigen/Core>
+
+#include <geometry_msgs/msg/point.hpp>
+
+namespace tier4_autoware_utils
+{
+// 2D
+struct Point2d;
+using Segment2d = boost::geometry::model::segment<Point2d>;
+using Box2d = boost::geometry::model::box<Point2d>;
+using LineString2d = boost::geometry::model::linestring<Point2d>;
+using LinearRing2d = boost::geometry::model::ring<Point2d>;
+using Polygon2d = boost::geometry::model::polygon<Point2d>;
+using Line2d = boost::geometry::model::linestring<Point2d>;
+using MultiPoint2d = boost::geometry::model::multi_point<Point2d>;
+using MultiLineString2d = boost::geometry::model::multi_linestring<LineString2d>;
+using MultiPolygon2d = boost::geometry::model::multi_polygon<Polygon2d>;
+
+// 3D
+struct Point3d;
+using Segment3d = boost::geometry::model::segment<Point3d>;
+using Box3d = boost::geometry::model::box<Point3d>;
+using LineString3d = boost::geometry::model::linestring<Point3d>;
+using LinearRing3d = boost::geometry::model::ring<Point3d>;
+using Polygon3d = boost::geometry::model::polygon<Point3d>;
+using MultiPoint3d = boost::geometry::model::multi_point<Point3d>;
+using MultiLineString3d = boost::geometry::model::multi_linestring<LineString3d>;
+using MultiPolygon3d = boost::geometry::model::multi_polygon<Polygon3d>;
+
+struct Point2d : public Eigen::Vector2d
+{
+  Point2d() = default;
+  Point2d(const double x, const double y) : Eigen::Vector2d(x, y) {}
+
+  [[nodiscard]] Point3d to_3d(const double z = 0.0) const;
+};
+
+struct Point3d : public Eigen::Vector3d
+{
+  Point3d() = default;
+  Point3d(const double x, const double y, const double z) : Eigen::Vector3d(x, y, z) {}
+
+  [[nodiscard]] Point2d to_2d() const;
+};
+
+inline Point3d Point2d::to_3d(const double z) const
+{
+  return Point3d{x(), y(), z};
+}
+
+inline Point2d Point3d::to_2d() const
+{
+  return Point2d{x(), y()};
+}
+
+inline geometry_msgs::msg::Point toMsg(const Point3d & point)
+{
+  geometry_msgs::msg::Point msg;
+  msg.x = point.x();
+  msg.y = point.y();
+  msg.z = point.z();
+  return msg;
+}
+
+inline Point3d fromMsg(const geometry_msgs::msg::Point & msg)
+{
+  Point3d point;
+  point.x() = msg.x;
+  point.y() = msg.y;
+  point.z() = msg.z;
+  return point;
+}
+}  // namespace tier4_autoware_utils
+
+BOOST_GEOMETRY_REGISTER_POINT_2D(                                       // NOLINT
+  tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y())       // NOLINT
+BOOST_GEOMETRY_REGISTER_POINT_3D(                                       // NOLINT
+  tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z())  // NOLINT
+BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d)        // NOLINT
+
+#endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_

common/autoware_universe_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp

@@ -0,0 +1,52 @@
+// Copyright 2022 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 TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_
+#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_
+
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+
+#include <autoware_perception_msgs/msg/detected_object.hpp>
+#include <autoware_perception_msgs/msg/predicted_object.hpp>
+#include <autoware_perception_msgs/msg/tracked_object.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <vector>
+
+namespace tier4_autoware_utils
+{
+bool isClockwise(const Polygon2d & polygon);
+Polygon2d inverseClockwise(const Polygon2d & polygon);
+geometry_msgs::msg::Polygon rotatePolygon(
+  const geometry_msgs::msg::Polygon & polygon, const double & angle);
+/// @brief rotate a polygon by some angle around the origin
+/// @param[in] polygon input polygon
+/// @param[in] angle angle of rotation [rad]
+/// @return rotated polygon
+Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle);
+Polygon2d toPolygon2d(
+  const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape);
+Polygon2d toPolygon2d(
+  const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape);
+Polygon2d toPolygon2d(const autoware_perception_msgs::msg::DetectedObject & object);
+Polygon2d toPolygon2d(const autoware_perception_msgs::msg::TrackedObject & object);
+Polygon2d toPolygon2d(const autoware_perception_msgs::msg::PredictedObject & object);
+Polygon2d toFootprint(
+  const geometry_msgs::msg::Pose & base_link_pose, const double base_to_front,
+  const double base_to_rear, const double width);
+double getArea(const autoware_perception_msgs::msg::Shape & shape);
+Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset);
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_