From fc1d549ea361280d150ad935bebf6e1af0d43ba8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 20 Jul 2022 23:01:47 +0900 Subject: [PATCH] feat(tier4_autoware_utils): add boost_polygon_utils.hpp (#1367) * feat(tier4_autoware_utils): add boost_polygon_utils.hpp Signed-off-by: Takayuki Murooka * minor fix Signed-off-by: Takayuki Murooka * fix build error Signed-off-by: Takayuki Murooka * add test Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix build error Signed-off-by: Takayuki Murooka * Revert "fix build error" This reverts commit eb86a9221e3f97158a15ec5996974aa751fa9ab7. * ClockWise -> Clockwise Signed-off-by: Takayuki Murooka --- common/tier4_autoware_utils/CMakeLists.txt | 1 + .../geometry/boost_polygon_utils.hpp | 43 +++ .../tier4_autoware_utils.hpp | 1 + common/tier4_autoware_utils/package.xml | 1 + .../src/geometry/boost_polygon_utils.cpp | 182 ++++++++++++ .../src/geometry/test_boost_polygon_utils.cpp | 263 ++++++++++++++++++ 6 files changed, 491 insertions(+) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp create mode 100644 common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp create mode 100644 common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index b8b5fb6bb1894..41497402e2d48 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(Boost REQUIRED) ament_auto_add_library(tier4_autoware_utils SHARED src/tier4_autoware_utils.cpp + src/geometry/boost_polygon_utils.cpp ) if(BUILD_TESTING) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp new file mode 100644 index 0000000000000..afafa86884fba --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -0,0 +1,43 @@ +// 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_auto_perception_msgs/msg/predicted_object.hpp" +// #include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include + +#include + +namespace tier4_autoware_utils +{ +namespace bg = boost::geometry; + +bool isClockwise(const Polygon2d & polygon); +Polygon2d inverseClockwise(const Polygon2d & polygon); +geometry_msgs::msg::Polygon rotatePolygon( + const geometry_msgs::msg::Polygon & polygon, const double & angle); +Polygon2d toPolygon2d( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape); +double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); + +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp index 51ad4e74065aa..3f2fed24b7e0b 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -16,6 +16,7 @@ #define TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index f3a665bf7a3ba..193cf657ea830 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -12,6 +12,7 @@ autoware_cmake + autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs builtin_interfaces diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp new file mode 100644 index 0000000000000..7abb9c503cc30 --- /dev/null +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -0,0 +1,182 @@ +// 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. + +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +namespace +{ +namespace bg = boost::geometry; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + bg::append(polygon.outer(), point); +} + +void appendPointToPolygon(Polygon2d & polygon, const Point2d point) +{ + bg::append(polygon.outer(), point); +} + +/* + * NOTE: Area is negative when footprint.points is clock wise. + * Area is positive when footprint.points is anti clock wise. + */ +double getPolygonArea(const geometry_msgs::msg::Polygon & footprint) +{ + double area = 0.0; + + for (size_t i = 0; i < footprint.points.size(); ++i) { + size_t j = (i + 1) % footprint.points.size(); + area += 0.5 * (footprint.points.at(i).x * footprint.points.at(j).y - + footprint.points.at(j).x * footprint.points.at(i).y); + } + + return area; +} + +double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions) +{ + return static_cast(dimensions.x * dimensions.y); +} + +double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions) +{ + return static_cast((dimensions.x / 2.0) * (dimensions.x / 2.0) * M_PI); +} +} // namespace + +namespace tier4_autoware_utils +{ +bool isClockwise(const Polygon2d & polygon) +{ + const int n = polygon.outer().size(); + const double x_offset = polygon.outer().at(0).x(); + const double y_offset = polygon.outer().at(0).y(); + double sum = 0.0; + for (std::size_t i = 0; i < polygon.outer().size(); ++i) { + sum += + (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) - + (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset); + } + + return sum < 0.0; +} + +Polygon2d inverseClockwise(const Polygon2d & polygon) +{ + auto output_polygon = polygon; + boost::geometry::reverse(output_polygon); + return output_polygon; +} + +geometry_msgs::msg::Polygon rotatePolygon( + const geometry_msgs::msg::Polygon & polygon, const double & angle) +{ + const double cos = std::cos(angle); + const double sin = std::sin(angle); + geometry_msgs::msg::Polygon rotated_polygon; + for (const auto & point : polygon.points) { + auto rotated_point = point; + rotated_point.x = cos * point.x - sin * point.y; + rotated_point.y = sin * point.x + cos * point.y; + rotated_polygon.points.push_back(rotated_point); + } + return rotated_polygon; +} + +Polygon2d toPolygon2d( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape) +{ + Polygon2d polygon; + + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const auto point0 = tier4_autoware_utils::calcOffsetPose( + pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) + .position; + const auto point1 = tier4_autoware_utils::calcOffsetPose( + pose, -shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) + .position; + const auto point2 = tier4_autoware_utils::calcOffsetPose( + pose, -shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) + .position; + const auto point3 = tier4_autoware_utils::calcOffsetPose( + pose, shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) + .position; + + appendPointToPolygon(polygon, point0); + appendPointToPolygon(polygon, point1); + appendPointToPolygon(polygon, point2); + appendPointToPolygon(polygon, point3); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + const double radius = shape.dimensions.x / 2.0; + constexpr int circle_discrete_num = 6; + for (int i = 0; i < circle_discrete_num; ++i) { + geometry_msgs::msg::Point point; + point.x = std::cos( + (static_cast(i) / static_cast(circle_discrete_num)) * 2.0 * M_PI + + M_PI / static_cast(circle_discrete_num)) * + radius + + pose.position.x; + point.y = std::sin( + (static_cast(i) / static_cast(circle_discrete_num)) * 2.0 * M_PI + + M_PI / static_cast(circle_discrete_num)) * + radius + + pose.position.y; + appendPointToPolygon(polygon, point); + } + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + const double poly_yaw = tf2::getYaw(pose.orientation); + const auto rotated_footprint = rotatePolygon(shape.footprint, poly_yaw); + for (const auto rel_point : rotated_footprint.points) { + geometry_msgs::msg::Point abs_point; + abs_point.x = pose.position.x + rel_point.x; + abs_point.y = pose.position.y + rel_point.y; + + appendPointToPolygon(polygon, abs_point); + } + } else { + throw std::logic_error("The shape type is not supported in tier4_autoware_utils."); + } + + // NOTE: push back the first point in order to close polygon + if (polygon.outer().size() > 0) { + appendPointToPolygon(polygon, polygon.outer().front()); + } + + return isClockwise(polygon) ? polygon : inverseClockwise(polygon); +} + +double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return getRectangleArea(shape.dimensions); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return getCircleArea(shape.dimensions); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + return getPolygonArea(shape.footprint); + } + + throw std::logic_error("The shape type is not supported in tier4_autoware_utils."); +} +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp new file mode 100644 index 0000000000000..8d2af01ea703a --- /dev/null +++ b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -0,0 +1,263 @@ +// 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. + +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include + +namespace bg = boost::geometry; + +using tier4_autoware_utils::Polygon2d; + +namespace +{ +geometry_msgs::msg::Point32 createPoint32(const double x, const double y) +{ + geometry_msgs::msg::Point32 p; + p.x = x; + p.y = y; + p.z = 0.0; + + return p; +} + +geometry_msgs::msg::Pose createPose(const double x, const double y, const double yaw) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = y; + p.position.z = 0.0; + p.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + + return p; +} +} // namespace + +TEST(boost_geometry, boost_isClockwise) +{ + using tier4_autoware_utils::isClockwise; + + // empty + Polygon2d empty_polygon; + EXPECT_THROW(isClockwise(empty_polygon), std::out_of_range); + + // normal case + Polygon2d clock_wise_polygon{{{0.0, 0.0}, {0.0, 1.0}, {1.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}}}; + EXPECT_TRUE(isClockwise(clock_wise_polygon)); + + Polygon2d anti_clock_wise_polygon{{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}, {0.0, 0.0}}}; + EXPECT_FALSE(isClockwise(anti_clock_wise_polygon)); + + // duplicated + Polygon2d duplicated_clock_wise_polygon{ + {{0.0, 0.0}, {0.0, 1.0}, {0.0, 1.0}, {1.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}}}; + EXPECT_TRUE(isClockwise(duplicated_clock_wise_polygon)); + + Polygon2d duplicated_anti_clock_wise_polygon{ + {{0.0, 0.0}, {1.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}, {0.0, 0.0}}}; + EXPECT_FALSE(isClockwise(duplicated_anti_clock_wise_polygon)); +} + +TEST(boost_geometry, boost_inverseClockwise) +{ + using tier4_autoware_utils::inverseClockwise; + using tier4_autoware_utils::isClockwise; + + // empty + Polygon2d empty_polygon; + const auto reversed_empty_polygon = inverseClockwise(empty_polygon); + EXPECT_EQ(static_cast(reversed_empty_polygon.outer().size()), 0); + + // normal case + Polygon2d clock_wise_polygon{{{0.0, 0.0}, {0.0, 1.0}, {1.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}}}; + EXPECT_FALSE(isClockwise(inverseClockwise(clock_wise_polygon))); + + Polygon2d anti_clock_wise_polygon{{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}, {0.0, 0.0}}}; + EXPECT_TRUE(isClockwise(inverseClockwise(anti_clock_wise_polygon))); + + // duplicated + Polygon2d duplicated_clock_wise_polygon{ + {{0.0, 0.0}, {0.0, 1.0}, {0.0, 1.0}, {1.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}}}; + EXPECT_FALSE(isClockwise(inverseClockwise(duplicated_clock_wise_polygon))); + + Polygon2d duplicated_anti_clock_wise_polygon{ + {{0.0, 0.0}, {1.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0}, {0.0, 0.0}}}; + EXPECT_TRUE(isClockwise(inverseClockwise(duplicated_anti_clock_wise_polygon))); +} + +TEST(boost_geometry, boost_rotatePolygon) +{ + constexpr double epsilon = 1e-6; + using tier4_autoware_utils::rotatePolygon; + + // empty + geometry_msgs::msg::Polygon empty_polygon; + const auto rotated_empty_polygon = rotatePolygon(empty_polygon, 0.0); + EXPECT_EQ(static_cast(rotated_empty_polygon.points.size()), 0); + + // normal case + geometry_msgs::msg::Polygon clock_wise_polygon; + clock_wise_polygon.points.push_back(createPoint32(0.0, 0.0)); + clock_wise_polygon.points.push_back(createPoint32(0.0, 1.0)); + clock_wise_polygon.points.push_back(createPoint32(1.0, 1.0)); + clock_wise_polygon.points.push_back(createPoint32(1.0, 0.0)); + clock_wise_polygon.points.push_back(createPoint32(0.0, 0.0)); + const auto rotated_clock_wise_polygon = rotatePolygon(clock_wise_polygon, M_PI_4); + + EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(0).x, 0.0); + EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(0).y, 0.0); + EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(1).x, -0.70710676908493042); + EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(1).y, 0.70710676908493042); + EXPECT_NEAR(rotated_clock_wise_polygon.points.at(2).x, 0.0, epsilon); + EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(2).y, 1.4142135381698608); + EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(3).x, 0.70710676908493042); + EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(3).y, 0.70710676908493042); + EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(4).x, 0.0); + EXPECT_DOUBLE_EQ(rotated_clock_wise_polygon.points.at(4).y, 0.0); +} + +TEST(boost_geometry, boost_toPolygon2d) +{ + using tier4_autoware_utils::toPolygon2d; + + { // bounding box + const double x = 1.0; + const double y = 1.0; + + const auto pose = createPose(1.0, 1.0, M_PI_4); + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.dimensions.x = x; + shape.dimensions.y = y; + + const auto poly = toPolygon2d(pose, shape); + EXPECT_DOUBLE_EQ(poly.outer().at(0).x(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(0).y(), 1.7071067811865475); + EXPECT_DOUBLE_EQ(poly.outer().at(1).x(), 1.7071067811865475); + EXPECT_DOUBLE_EQ(poly.outer().at(1).y(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(2).x(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(2).y(), 0.29289321881345254); + EXPECT_DOUBLE_EQ(poly.outer().at(3).x(), 0.29289321881345254); + EXPECT_DOUBLE_EQ(poly.outer().at(3).y(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(4).x(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(4).y(), 1.7071067811865475); + } + + { // cylinder + const double diameter = 1.0; + + const auto pose = createPose(1.0, 1.0, M_PI_4); + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + shape.dimensions.x = diameter; + + const auto poly = toPolygon2d(pose, shape); + EXPECT_DOUBLE_EQ(poly.outer().at(0).x(), 1.4330127018922194); + EXPECT_DOUBLE_EQ(poly.outer().at(0).y(), 1.25); + EXPECT_DOUBLE_EQ(poly.outer().at(1).x(), 1.4330127018922194); + EXPECT_DOUBLE_EQ(poly.outer().at(1).y(), 0.75); + EXPECT_DOUBLE_EQ(poly.outer().at(2).x(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(2).y(), 0.5); + EXPECT_DOUBLE_EQ(poly.outer().at(3).x(), 0.56698729810778059); + EXPECT_DOUBLE_EQ(poly.outer().at(3).y(), 0.75); + EXPECT_DOUBLE_EQ(poly.outer().at(4).x(), 0.56698729810778081); + EXPECT_DOUBLE_EQ(poly.outer().at(4).y(), 1.25); + EXPECT_DOUBLE_EQ(poly.outer().at(5).x(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(5).y(), 1.5); + } + + { // polygon + const double x = 0.5; + const double y = 0.5; + + const auto pose = createPose(1.0, 1.0, M_PI_4); + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + shape.footprint.points.push_back(createPoint32(-x, -y)); + shape.footprint.points.push_back(createPoint32(-x, y)); + shape.footprint.points.push_back(createPoint32(x, y)); + shape.footprint.points.push_back(createPoint32(x, -y)); + + const auto poly = toPolygon2d(pose, shape); + EXPECT_DOUBLE_EQ(poly.outer().at(0).x(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(0).y(), 0.29289323091506958); + EXPECT_DOUBLE_EQ(poly.outer().at(1).x(), 0.29289323091506958); + EXPECT_DOUBLE_EQ(poly.outer().at(1).y(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(2).x(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(2).y(), 1.7071067690849304); + EXPECT_DOUBLE_EQ(poly.outer().at(3).x(), 1.7071067690849304); + EXPECT_DOUBLE_EQ(poly.outer().at(3).y(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(4).x(), 1.0); + EXPECT_DOUBLE_EQ(poly.outer().at(4).y(), 0.29289323091506958); + } +} + +TEST(boost_geometry, boost_getArea) +{ + using tier4_autoware_utils::getArea; + + { // bounding box + const double x = 1.0; + const double y = 2.0; + + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.dimensions.x = x; + shape.dimensions.y = y; + + const double area = getArea(shape); + EXPECT_DOUBLE_EQ(area, x * y); + } + + { // cylinder + const double diameter = 1.0; + + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + shape.dimensions.x = diameter; + + const double area = getArea(shape); + EXPECT_DOUBLE_EQ(area, std::pow(diameter / 2.0, 2.0) * M_PI); + } + + { // polygon + const double x = 1.0; + const double y = 2.0; + + // clock wise + autoware_auto_perception_msgs::msg::Shape clock_wise_shape; + clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); + clock_wise_shape.footprint.points.push_back(createPoint32(0.0, y)); + clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); + clock_wise_shape.footprint.points.push_back(createPoint32(x, 0.0)); + + const double clock_wise_area = getArea(clock_wise_shape); + EXPECT_DOUBLE_EQ(clock_wise_area, -x * y); + + // anti clock wise + autoware_auto_perception_msgs::msg::Shape anti_clock_wise_shape; + anti_clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + anti_clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); + anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, 0.0)); + anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); + anti_clock_wise_shape.footprint.points.push_back(createPoint32(0.0, y)); + + const double anti_clock_wise_area = getArea(anti_clock_wise_shape); + EXPECT_DOUBLE_EQ(anti_clock_wise_area, x * y); + } +}