Merge branch 'master' into feature/set_route

Author: HansRobo

common/agnocast_wrapper/CHANGELOG.rst

@@ -5,6 +5,14 @@ Changelog for package agnocast_wrapper
 16.4.2 (2025-05-23)
 -------------------
 
+16.4.4 (2025-05-28)
+-------------------
+* Merge remote-tracking branch 'origin/master' into feature/delay_curventure_calculation
+* Merge branch 'master' into feature/delay_curventure_calculation
+* Merge remote-tracking branch 'origin/master' into feature/delay_curventure_calculation
+* Merge remote-tracking branch 'origin/master' into feature/speed_up
+* Contributors: Masaya Kataoka
+
 16.4.3 (2025-05-27)
 -------------------
 * Merge branch 'master' into refactor/behavior-tree-2

common/agnocast_wrapper/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>agnocast_wrapper</name>
-  <version>16.4.3</version>
+  <version>16.4.4</version>
   <description>A wrapper package for agnocast</description>
   <maintainer email="[email protected]">Mateusz Palczuk</maintainer>
   <license>Apache License 2.0</license>

common/get_parameter/CHANGELOG.rst

@@ -5,6 +5,14 @@ Changelog for package get_parameter
 16.4.2 (2025-05-23)
 -------------------
 
+16.4.4 (2025-05-28)
+-------------------
+* Merge remote-tracking branch 'origin/master' into feature/delay_curventure_calculation
+* Merge branch 'master' into feature/delay_curventure_calculation
+* Merge remote-tracking branch 'origin/master' into feature/delay_curventure_calculation
+* Merge remote-tracking branch 'origin/master' into feature/speed_up
+* Contributors: Masaya Kataoka
+
 16.4.3 (2025-05-27)
 -------------------
 * Merge branch 'master' into refactor/behavior-tree-2

common/get_parameter/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>get_parameter</name>
-  <version>16.4.3</version>
+  <version>16.4.4</version>
   <description>Thin wrapper for rclcpp::Node::get_parameter</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

common/math/arithmetic/CHANGELOG.rst

@@ -24,6 +24,14 @@ Changelog for package arithmetic
 16.4.2 (2025-05-23)
 -------------------
 
+16.4.4 (2025-05-28)
+-------------------
+* Merge remote-tracking branch 'origin/master' into feature/delay_curventure_calculation
+* Merge branch 'master' into feature/delay_curventure_calculation
+* Merge remote-tracking branch 'origin/master' into feature/delay_curventure_calculation
+* Merge remote-tracking branch 'origin/master' into feature/speed_up
+* Contributors: Masaya Kataoka
+
 16.4.3 (2025-05-27)
 -------------------
 * Merge branch 'master' into refactor/behavior-tree-2

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>16.4.3</version>
+  <version>16.4.4</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

@@ -24,6 +24,28 @@ Changelog for package geometry
 16.4.2 (2025-05-23)
 -------------------
 
+16.4.4 (2025-05-28)
+-------------------
+* Merge pull request `#1609 <https://github.com/tier4/scenario_simulator_v2/issues/1609>`_ from tier4/feature/delay_curventure_calculation
+  Feature/delay curventure calculation
+* check resolution is not zero
+* use const &
+* Merge remote-tracking branch 'origin/master' into feature/delay_curventure_calculation
+* Merge branch 'master' into feature/delay_curventure_calculation
+* apply reformat
+* make isApproximatelyEqualTo function to public
+* Merge remote-tracking branch 'origin/master' into feature/delay_curventure_calculation
+* fix test case
+* fix typo
+* check waypoint duplication
+* delay curventure calculation
+* delay curventure calculation
+* Merge remote-tracking branch 'origin/master' into feature/speed_up
+* enable calculate route before requestFollowTrajectory
+* enable construct polyline trajectory from spline
+* Speed up by changing to logic that does not use exceptions
+* Contributors: Masaya Kataoka, Taiga
+
 16.4.3 (2025-05-27)
 -------------------
 * Merge branch 'master' into refactor/behavior-tree-2

common/math/geometry/include/geometry/solver/polynomial_solver.hpp

