Merge branch 'master' into fix/RJD-1296-fix-random001-ego-issue

.github/workflows/custom_spell.json

@@ -41,6 +41,7 @@
     "TESTRANDOMIZER",
     "travelling",
     "Tschirnhaus",
+    "walltime",
     "xerces",
     "xercesc",
     "yamacir-kit"

common/math/arithmetic/CHANGELOG.rst

@@ -21,6 +21,29 @@ Changelog for package arithmetic
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.1.1 (2024-09-03)
+------------------
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc
+* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto
+
+4.1.0 (2024-09-03)
+------------------
+* Merge branch 'master' into RJD-1278/fix-line-segment
+* Merge branch 'master' into RJD-1278/fix-line-segment
+* Merge branch 'master' into RJD-1278/fix-line-segment
+* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue
+* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D
+* Contributors: Michał Ciasnocha
+
 4.0.4 (2024-09-02)
 ------------------
 * Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar

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>4.0.4</version>
+  <version>4.1.1</version>
   <description>arithmetic library for scenario_simulator_v2</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

common/math/geometry/CHANGELOG.rst

@@ -21,6 +21,49 @@ Changelog for package geometry
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.1.1 (2024-09-03)
+------------------
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control
+* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control
+* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc
+* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto
+
+4.1.0 (2024-09-03)
+------------------
+* Merge pull request `#1353 <https://github.com/tier4/scenario_simulator_v2/issues/1353>`_ from tier4/RJD-1278/fix-line-segment
+  Rjd 1278/fix line segment
+* Merge branch 'master' into RJD-1278/fix-line-segment
+* make const members public
+* Merge branch 'master' into RJD-1278/fix-line-segment
+* Merge branch 'master' into RJD-1278/fix-line-segment
+* remove{}
+* expand on the note, add else to if-stmts
+* note
+* add else to if statements
+* rename getSlope, add consts
+* remove unnecessary lambda
+* simplify denormalize logic
+* use has_value
+* rename getIntersection2DSValue, minor logical fixes regarding 2D vs 3D
+* LineSegment 2D vs 3D indistinction fixes
+* return const& and remove implicit conversions
+* vector fields for LineSegment class, cleanup
+* use isInBounds function
+* combine 2 PR, apply review requests
+* Merge branch 'RJD-1278/fix-1344-getIntersection2DSValue' of github.com:tier4/scenario_simulator_v2 into RJD-1278/fix-line-segment
+* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue
+* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D
+* isIntesect2D initial solution
+* 1344 fix initial solution
+* Contributors: Masaya Kataoka, Michał Ciasnocha, robomic
+
 4.0.4 (2024-09-02)
 ------------------
 * Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar

common/math/geometry/include/geometry/polygon/line_segment.hpp

