Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2…

… into feature/reset_behavior_plugin

Signed-off-by: Masaya Kataoka <[email protected]>

.github/workflows/CheckMessageDepsUpdate.yaml

@@ -8,18 +8,13 @@ jobs:
   check_message_updates:
     name: Check message updates
     runs-on: ubuntu-22.04
-    container: ros:humble
     steps:
       - name: checkout
         uses: actions/checkout@v4
         with:
           fetch-depth: 0
           path: scenario_simulator_v2
 
-      - name: Install dependencies
-        run: |
-          vcs import scenario_simulator_v2/external < scenario_simulator_v2/dependency_humble.repos
-
       - name: Set up Python
         uses: actions/setup-python@v4
         with:
@@ -29,6 +24,11 @@ jobs:
         with:
           poetry-version: "1.5.1"
 
+      - name: Install dependencies
+        run: |
+          pip3 install -U vcstool
+          vcs import scenario_simulator_v2/external < scenario_simulator_v2/dependency_humble.repos
+
       - name: Install Python dependencies
         run: |
           cd scenario_simulator_v2

.github/workflows/SimModelUpdateNotification.yaml

@@ -9,6 +9,11 @@ jobs:
   notify:
     runs-on: ubuntu-latest
     steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+
       - name: Add comment
         run: |
           gh pr comment  "$PR_NUMBER" --edit-last --body "This PR edits vehicle model that is copied from [simple_planning_simulator](https://github.com/autowarefoundation/autoware.universe/tree/main/simulator/simple_planning_simulator). Please consider making changes to the original code to avoid confusion or consult developers (@hakuturu583, @yamacir-kit and @HansRobo )." \

common/math/arithmetic/CHANGELOG.rst

@@ -2,6 +2,28 @@
 Changelog for package arithmetic
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.6.1 (2024-03-19)
+------------------
+
+1.6.0 (2024-03-14)
+------------------
+
+1.5.1 (2024-03-13)
+------------------
+
+1.5.0 (2024-03-12)
+------------------
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+  # Conflicts:
+  #	simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp
+  #	simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp
+  #	simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp
+  #	test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Contributors: Kotaro Yoshimoto
+
 1.4.2 (2024-03-01)
 ------------------
 

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>1.4.2</version>
+  <version>1.6.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

@@ -2,6 +2,42 @@
 Changelog for package geometry
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.6.1 (2024-03-19)
+------------------
+
+1.6.0 (2024-03-14)
+------------------
+
+1.5.1 (2024-03-13)
+------------------
+
+1.5.0 (2024-03-12)
+------------------
+* Merge pull request `#1209 <https://github.com/tier4/scenario_simulator_v2/issues/1209>`_ from tier4/feature/ego_slope
+  Consider road slope in distance measurement and entity poses
+* chore: enable flag defaultly
+* Update common/math/geometry/src/spline/hermite_curve.cpp
+  Co-authored-by: Masaya Kataoka <[email protected]>
+* chore: apply formatter
+* feat: add fill_pitch option to CatmullRomSpline::getPose
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* chore(geometry): add some tests for LineSegment::getPose
+* refactor: rename fit_orientation_to_lanelet to fill_pitch in HermiteCurve::getPose
+* refactor: add fill_pitch option to LineSegment::getPose
+* refactor: add fit_orientation_to_lanelet option to HermiteCurve::getPose
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+  # Conflicts:
+  #	simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp
+  #	simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp
+  #	simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp
+  #	test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge branch 'master' into feature/ego_slope
+* fix test case
+* update slop calculation logic
+* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Masaya Kataoka
+
 1.4.2 (2024-03-01)
 ------------------
 

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

@@ -40,7 +40,8 @@ class LineSegment
   const geometry_msgs::msg::Point end_point;
   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 -> geometry_msgs::msg::Pose;
