From 17b965be5bdbf2142ccba26215024017ce00e0b3 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 5 Dec 2024 12:20:09 +0900 Subject: [PATCH 1/4] Add some test cases for getClosestPoses --- .../geometry/test/src/test_bounding_box.cpp | 69 +++++++++++++++++-- 1 file changed, 65 insertions(+), 4 deletions(-) diff --git a/common/math/geometry/test/src/test_bounding_box.cpp b/common/math/geometry/test/src/test_bounding_box.cpp index b3449f994e4..a954c2c734a 100644 --- a/common/math/geometry/test/src/test_bounding_box.cpp +++ b/common/math/geometry/test/src/test_bounding_box.cpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "expect_eq_macros.hpp" +#include "geometry/distance.hpp" +#include "test_utils.hpp" #include #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include TEST(BoundingBox, getPointsFromBboxDefault) { @@ -83,7 +84,8 @@ TEST(BoundingBox, toPolygon2D_onlyTranslation) } /** - * @note Test obtaining polygon from bounding box with full transformation applied (translation + rotation). + * @note Test obtaining polygon from bounding box with full transformation applied (translation + + * rotation). */ TEST(BoundingBox, toPolygon2D_fullPose) { @@ -135,6 +137,65 @@ TEST(BoundingBox, getPolygonDistanceWithoutCollision) EXPECT_DOUBLE_EQ(ans.value(), 3.0); } +TEST(BoudingBox, getClosestPoses) +{ + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0); + geometry_msgs::msg::Pose pose0; + geometry_msgs::msg::Pose pose1 = makePose(5.0, 5.0); + + { + const auto actual = math::geometry::getClosestPoses(pose0, bbox, pose1, bbox); + ASSERT_TRUE(actual); + + geometry_msgs::msg::Pose expected_pose0 = makePose(0.5, 0.5); + geometry_msgs::msg::Pose expected_pose1 = makePose(4.5, 4.5); + EXPECT_POSE_EQ(actual.value().first, expected_pose1); + EXPECT_POSE_EQ(actual.value().second, expected_pose0); + } + + { // reverse order + const auto actual = math::geometry::getClosestPoses(pose1, bbox, pose0, bbox); + ASSERT_TRUE(actual); + + geometry_msgs::msg::Pose expected_pose0 = makePose(0.5, 0.5); + geometry_msgs::msg::Pose expected_pose1 = makePose(4.5, 4.5); + EXPECT_POSE_EQ(actual.value().first, expected_pose0); + EXPECT_POSE_EQ(actual.value().second, expected_pose1); + } +} + +TEST(BoudingBox, getClosestPosesWithAlmostTouch) +{ + traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(1.0, 1.0, 1.0); + geometry_msgs::msg::Pose pose0; + traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 10.0, 1.0); + geometry_msgs::msg::Pose pose1 = makePose(1.1, 0.0); + + { + const auto actual = math::geometry::getClosestPoses(pose0, bbox0, pose1, bbox1); + ASSERT_TRUE(actual); + const auto distance = math::geometry::getDistance(actual.value().first, actual.value().second); + EXPECT_NEAR(distance, 0.1, 0.0001); + } + + { // reverse order + const auto actual = math::geometry::getClosestPoses(pose1, bbox1, pose0, bbox0); + ASSERT_TRUE(actual); + const auto distance = math::geometry::getDistance(actual.value().first, actual.value().second); + EXPECT_NEAR(distance, 0.1, 0.0001); + } +} + +TEST(BoudingBox, getClosestPosesWithIntersection) +{ + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0); + geometry_msgs::msg::Pose pose0; + geometry_msgs::msg::Pose pose1 = makePose(0.6, 0.6); + + const auto actual = math::geometry::getClosestPoses(pose0, bbox, pose1, bbox); + ASSERT_FALSE(actual); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From dc04dcf9d2816d471195f17d09e038357c2f65ec Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 5 Dec 2024 12:47:38 +0900 Subject: [PATCH 2/4] Apply clang-format --- common/math/geometry/test/src/test_bounding_box.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/math/geometry/test/src/test_bounding_box.cpp b/common/math/geometry/test/src/test_bounding_box.cpp index a954c2c734a..d213e98f1e4 100644 --- a/common/math/geometry/test/src/test_bounding_box.cpp +++ b/common/math/geometry/test/src/test_bounding_box.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "expect_eq_macros.hpp" -#include "geometry/distance.hpp" -#include "test_utils.hpp" +#include #include #include #include -#include +#include "expect_eq_macros.hpp" +#include "geometry/distance.hpp" +#include "test_utils.hpp" TEST(BoundingBox, getPointsFromBboxDefault) { From 4afe1e73728c4e9c5925a73bde0363916a1fa5f0 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 5 Dec 2024 12:47:58 +0900 Subject: [PATCH 3/4] Fix getClosestPoses with naive algorithm --- common/math/geometry/src/bounding_box.cpp | 27 +++++++++++------------ 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/common/math/geometry/src/bounding_box.cpp b/common/math/geometry/src/bounding_box.cpp index db3d75b2f9f..49b90bf48dc 100644 --- a/common/math/geometry/src/bounding_box.cpp +++ b/common/math/geometry/src/bounding_box.cpp @@ -62,22 +62,12 @@ std::optional> get const auto poly0 = toPolygon2D(pose0, bbox0); const auto poly1 = toPolygon2D(pose1, bbox1); - if (boost::geometry::intersects(poly0, poly1)) { - return std::nullopt; - } - if (boost::geometry::intersects(poly1, poly0)) { - return std::nullopt; - } if (boost::geometry::disjoint(poly0, poly1)) { auto point0 = boost_point(); auto point1 = boost_point(); auto min_distance = boost::numeric::bounds::highest(); - auto segments = boost::make_iterator_range( - boost::geometry::segments_begin(poly0), boost::geometry::segments_end(poly0)); - auto points = boost::make_iterator_range( - boost::geometry::points_begin(poly1), boost::geometry::points_end(poly1)); - auto findNearestPointInSegment = [&](const auto & segment, const auto & points) { + auto findNearestPointToSegment = [&](const auto & segment, const auto & points) { for (auto && point : points) { auto nearest_point_from_segment = pointToSegmentProjection(point, *segment.first, *segment.second); @@ -90,9 +80,18 @@ std::optional> get } }; - for (auto && segment : segments) { - findNearestPointInSegment(segment, points); - } + auto findNearestPointInPolygon = [&](const auto & poly0, const auto & poly1) { + auto segments = boost::make_iterator_range( + boost::geometry::segments_begin(poly0), boost::geometry::segments_end(poly0)); + auto points = boost::geometry::exterior_ring(poly1); + + for (auto && segment : segments) { + findNearestPointToSegment(segment, points); + } + }; + + findNearestPointInPolygon(poly0, poly1); + findNearestPointInPolygon(poly1, poly0); return std::make_pair(toPose(point0), toPose(point1)); } From 9f3abdbd4ab520ad52bbeb987ac5fa0e04a62217 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 5 Dec 2024 12:54:00 +0900 Subject: [PATCH 4/4] Fix spell mistake --- common/math/geometry/test/src/test_bounding_box.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/common/math/geometry/test/src/test_bounding_box.cpp b/common/math/geometry/test/src/test_bounding_box.cpp index d213e98f1e4..00232501d72 100644 --- a/common/math/geometry/test/src/test_bounding_box.cpp +++ b/common/math/geometry/test/src/test_bounding_box.cpp @@ -137,7 +137,7 @@ TEST(BoundingBox, getPolygonDistanceWithoutCollision) EXPECT_DOUBLE_EQ(ans.value(), 3.0); } -TEST(BoudingBox, getClosestPoses) +TEST(BoundingBox, getClosestPoses) { traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0); geometry_msgs::msg::Pose pose0; @@ -164,7 +164,7 @@ TEST(BoudingBox, getClosestPoses) } } -TEST(BoudingBox, getClosestPosesWithAlmostTouch) +TEST(BoundingBox, getClosestPosesWithAlmostTouch) { traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(1.0, 1.0, 1.0); geometry_msgs::msg::Pose pose0; @@ -186,7 +186,7 @@ TEST(BoudingBox, getClosestPosesWithAlmostTouch) } } -TEST(BoudingBox, getClosestPosesWithIntersection) +TEST(BoundingBox, getClosestPosesWithIntersection) { traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(1.0, 1.0, 1.0); geometry_msgs::msg::Pose pose0;