@@ -29,43 +29,48 @@ namespace geometry
 class LineSegment
 {
 public:
+  const geometry_msgs::msg::Point start_point;
+  const geometry_msgs::msg::Point end_point;
+  const geometry_msgs::msg::Vector3 vector;
+  const geometry_msgs::msg::Vector3 vector_2d;
+  const double length;
+  const double length_2d;
+
   LineSegment(
     const geometry_msgs::msg::Point & start_point, const geometry_msgs::msg::Point & end_point);
   LineSegment(
     const geometry_msgs::msg::Point & start_point, const geometry_msgs::msg::Vector3 & vec,
     double length);
   ~LineSegment();
   LineSegment & operator=(const LineSegment &);
-  const geometry_msgs::msg::Point start_point;
-  const geometry_msgs::msg::Point end_point;
+
+  auto isIntersect2D(const geometry_msgs::msg::Point & point) const -> bool;
+  auto isIntersect2D(const LineSegment & line) const -> bool;
+  auto isInBounds2D(const geometry_msgs::msg::Point & point) const -> bool;
+
   auto getPoint(const double s, const bool denormalize_s = false) const
     -> geometry_msgs::msg::Point;
   auto getPose(const double s, const bool denormalize_s = false, const bool fill_pitch = true) const
     -> geometry_msgs::msg::Pose;
-  auto isIntersect2D(const geometry_msgs::msg::Point & point) const -> bool;
-  auto isIntersect2D(const LineSegment & l0) const -> bool;
-  auto getIntersection2DSValue(
+  auto get2DIntersectionSValue(
     const geometry_msgs::msg::Point & point, const bool denormalize_s = false) const
     -> std::optional<double>;
-  auto getIntersection2DSValue(const LineSegment & line, const bool denormalize_s = false) const
+  auto get2DIntersectionSValue(const LineSegment & line, const bool denormalize_s = false) const
     -> std::optional<double>;
   auto getIntersection2D(const LineSegment & line) const
     -> std::optional<geometry_msgs::msg::Point>;
   auto getSValue(
     const geometry_msgs::msg::Pose & pose, double threshold_distance, bool denormalize_s) const
     -> std::optional<double>;
-  auto getVector() const -> geometry_msgs::msg::Vector3;
   auto getNormalVector() const -> geometry_msgs::msg::Vector3;
-  auto get2DVector() const -> geometry_msgs::msg::Vector3;
-  auto getLength() const -> double;
-  auto get2DLength() const -> double;
-  auto getSlope() const -> double;
+  auto get2DVectorSlope() const -> double;
   auto getSquaredDistanceIn2D(
     const geometry_msgs::msg::Point & point, const double s, const bool denormalize_s = false) const
     -> double;
   auto getSquaredDistanceVector(
     const geometry_msgs::msg::Point & point, const double s, const bool denormalize_s = false) const
     -> geometry_msgs::msg::Vector3;
+  auto relativePointPosition2D(const geometry_msgs::msg::Point & point) const -> int;
 
 private:
   auto denormalize(const std::optional<double> & s, const bool throw_error_on_out_of_range = true)

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>4.0.4</version>
+  <version>4.1.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>

common/math/geometry/src/intersection/intersection.cpp

@@ -13,30 +13,33 @@
 // limitations under the License.
 
 #include <geometry/intersection/intersection.hpp>
+#include <limits>
 #include <optional>
+#include <scenario_simulator_exception/exception.hpp>
 
 namespace math
 {
 namespace geometry
 {
 bool isIntersect2D(const LineSegment & line0, const LineSegment & line1)
 {
-  double s, t;
-  s = (line0.start_point.x - line0.end_point.x) * (line1.start_point.y - line0.start_point.y) -
-      (line0.start_point.y - line0.end_point.y) * (line1.start_point.x - line0.start_point.x);
-  t = (line0.start_point.x - line0.end_point.x) * (line1.end_point.y - line0.start_point.y) -
-      (line0.start_point.y - line0.end_point.y) * (line1.end_point.x - line0.start_point.x);
-  if (s * t > 0) {
-    return false;
-  }
-  s = (line1.start_point.x - line1.end_point.x) * (line0.start_point.y - line1.start_point.y) -
-      (line1.start_point.y - line1.end_point.y) * (line0.start_point.x - line1.start_point.x);
-  t = (line1.start_point.x - line1.end_point.x) * (line0.end_point.y - line1.start_point.y) -
-      (line1.start_point.y - line1.end_point.y) * (line0.end_point.x - line1.start_point.x);
-  if (s * t > 0) {
-    return false;
+  const auto &p0 = line0.start_point, &q0 = line0.end_point;
+  const auto &p1 = line1.start_point, &q1 = line1.end_point;
+
+  const int relative_position_p0 = line1.relativePointPosition2D(p0);
+  const int relative_position_q0 = line1.relativePointPosition2D(q0);
+  const int relative_position_p1 = line0.relativePointPosition2D(p1);
+  const int relative_position_q1 = line0.relativePointPosition2D(q1);
+
+  if (
+    relative_position_p1 == 0 && relative_position_q1 == 0 && relative_position_p0 == 0 &&
+    relative_position_q0 == 0) {
+    return line0.isInBounds2D(p1) || line0.isInBounds2D(q1) || line1.isInBounds2D(p0) ||
+           line1.isInBounds2D(q0);
+  } else {
+    return relative_position_p1 != relative_position_q1 &&
+           relative_position_p0 != relative_position_q0;
   }
-  return true;
 }
 
 bool isIntersect2D(const std::vector<LineSegment> & lines)
@@ -54,21 +57,35 @@ bool isIntersect2D(const std::vector<LineSegment> & lines)
 std::optional<geometry_msgs::msg::Point> getIntersection2D(
   const LineSegment & line0, const LineSegment & line1)
 {
-  if (!isIntersect2D(line0, line1)) {
+  if (not line0.isIntersect2D(line1)) {
     return std::nullopt;
+  } else {
+    // 'line0' represented as a0x + b0y = c0
+    const double a0 = line0.vector_2d.y;
+    const double b0 = -line0.vector_2d.x;
+    const double c0 = a0 * line0.start_point.x + b0 * line0.start_point.y;
+
+    // 'line1' represented as a1x + b1y = c1
+    const double a1 = line1.vector_2d.y;
+    const double b1 = -line1.vector_2d.x;
+    const double c1 = a1 * line1.start_point.x + b1 * line1.start_point.y;
+
+    const double determinant = a0 * b1 - a1 * b0;
+
+    if (std::abs(determinant) <= std::numeric_limits<double>::epsilon()) {
+      // The lines do intersect, but they are collinear and overlap.
+      THROW_SIMULATION_ERROR(
+        "Line segments are collinear. So determinant is zero.",
+        "If this message was displayed, something completely unexpected happens.",
+        "This message is not originally intended to be displayed, if you see it, please "
+        "contact the developer of traffic_simulator.");
+    } else {
+      return geometry_msgs::build<geometry_msgs::msg::Point>()
+        .x((b1 * c0 - b0 * c1) / determinant)
+        .y((a0 * c1 - a1 * c0) / determinant)
+        .z(0.0);
+    }
   }
-  const auto det =
-    (line0.start_point.x - line0.end_point.x) * (line1.end_point.y - line1.start_point.y) -
-    (line1.end_point.x - line1.start_point.x) * (line0.start_point.y - line0.end_point.y);
-  const auto t =
-    ((line1.end_point.y - line1.start_point.y) * (line1.end_point.x - line0.end_point.x) +
-     (line1.start_point.x - line1.end_point.x) * (line1.end_point.y - line0.end_point.y)) /
-    det;
-  geometry_msgs::msg::Point point;
-  point.x = t * line0.start_point.x + (1.0 - t) * line0.end_point.x;
-  point.y = t * line0.start_point.y + (1.0 - t) * line0.end_point.y;
-  point.z = t * line0.start_point.z + (1.0 - t) * line0.end_point.z;
-  return point;
 }
 
 std::vector<geometry_msgs::msg::Point> getIntersection2D(const std::vector<LineSegment> & lines)