+  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(

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

@@ -39,7 +39,7 @@ class CatmullRomSpline : public CatmullRomSplineInterface
   auto getPoint(const double s, const double offset) const -> geometry_msgs::msg::Point;
   auto getTangentVector(const double s) const -> geometry_msgs::msg::Vector3;
   auto getNormalVector(const double s) const -> geometry_msgs::msg::Vector3;
-  auto getPose(const double s) const -> geometry_msgs::msg::Pose;
+  auto getPose(const double s, const bool fill_pitch = true) const -> geometry_msgs::msg::Pose;
   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>;

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

@@ -48,7 +48,8 @@ class HermiteCurve
   std::vector<geometry_msgs::msg::Point> getTrajectory(size_t num_points = 30) const;
   const std::vector<geometry_msgs::msg::Point> getTrajectory(
     double start_s, double end_s, double resolution, bool denormalize_s = false) const;
-  const geometry_msgs::msg::Pose getPose(double s, bool denormalize_s = false) const;
+  const geometry_msgs::msg::Pose getPose(
+    double s, bool denormalize_s = false, bool fill_pitch = true) const;
   const geometry_msgs::msg::Point getPoint(double s, bool denormalize_s = false) const;
   const geometry_msgs::msg::Vector3 getTangentVector(double s, bool denormalize_s = false) const;
   const geometry_msgs::msg::Vector3 getNormalVector(double s, bool denormalize_s = false) const;

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>1.4.2</version>
+  <version>1.6.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/polygon/line_segment.cpp

@@ -93,18 +93,23 @@ auto LineSegment::getPoint(const double s, const bool denormalize_s) const
  * @brief Get pose on the line segment from s value. Orientation of thee return value was calculated from the vector of the line segment.
  * @param s Normalized s value in coordinate along line segment.
  * @param denormalize_s If true, s value should be normalized in range [0,1]. If false, s value is not normalized.
+ * @param fill_pitch If true, the pitch value of the orientation is filled. If false, the pitch value of the orientation is 0.
+ *        This parameter is introduced for backward-compatibility.
  * @return geometry_msgs::msg::Pose 
  */
-auto LineSegment::getPose(const double s, const bool denormalize_s) const
+auto LineSegment::getPose(const double s, const bool denormalize_s, const bool fill_pitch) const
   -> geometry_msgs::msg::Pose
 {
   return geometry_msgs::build<geometry_msgs::msg::Pose>()
     .position(getPoint(s, denormalize_s))
-    .orientation([this]() -> geometry_msgs::msg::Quaternion {
+    .orientation([this, fill_pitch]() -> geometry_msgs::msg::Quaternion {
       const auto tangent_vec = getVector();
       return quaternion_operation::convertEulerAngleToQuaternion(
-        geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.0).y(0.0).z(
-          std::atan2(tangent_vec.y, tangent_vec.x)));
+        geometry_msgs::build<geometry_msgs::msg::Vector3>()
+          .x(0.0)
+          .y(
+            fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0)
+          .z(std::atan2(tangent_vec.y, tangent_vec.x)));
     }());
 }
 

common/math/geometry/src/spline/catmull_rom_spline.cpp

@@ -641,7 +641,8 @@ auto CatmullRomSpline::getTangentVector(const double s) const -> geometry_msgs::
   }
 }
 
-auto CatmullRomSpline::getPose(const double s) const -> geometry_msgs::msg::Pose
+auto CatmullRomSpline::getPose(const double s, const bool fill_pitch) const
+  -> geometry_msgs::msg::Pose
 {
   switch (control_points.size()) {
     case 0:
@@ -664,10 +665,10 @@ auto CatmullRomSpline::getPose(const double s) const -> geometry_msgs::msg::Pose
           "This message is not originally intended to be displayed, if you see it, please "
           "contact the developer of traffic_simulator.");
       }
-      return line_segments_[0].getPose(s, true);
+      return line_segments_[0].getPose(s, true, fill_pitch);
     default:
       const auto index_and_s = getCurveIndexAndS(s);