@@ -101,6 +101,15 @@ class PolynomialSolver
    */
   constexpr static double tolerance = 1e-7;
 
+  /**
+   * @brief check the value0 and value1 is equal or not with considering tolerance.
+   * @param value0 the value you want to compare
+   * @param value1 the value you want to compared
+   * @return true value0 and value1 are equal
+   * @return false value0 and value1 are not equal
+   */
+  auto isApproximatelyEqualTo(const double value0, const double value1) const -> bool;
+
 private:
   /**
    * @brief solve cubic equation x^3 + a*x^2 + b*x + c = 0
@@ -119,14 +128,6 @@ class PolynomialSolver
   auto filterByRange(
     const std::vector<double> & values, const double min_value, const double max_value) const
     -> std::vector<double>;
-  /**
-   * @brief check the value0 and value1 is equal or not with considering tolerance.
-   * @param value0 the value you want to compare
-   * @param value1 the value you want to compared
-   * @return true value0 and value1 are equal
-   * @return false value0 and value1 are not equal
-   */
-  auto isApproximatelyEqualTo(const double value0, const double value1) const -> bool;
 };
 }  // namespace geometry
 }  // namespace math

common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp

@@ -19,8 +19,10 @@
 #include <geometry/spline/catmull_rom_spline_interface.hpp>
 #include <geometry/spline/hermite_curve.hpp>
 #include <geometry_msgs/msg/point.hpp>
+#include <memory>
 #include <optional>
 #include <string>
+#include <traffic_simulator_msgs/msg/polyline_trajectory.hpp>
 #include <utility>
 #include <vector>
 
@@ -33,6 +35,9 @@ class CatmullRomSpline : public CatmullRomSplineInterface
 public:
   CatmullRomSpline() = default;
   explicit CatmullRomSpline(const std::vector<geometry_msgs::msg::Point> & control_points);
+  explicit CatmullRomSpline(
+    const geometry_msgs::msg::Point & start_point,
+    const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & trajectory);
   auto getLength() const -> double override { return total_length_; }
   auto getMaximum2DCurvature() const -> double;
   auto getPoint(const double s) const -> geometry_msgs::msg::Point;
@@ -43,6 +48,8 @@ class CatmullRomSpline : public CatmullRomSplineInterface
   auto getTrajectory(
     const double start_s, const double end_s, const double resolution,
     const double offset = 0.0) const -> std::vector<geometry_msgs::msg::Point>;
+  auto getTrajectoryPoses(const double start_s, const double end_s, const double resolution) const
+    -> std::vector<geometry_msgs::msg::Pose>;
   auto getSValue(const geometry_msgs::msg::Pose & pose, double threshold_distance = 3.0) const
     -> std::optional<double>;
   auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point & point, const double s) const
@@ -78,7 +85,8 @@ class CatmullRomSpline : public CatmullRomSplineInterface
   std::vector<LineSegment> line_segments_;
   std::vector<HermiteCurve> curves_;
   std::vector<double> length_list_;
-  std::vector<double> maximum_2d_curvatures_;
+  /// @note Since curvature calculation requires a large computational cost, it is inefficient to calculate it in the member initializer list and must be calculated when the getMaximum2DCurvature function is first called.
+  mutable std::vector<double> maximum_2d_curvatures_;
   double total_length_;
 };
 }  // namespace geometry

common/math/geometry/include/geometry/vector3/operator.hpp

@@ -20,6 +20,7 @@
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <limits>
 
 namespace math
 {
@@ -131,6 +132,16 @@ auto operator+=(T & a, const U & b) -> decltype(auto)
   a.z += b.z;
   return a;
 }
+
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeVector3<T>, IsLikeVector3<U>>, std::nullptr_t> =
+    nullptr>
+auto operator==(const T & a, const U & b) -> bool
+{
+  constexpr decltype(a.x) e = std::numeric_limits<decltype(a.x)>::epsilon();
+  return (std::abs(a.x - b.x) < e) && (std::abs(a.y - b.y) < e) && (std::abs(a.z - b.z) < e);
+}
 }  // namespace geometry
 }  // namespace math