Merge branch 'RJD-1057-remove-functions-forwarded-to-entity-base-midd…

…le' into RJD-1057-remove-functions-forwarded-to-entity-base-refactor
tier4 · Dec 18, 2024 · 7ba59db · 7ba59db
2 parents fa3e08e + f2b2dfc
commit 7ba59db
Show file tree

Hide file tree

Showing 60 changed files with 371 additions and 44 deletions.
diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst
@@ -21,6 +21,15 @@ Changelog for package arithmetic
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Contributors: Kotaro Yoshimoto
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge branch 'master' into feature/multi-level-lanelet-support

diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>arithmetic</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>arithmetic library for scenario_simulator_v2</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst
@@ -21,6 +21,21 @@ Changelog for package geometry
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge pull request `#1475 <https://github.com/tier4/scenario_simulator_v2/issues/1475>`_ from tier4/fix/math-closest-point
+  Fix bug in `math::geometry::getClosestPoses`
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Fix spell mistake
+* Fix getClosestPoses with naive algorithm
+* Apply clang-format
+* Add some test cases for getClosestPoses
+* Contributors: Kotaro Yoshimoto, f0reachARR
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge pull request `#1481 <https://github.com/tier4/scenario_simulator_v2/issues/1481>`_ from tier4/feature/multi-level-lanelet-support

diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>geometry</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>geometry math library for scenario_simulator_v2 application</description>
   <maintainer email="[email protected]">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/math/geometry/src/bounding_box.cpp b/common/math/geometry/src/bounding_box.cpp
@@ -62,22 +62,12 @@ std::optional<std::pair<geometry_msgs::msg::Pose, geometry_msgs::msg::Pose>> 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<double>::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<std::pair<geometry_msgs::msg::Pose, geometry_msgs::msg::Pose>> 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));
   }

diff --git a/common/math/geometry/test/src/test_bounding_box.cpp b/common/math/geometry/test/src/test_bounding_box.cpp
@@ -19,6 +19,7 @@
 #include <scenario_simulator_exception/exception.hpp>
 
 #include "expect_eq_macros.hpp"
+#include "geometry/distance.hpp"
 #include "test_utils.hpp"
 
 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(BoundingBox, 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(BoundingBox, 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(BoundingBox, 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);

diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst
@@ -21,6 +21,15 @@ Changelog for package scenario_simulator_exception
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Contributors: Kotaro Yoshimoto
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge branch 'master' into feature/multi-level-lanelet-support

diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>scenario_simulator_exception</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>Exception types for scenario simulator</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst
@@ -21,6 +21,15 @@ Changelog for package junit_exporter
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Contributors: Kotaro Yoshimoto
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge branch 'master' into feature/multi-level-lanelet-support

diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>simple_junit</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>Lightweight JUnit library for ROS 2</description>
   <maintainer email="[email protected]">Masaya Kataoka</maintainer>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>

diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst
@@ -21,6 +21,15 @@ Changelog for package status_monitor
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Contributors: Kotaro Yoshimoto
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge branch 'master' into feature/multi-level-lanelet-support

diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>status_monitor</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>none</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst
@@ -21,6 +21,15 @@ Changelog for package concealer
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Contributors: Kotaro Yoshimoto
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge branch 'master' into feature/multi-level-lanelet-support

diff --git a/external/concealer/package.xml b/external/concealer/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>concealer</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator.</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst
@@ -24,6 +24,15 @@ Changelog for package embree_vendor
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Contributors: Kotaro Yoshimoto
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge branch 'master' into feature/multi-level-lanelet-support

diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>embree_vendor</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>vendor packages for intel raytracing kernel library</description>
   <maintainer email="[email protected]">masaya</maintainer>
   <license>Apache 2.0</license>

diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst
@@ -21,6 +21,15 @@ Changelog for package kashiwanoha_map
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Contributors: Kotaro Yoshimoto
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge branch 'master' into feature/multi-level-lanelet-support

diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>kashiwanoha_map</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>map package for kashiwanoha</description>
   <maintainer email="[email protected]">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>

diff --git a/map/simple_cross_map/CHANGELOG.rst b/map/simple_cross_map/CHANGELOG.rst
@@ -9,6 +9,15 @@ Changelog for package simple_cross_map
 * Merge branch 'master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Contributors: Kotaro Yoshimoto
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge branch 'master' into feature/multi-level-lanelet-support

diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>simple_cross_map</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>map package for simple cross</description>
   <maintainer email="[email protected]">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>

diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst
@@ -21,6 +21,15 @@ Changelog for package cpp_mock_scenarios
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+7.3.1 (2024-12-17)
+------------------
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Merge branch 'master' into fix/math-closest-point
+* Contributors: Kotaro Yoshimoto
+
 7.3.0 (2024-12-16)
 ------------------
 * Merge branch 'master' into feature/multi-level-lanelet-support

diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cpp_mock_scenarios</name>
-  <version>7.3.0</version>
+  <version>7.3.1</version>
   <description>C++ mock scenarios</description>
   <maintainer email="[email protected]">masaya</maintainer>
   <license>Apache License 2.0</license>