-      return curves_[index_and_s.first].getPose(index_and_s.second, true);
+      return curves_[index_and_s.first].getPose(index_and_s.second, true, fill_pitch);
   }
 }
 

common/math/geometry/src/spline/hermite_curve.cpp

@@ -279,7 +279,9 @@ const geometry_msgs::msg::Vector3 HermiteCurve::getTangentVector(double s, bool
   return vec;
 }
 
-const geometry_msgs::msg::Pose HermiteCurve::getPose(double s, bool denormalize_s) const
+/// @note fill_pitch is introduced for backward-compatibility.
+const geometry_msgs::msg::Pose HermiteCurve::getPose(
+  double s, bool denormalize_s, bool fill_pitch) const
 {
   if (denormalize_s) {
     s = s / getLength();
@@ -288,7 +290,7 @@ const geometry_msgs::msg::Pose HermiteCurve::getPose(double s, bool denormalize_
   geometry_msgs::msg::Vector3 tangent_vec = getTangentVector(s, false);
   geometry_msgs::msg::Vector3 rpy;
   rpy.x = 0.0;
-  rpy.y = 0.0;
+  rpy.y = fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0;
   rpy.z = std::atan2(tangent_vec.y, tangent_vec.x);
   pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(rpy);
   pose.position = getPoint(s);

common/math/geometry/test/test_catmull_rom_spline.cpp

@@ -30,6 +30,14 @@ math::geometry::CatmullRomSpline makeLine()
   return math::geometry::CatmullRomSpline(points);
 }
 
+/// @brief Helper function generating sloped line: p(0,0,0)-> p(1,3,1) -> p(2,6,2)
+math::geometry::CatmullRomSpline makeSlopedLine()
+{
+  const std::vector<geometry_msgs::msg::Point> points{
+    makePoint(0.0, 0.0, 0.0), makePoint(1.0, 3.0, 1.0), makePoint(2.0, 6.0, 2.0)};
+  return math::geometry::CatmullRomSpline(points);
+}
+
 /// @brief Helper function generating curve: p(0,0)-> p(1,1)-> p(2,0)
 math::geometry::CatmullRomSpline makeCurve()
 {
@@ -465,6 +473,18 @@ TEST(CatmullRomSpline, getPoseLine)
     quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, std::atan(3.0))), EPS);
 }
 
+TEST(CatmullRomSpline, getPoseLineWithPitch)
+{
+  const math::geometry::CatmullRomSpline spline = makeSlopedLine();
+  const auto pose = spline.getPose(std::hypot(1.5, 4.5, 1.5), true);
+  EXPECT_POINT_NEAR(pose.position, makePoint(1.5, 4.5, 1.5), EPS);
+  EXPECT_QUATERNION_NEAR(
+    pose.orientation,
+    quaternion_operation::convertEulerAngleToQuaternion(
+      makeVector(0.0, std::atan2(-1.0, std::sqrt(1.0 + 3.0 * 3.0)), std::atan(3.0))),
+    EPS);
+}
+
 TEST(CatmullRomSpline, getPoseCurve)
 {
   const math::geometry::CatmullRomSpline spline = makeCurve();
@@ -474,6 +494,15 @@ TEST(CatmullRomSpline, getPoseCurve)
   EXPECT_QUATERNION_NEAR(pose.orientation, geometry_msgs::msg::Quaternion(), eps);
 }
 
