Feature/ego slope

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 = false)
+    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 = false) 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 = false) 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/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
   }

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp

@@ -81,6 +81,8 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua
 
   std::vector<Double> results;  // for description
 
+  const bool consider_z;
+
   explicit DistanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 
   auto description() const -> std::string;

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/position.hpp

@@ -26,7 +26,7 @@ namespace openscenario_interpreter
 {
 inline namespace syntax
 {
-/* ---- Position ---------------------------------------------------------------
+/* ---- Position 1.2 -----------------------------------------------------------
  *
  * <xsd:complexType name="Position">
  *   <xsd:choice>
@@ -38,6 +38,8 @@ inline namespace syntax
  *     <xsd:element name="LanePosition" type="LanePosition"/>
  *     <xsd:element name="RelativeLanePosition" type="RelativeLanePosition"/>
  *     <xsd:element name="RoutePosition" type="RoutePosition"/>
+ *     <xsd:element name="GeoPosition" type="GeoPosition" minOccurs="0"/>
+ *     <xsd:element name="TrajectoryPosition" type="TrajectoryPosition" minOccurs="0"/>
  *   </xsd:choice>
  * </xsd:complexType>
  *
@@ -61,6 +63,8 @@ DEFINE_LAZY_VISITOR(
   CASE(LanePosition),
   // CASE(RelativeLanePosition),
   // CASE(RoutePosition),
+  // CASE(GeoPosition),
+  // CASE(TrajectoryPosition),
 );
 
 DEFINE_LAZY_VISITOR(
@@ -73,6 +77,8 @@ DEFINE_LAZY_VISITOR(
   CASE(LanePosition),
   // CASE(RelativeLanePosition),
   // CASE(RoutePosition),
+  // CASE(GeoPosition),
+  // CASE(TrajectoryPosition),
 );
 }  // namespace syntax
 }  // namespace openscenario_interpreter

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp

@@ -49,6 +49,8 @@ struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversio
 
   std::vector<Double> results;  // for description
 
+  const bool consider_z;
+
   explicit ReachPositionCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 
   auto description() const -> String;

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp

@@ -83,6 +83,8 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi
 
   std::vector<Double> results;  // for description
 
+  const bool consider_z;
+
   explicit RelativeDistanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 
   auto description() const -> String;

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/world_position.hpp

@@ -27,7 +27,7 @@ namespace openscenario_interpreter
 {
 inline namespace syntax
 {
-/* ---- WorldPosition ----------------------------------------------------------
+/* ---- WorldPosition 1.2 ------------------------------------------------------
  *
  *  <xsd:complexType name="WorldPosition">
  *    <xsd:attribute name="x" type="Double" use="required"/>

openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp

@@ -30,7 +30,7 @@ auto AcquirePositionAction::start() -> void
 {
   const auto acquire_position = overload(
     [](const WorldPosition & position, auto && actor) {
-      return applyAcquirePositionAction(actor, static_cast<geometry_msgs::msg::Pose>(position));
+      return applyAcquirePositionAction(actor, static_cast<NativeWorldPosition>(position));
     },
     [](const RelativeWorldPosition & position, auto && actor) {
       return applyAcquirePositionAction(actor, static_cast<NativeLanePosition>(position));

openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp

@@ -21,6 +21,7 @@
 #include <openscenario_interpreter/syntax/scenario_object.hpp>
 #include <openscenario_interpreter/utility/overload.hpp>
 #include <openscenario_interpreter/utility/print.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <sstream>
 
 // NOTE: Ignore spell miss due to OpenSCENARIO standard.
@@ -42,7 +43,12 @@ DistanceCondition::DistanceCondition(
   value(readAttribute<Double>("value", node, scope)),
   position(readElement<Position>("Position", node, scope)),
   triggering_entities(triggering_entities),
-  results(triggering_entities.entity_refs.size(), Double::nan())
+  results(triggering_entities.entity_refs.size(), Double::nan()),
+  consider_z([]() {
+    rclcpp::Node node{"get_parameter", "simulation"};
+    node.declare_parameter("consider_pose_by_road_slope", false);
+    return node.get_parameter("consider_pose_by_road_slope").as_bool();
+  }())
 {
 }
 
@@ -101,6 +107,16 @@ auto DistanceCondition::distance(const EntityRef & triggering_entity) const -> d
   return std::numeric_limits<double>::quiet_NaN();
 }
 
+// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z)
+static double hypot(const double x, const double y, const double z, const bool consider_z)
+{
+  if (consider_z) {
+    return std::hypot(x, y, z);
+  } else {
+    return std::hypot(x, y);
+  }
+}
+
 template <>
 auto DistanceCondition::distance<
   CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, false>(
@@ -111,22 +127,30 @@ auto DistanceCondition::distance<
       [&](const WorldPosition & position) {
         const auto relative_world = makeNativeRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
-        return std::hypot(relative_world.position.x, relative_world.position.y);
+        return hypot(
+          relative_world.position.x, relative_world.position.y, relative_world.position.z,
+          consider_z);
       },
       [&](const RelativeWorldPosition & position) {
         const auto relative_world = makeNativeRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
-        return std::hypot(relative_world.position.x, relative_world.position.y);
+        return hypot(
+          relative_world.position.x, relative_world.position.y, relative_world.position.z,
+          consider_z);
       },
       [&](const RelativeObjectPosition & position) {
         const auto relative_world = makeNativeRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
-        return std::hypot(relative_world.position.x, relative_world.position.y);
+        return hypot(
+          relative_world.position.x, relative_world.position.y, relative_world.position.z,
+          consider_z);
       },
       [&](const LanePosition & position) {
         const auto relative_world = makeNativeRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
-        return std::hypot(relative_world.position.x, relative_world.position.y);
+        return hypot(
+          relative_world.position.x, relative_world.position.y, relative_world.position.z,
+          consider_z);
       }),
     position);
 }
@@ -141,22 +165,30 @@ auto DistanceCondition::distance<
       [&](const WorldPosition & position) {
         const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
-        return std::hypot(relative_world.position.x, relative_world.position.y);
+        return hypot(
+          relative_world.position.x, relative_world.position.y, relative_world.position.z,
+          consider_z);
       },
       [&](const RelativeWorldPosition & position) {
         const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
-        return std::hypot(relative_world.position.x, relative_world.position.y);
+        return hypot(
+          relative_world.position.x, relative_world.position.y, relative_world.position.z,
+          consider_z);
       },
       [&](const RelativeObjectPosition & position) {
         const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
-        return std::hypot(relative_world.position.x, relative_world.position.y);
+        return hypot(
+          relative_world.position.x, relative_world.position.y, relative_world.position.z,
+          consider_z);
       },
       [&](const LanePosition & position) {
         const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
-        return std::hypot(relative_world.position.x, relative_world.position.y);
+        return hypot(
+          relative_world.position.x, relative_world.position.y, relative_world.position.z,
+          consider_z);
       }),
     position);
 }

openscenario/openscenario_interpreter/src/syntax/position.cpp

@@ -31,7 +31,9 @@ Position::Position(const pugi::xml_node & node, Scope & scope)
       std::make_pair(  "RelativeRoadPosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }),
       std::make_pair(          "LanePosition", [&](auto && node) { return make<         LanePosition>(node, scope); }),
       std::make_pair(  "RelativeLanePosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }),
-      std::make_pair(         "RoutePosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; })))
+      std::make_pair(         "RoutePosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }),
+      std::make_pair(           "GeoPosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }),
+      std::make_pair(    "TrajectoryPosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; })))
 // clang-format on
 {
 }