+TEST(CatmullRomSpline, getPoseCurveWithPitch)
+{
+  const math::geometry::CatmullRomSpline spline = makeCurve();
+  const auto pose = spline.getPose(1.5, true);
+  constexpr double eps = 0.02;
+  EXPECT_POINT_NEAR(pose.position, makePoint(1.0, 1.0), eps);
+  EXPECT_QUATERNION_NEAR(pose.orientation, geometry_msgs::msg::Quaternion(), eps);
+}
+
 TEST(CatmullRomSpline, getSValue1)
 {
   const std::vector<geometry_msgs::msg::Point> points{

common/math/geometry/test/test_line_segment.cpp

@@ -387,21 +387,33 @@ TEST(LineSegment, GetPose)
     /// Orientation should be defined by the direction of the `line`, so the orientation should be parallel to the z axis.
     // [Snippet_getPose_with_s_0]
     EXPECT_POSE_EQ(
-      line.getPose(0, false),
+      line.getPose(0, false, false),
       geometry_msgs::build<geometry_msgs::msg::Pose>()
         .position(geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(0).z(0))
         .orientation(geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(0).y(0).z(0).w(1)));
+    EXPECT_POSE_EQ(
+      line.getPose(0, false, true),
+      geometry_msgs::build<geometry_msgs::msg::Pose>()
+        .position(geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(0).z(0))
+        .orientation(quaternion_operation::convertEulerAngleToQuaternion(
+          geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0).y(-M_PI * 0.5).z(0))));
     // [Snippet_getPose_with_s_0]
     /// @snippet test/test_line_segment.cpp Snippet_getPose_with_s_0
 
     /// @note When the `s = 1`(1st argument of the `LineSegment::getPose` function), the return value of the `LineSegment::getPose` function should be an end point of the `line`.
     /// Orientation should be defined by the direction of the `line`, so the orientation should be parallel to the z axis.
     // [Snippet_getPose_with_s_1]
     EXPECT_POSE_EQ(
-      line.getPose(1, false),
+      line.getPose(1, false, false),
       geometry_msgs::build<geometry_msgs::msg::Pose>()
         .position(geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(0).z(1))
         .orientation(geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(0).y(0).z(0).w(1)));
+    EXPECT_POSE_EQ(
+      line.getPose(1, false, true),
+      geometry_msgs::build<geometry_msgs::msg::Pose>()
+        .position(geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(0).z(1))
+        .orientation(quaternion_operation::convertEulerAngleToQuaternion(
+          geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0).y(-M_PI * 0.5).z(0))));
     // [Snippet_getPose_with_s_1]
     /// @snippet test/test_line_segment.cpp Snippet_getPose_with_s_1
   }

common/scenario_simulator_exception/CHANGELOG.rst

@@ -2,6 +2,29 @@
 Changelog for package scenario_simulator_exception
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.6.1 (2024-03-19)
+------------------
+
+1.6.0 (2024-03-14)
+------------------
+
+1.5.1 (2024-03-13)
+------------------
+
+1.5.0 (2024-03-12)
+------------------
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+  # Conflicts:
+  #	simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp
+  #	simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp
+  #	simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp
+  #	test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge branch 'master' into feature/ego_slope
+* Contributors: Dawid Moszyński, Kotaro Yoshimoto
+
 1.4.2 (2024-03-01)
 ------------------
 

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>1.4.2</version>
+  <version>1.6.1</version>
   <description>Exception types for scenario simulator</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

common/simple_junit/CHANGELOG.rst

@@ -2,6 +2,29 @@
 Changelog for package junit_exporter
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.6.1 (2024-03-19)
+------------------
+
+1.6.0 (2024-03-14)
+------------------
+
+1.5.1 (2024-03-13)
+------------------
+
+1.5.0 (2024-03-12)
+------------------
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+  # Conflicts:
+  #	simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp
+  #	simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp
+  #	simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp
+  #	test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge remote-tracking branch 'origin/master' into feature/ego_slope
+* Merge branch 'master' into feature/ego_slope
+* Contributors: Dawid Moszyński, Kotaro Yoshimoto
+
 1.4.2 (2024-03-01)
 ------------------
 

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>1.4.2</version>
+  <version>1.6.1</version>
   <description>Lightweight JUnit library for ROS 2</description>
   <maintainer email="[email protected]">Masaya Kataoka</maintainer>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>