diff --git a/.github/workflows/SimModelUpdateNotification.yaml b/.github/workflows/SimModelUpdateNotification.yaml
index ee34a04c446..1525a9d40a1 100644
--- a/.github/workflows/SimModelUpdateNotification.yaml
+++ b/.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 )." \
diff --git a/common/math/geometry/include/geometry/polygon/line_segment.hpp b/common/math/geometry/include/geometry/polygon/line_segment.hpp
index c160b1e3ec2..ec17419f31d 100644
--- a/common/math/geometry/include/geometry/polygon/line_segment.hpp
+++ b/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(
diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp
index 0c9d47bec1d..947242a4fad 100644
--- a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp
+++ b/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>;
diff --git a/common/math/geometry/include/geometry/spline/hermite_curve.hpp b/common/math/geometry/include/geometry/spline/hermite_curve.hpp
index a1afdb4f0c7..1722b05f951 100644
--- a/common/math/geometry/include/geometry/spline/hermite_curve.hpp
+++ b/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;
diff --git a/common/math/geometry/src/polygon/line_segment.cpp b/common/math/geometry/src/polygon/line_segment.cpp
index 5bc4f67e9fe..827d529660c 100644
--- a/common/math/geometry/src/polygon/line_segment.cpp
+++ b/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)));
     }());
 }
 
diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp
index baf7a66eaf9..11c2143373d 100644
--- a/common/math/geometry/src/spline/catmull_rom_spline.cpp
+++ b/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);
   }
 }
 
diff --git a/common/math/geometry/src/spline/hermite_curve.cpp b/common/math/geometry/src/spline/hermite_curve.cpp
index a06eb25b530..d04254adf16 100644
--- a/common/math/geometry/src/spline/hermite_curve.cpp
+++ b/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);
diff --git a/common/math/geometry/test/test_catmull_rom_spline.cpp b/common/math/geometry/test/test_catmull_rom_spline.cpp
index a3f2976855d..0ec17c5b42c 100644
--- a/common/math/geometry/test/test_catmull_rom_spline.cpp
+++ b/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{
diff --git a/common/math/geometry/test/test_line_segment.cpp b/common/math/geometry/test/test_line_segment.cpp
index 021a4f59a36..cb646135004 100644
--- a/common/math/geometry/test/test_line_segment.cpp
+++ b/common/math/geometry/test/test_line_segment.cpp
@@ -387,10 +387,16 @@ 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
 
@@ -398,10 +404,16 @@ 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_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
   }
diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp
index 11174493820..6016eb0d322 100644
--- a/external/concealer/src/autoware_universe.cpp
+++ b/external/concealer/src/autoware_universe.cpp
@@ -156,8 +156,8 @@ auto AutowareUniverse::getGearCommand() const -> autoware_auto_vehicle_msgs::msg
 auto AutowareUniverse::getGearSign() const -> double
 {
   using autoware_auto_vehicle_msgs::msg::GearCommand;
-  // @todo Add support for GearCommand::NONE to return 0.0
-  // @sa https://github.com/autowarefoundation/autoware.universe/blob/main/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L475
+  /// @todo Add support for GearCommand::NONE to return 0.0
+  /// @sa https://github.com/autowarefoundation/autoware.universe/blob/main/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L475
   return getGearCommand().command == GearCommand::REVERSE or
              getGearCommand().command == GearCommand::REVERSE_2
            ? -1.0
diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp
index 0a0945e0b6f..5c30020102f 100644
--- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp
+++ b/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;
diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/position.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/position.hpp
index 7b76e000dfe..4141742d53d 100644
--- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/position.hpp
+++ b/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
diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp
index 35e98272ce5..527614ad500 100644
--- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp
+++ b/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;
diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp
index 6aa1d54cbda..73593152962 100644
--- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp
+++ b/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;
diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/world_position.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/world_position.hpp
index 19dc86ca37e..0e99ca4317b 100644
--- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/world_position.hpp
+++ b/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"/>
diff --git a/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp b/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp
index 2141bef7bbd..89f2f6a4232 100644
--- a/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp
+++ b/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));
diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp
index c56a27791f8..8711ad4d2f9 100644
--- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp
+++ b/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,12 @@ 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)
+{
+  return consider_z ? std::hypot(x, y, z) : std::hypot(x, y);
+}
+
 template <>
 auto DistanceCondition::distance<
   CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, false>(
@@ -111,22 +123,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 +161,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);
 }
diff --git a/openscenario/openscenario_interpreter/src/syntax/double.cpp b/openscenario/openscenario_interpreter/src/syntax/double.cpp
index 0886c72b0cd..101bb74d7ee 100644
--- a/openscenario/openscenario_interpreter/src/syntax/double.cpp
+++ b/openscenario/openscenario_interpreter/src/syntax/double.cpp
@@ -97,7 +97,7 @@ auto operator>>(std::istream & is, Double & datum) -> std::istream &
 
 auto operator<<(std::ostream & os, const Double & datum) -> std::ostream &
 {
-  return os << std::fixed << datum.data;
+  return os << std::fixed << std::setprecision(30) << datum.data;
 }
 }  // namespace syntax
 }  // namespace openscenario_interpreter
diff --git a/openscenario/openscenario_interpreter/src/syntax/position.cpp b/openscenario/openscenario_interpreter/src/syntax/position.cpp
index 1d1c9067764..39764457b8c 100644
--- a/openscenario/openscenario_interpreter/src/syntax/position.cpp
+++ b/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
 {
 }
diff --git a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp
index 6e37f90ca3f..07dc4e81d06 100644
--- a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp
+++ b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp
@@ -18,6 +18,7 @@
 #include <openscenario_interpreter/syntax/reach_position_condition.hpp>
 #include <openscenario_interpreter/utility/overload.hpp>
 #include <openscenario_interpreter/utility/print.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <traffic_simulator/helper/helper.hpp>
 
 namespace openscenario_interpreter
@@ -30,7 +31,12 @@ ReachPositionCondition::ReachPositionCondition(
   position(readElement<Position>("Position", node, scope)),
   compare(Rule::lessThan),
   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();
+  }())
 {
 }
 
@@ -47,6 +53,12 @@ auto ReachPositionCondition::description() const -> String
   return description.str();
 }
 
+// @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)
+{
+  return consider_z ? std::hypot(x, y, z) : std::hypot(x, y);
+}
+
 auto ReachPositionCondition::evaluate() -> Object
 {
   // TODO USE DistanceCondition::distance
@@ -54,22 +66,22 @@ auto ReachPositionCondition::evaluate() -> Object
     [&](const WorldPosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return std::hypot(pose.position.x, pose.position.y);
+      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
     },
     [&](const RelativeWorldPosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return std::hypot(pose.position.x, pose.position.y);
+      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
     },
     [&](const RelativeObjectPosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return std::hypot(pose.position.x, pose.position.y);
+      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
     },
     [&](const LanePosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return std::hypot(pose.position.x, pose.position.y);
+      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
     });
 
   results.clear();
diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp
index e2669fd8dfe..5db70e68fa1 100644
--- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp
+++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp
@@ -36,7 +36,12 @@ RelativeDistanceCondition::RelativeDistanceCondition(
   rule(readAttribute<Rule>("rule", node, scope)),
   value(readAttribute<Double>("value", 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();
+  }())
 {
 }
 
@@ -68,6 +73,9 @@ auto RelativeDistanceCondition::distance<
   }
 }
 
+/**
+ * @note This implementation differs from the OpenSCENARIO standard. See the section "6.4. Distances" in the OpenSCENARIO User Guide.
+ */
 template <>
 auto RelativeDistanceCondition::distance<
   CoordinateSystem::entity, RelativeDistanceType::longitudinal, true>(
@@ -97,6 +105,9 @@ auto RelativeDistanceCondition::distance<
   }
 }
 
+/**
+ * @note This implementation differs from the OpenSCENARIO standard. See the section "6.4. Distances" in the OpenSCENARIO User Guide.
+ */
 template <>
 auto RelativeDistanceCondition::distance<
   CoordinateSystem::entity, RelativeDistanceType::lateral, true>(
@@ -112,6 +123,12 @@ auto RelativeDistanceCondition::distance<
   }
 }
 
+// @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)
+{
+  return consider_z ? std::hypot(x, y, z) : std::hypot(x, y);
+}
+
 template <>
 auto RelativeDistanceCondition::distance<
   CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, true>(
@@ -120,9 +137,11 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added and
     global().entities->at(entity_ref).as<ScenarioObject>().is_added) {
-    return std::hypot(
+    return hypot(
       makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x,
-      makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y);
+      makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y,
+      makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.z,
+      consider_z);
   } else {
     return Double::nan();
   }
@@ -136,9 +155,10 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added and
     global().entities->at(entity_ref).as<ScenarioObject>().is_added) {
-    return std::hypot(
+    return hypot(
       makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x,
-      makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y);
+      makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y,
+      makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.z, consider_z);
   } else {
     return Double::nan();
   }
diff --git a/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp b/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp
index c8314e69878..4e59e603519 100644
--- a/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp
+++ b/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp
@@ -56,7 +56,7 @@ auto TeleportAction::teleport(const EntityRef & entity_ref, const Position & pos
 {
   auto teleport = overload(
     [&](const WorldPosition & position) {
-      return applyTeleportAction(entity_ref, static_cast<geometry_msgs::msg::Pose>(position));
+      return applyTeleportAction(entity_ref, static_cast<NativeWorldPosition>(position));
     },
     [&](const RelativeWorldPosition & position) {
       return applyTeleportAction(
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp
index f25eb8b7dcb..ce7ac052d91 100644
--- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp
+++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp
@@ -136,6 +136,7 @@ class ScenarioSimulator : public rclcpp::Node
     -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse;
 
   int getSocketPort();
+
   std::vector<traffic_simulator_msgs::VehicleParameters> ego_vehicles_;
   std::vector<traffic_simulator_msgs::VehicleParameters> vehicles_;
   std::vector<traffic_simulator_msgs::PedestrianParameters> pedestrians_;
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp
index 34cd85d89a1..cd5107362a7 100644
--- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp
+++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp
@@ -60,6 +60,8 @@ class EgoEntitySimulation
 
   const bool consider_acceleration_by_road_slope_;
 
+  const bool consider_pose_by_road_slope_;
+
 public:
   const std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
 
@@ -88,7 +90,7 @@ class EgoEntitySimulation
   explicit EgoEntitySimulation(
     const traffic_simulator_msgs::msg::VehicleParameters &, double,
     const std::shared_ptr<hdmap_utils::HdMapUtils> &, const rclcpp::Parameter & use_sim_time,
-    const bool consider_acceleration_by_road_slope);
+    const bool consider_acceleration_by_road_slope, const bool consider_pose_by_road_slope);
 
   auto overwrite(
     const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time,
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp
index edc91b94a27..c91ec05a327 100644
--- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp
+++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp
@@ -27,7 +27,7 @@
 #include "eigen3/Eigen/Core"
 #include "eigen3/Eigen/LU"
 
-// @note copied from https://github.com/tier4/autoware.universe/blob/v0.17.0/common/interpolation/include/interpolation/interpolation_utils.hpp
+/// @note copied from https://github.com/tier4/autoware.universe/blob/v0.17.0/common/interpolation/include/interpolation/interpolation_utils.hpp
 namespace interpolation_utils
 {
 inline bool isIncreasing(const std::vector<double> & x)
@@ -119,7 +119,7 @@ void validateKeysAndValues(
 }
 }  // namespace interpolation_utils
 
-// @note copied from https://github.com/tier4/autoware.universe/blob/v0.17.0/common/interpolation/include/interpolation/linear_interpolation.hpp
+/// @note copied from https://github.com/tier4/autoware.universe/blob/v0.17.0/common/interpolation/include/interpolation/linear_interpolation.hpp
 namespace interpolation
 {
 double lerp(const double src_val, const double dst_val, const double ratio);
@@ -137,7 +137,7 @@ double lerp(
 using Table = std::vector<std::vector<std::string>>;
 using Map = std::vector<std::vector<double>>;
 
-// @note copied from https://github.com/autowarefoundation/autoware.universe/blob/main/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp
+/// @note copied from https://github.com/autowarefoundation/autoware.universe/blob/main/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp
 class CSVLoader
 {
 public:
diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp
index edf4e02418f..5c1944a57cb 100644
--- a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp
+++ b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp
@@ -82,6 +82,13 @@ auto OccupancyGridSensor<nav_msgs::msg::OccupancyGrid>::getOccupancyGrid(
       throw SimulationRuntimeError("Failed to calculate ego pose with north up.");
     }
     simulation_interface::toMsg(ego->pose(), ego_pose_north_up);
+    /**
+     * @note
+     * There is no problem with the yaw axis being north-up, but unless the pitch and roll axes are
+     * adjusted to the slope of the road surface, the grid map will not project the obstacles correctly.
+     * However, the current implementation of autoware.universe does not consider the roll and pitch axes,
+     * so it is not considered here either.
+     */
     ego_pose_north_up.orientation = geometry_msgs::msg::Quaternion();
   }
 
diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp
index 2a5a52b6cc6..ad7d92218e2 100644
--- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp
+++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp
@@ -220,10 +220,16 @@ auto ScenarioSimulator::spawnVehicleEntity(
       }
       return get_parameter("consider_acceleration_by_road_slope").as_bool();
     };
+    auto get_consider_pose_by_road_slope = [&]() {
+      if (!has_parameter("consider_pose_by_road_slope")) {
+        declare_parameter("consider_pose_by_road_slope", false);
+      }
+      return get_parameter("consider_pose_by_road_slope").as_bool();
+    };
     ego_entity_simulation_ = std::make_shared<vehicle_simulation::EgoEntitySimulation>(
       parameters, step_time_, hdmap_utils_,
       get_parameter_or("use_sim_time", rclcpp::Parameter("use_sim_time", false)),
-      get_consider_acceleration_by_road_slope());
+      get_consider_acceleration_by_road_slope(), get_consider_pose_by_road_slope());
     traffic_simulator_msgs::msg::EntityStatus initial_status;
     initial_status.name = parameters.name;
     simulation_interface::toMsg(req.pose(), initial_status.pose);
diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp
index e279f08601f..c06fe1f7a93 100644
--- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp
+++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp
@@ -34,13 +34,15 @@ static auto getParameter(const std::string & name, T value = {})
 EgoEntitySimulation::EgoEntitySimulation(
   const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils,
-  const rclcpp::Parameter & use_sim_time, const bool consider_acceleration_by_road_slope)
+  const rclcpp::Parameter & use_sim_time, const bool consider_acceleration_by_road_slope,
+  const bool consider_pose_by_road_slope)
 : autoware(std::make_unique<concealer::AutowareUniverse>()),
   vehicle_model_type_(getVehicleModelType()),
   vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)),
   hdmap_utils_ptr_(hdmap_utils),
   vehicle_parameters(parameters),
-  consider_acceleration_by_road_slope_(consider_acceleration_by_road_slope)
+  consider_acceleration_by_road_slope_(consider_acceleration_by_road_slope),
+  consider_pose_by_road_slope_(consider_pose_by_road_slope)
 {
   autoware->set_parameter(use_sim_time);
 }
@@ -108,8 +110,8 @@ auto EgoEntitySimulation::makeSimulationModel(
   const auto steer_time_delay           = getParameter<double>("steer_time_delay",            0.24);
   const auto vel_lim                    = getParameter<double>("vel_lim",                     parameters.performance.max_speed);  // 50.0
   const auto vel_rate_lim               = getParameter<double>("vel_rate_lim",                parameters.performance.max_acceleration);  // 7.0
-  const auto vel_time_constant          = getParameter<double>("vel_time_constant",           0.1);  // @note 0.5 is default value on simple_planning_simulator
-  const auto vel_time_delay             = getParameter<double>("vel_time_delay",              0.1);  // @note 0.25 is default value on simple_planning_simulator
+  const auto vel_time_constant          = getParameter<double>("vel_time_constant",           0.1);  /// @note 0.5 is default value on simple_planning_simulator
+  const auto vel_time_delay             = getParameter<double>("vel_time_delay",              0.1);  /// @note 0.25 is default value on simple_planning_simulator
   const auto wheel_base                 = getParameter<double>("wheel_base",                  parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x);
   // clang-format on
 
@@ -318,8 +320,8 @@ auto EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus(
   const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const
   -> std::optional<traffic_simulator_msgs::msg::LaneletPose>
 {
-  // @note The lanelet matching algorithm should be equivalent to the one used in
-  // EgoEntity::setMapPose
+  /// @note The lanelet matching algorithm should be equivalent to the one used in
+  /// EgoEntity::setMapPose
   const auto unique_route_lanelets =
     traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets());
   const auto matching_length = [entity_width] { return entity_width * 0.5 + 1.0; }();
@@ -353,7 +355,7 @@ auto EgoEntitySimulation::calculateEgoPitch() const -> double
 
   auto centerline_points = hdmap_utils_ptr_->getCenterPoints(ego_lanelet.value().lanelet_id);
 
-  // @note Copied from motion_util::findNearestSegmentIndex
+  /// @note Copied from motion_util::findNearestSegmentIndex
   auto find_nearest_segment_index = [](
                                       const std::vector<geometry_msgs::msg::Point> & points,
                                       const geometry_msgs::msg::Point & point) {
@@ -385,11 +387,11 @@ auto EgoEntitySimulation::calculateEgoPitch() const -> double
   const auto & prev_point = centerline_points.at(ego_seg_idx);
   const auto & next_point = centerline_points.at(ego_seg_idx + 1);
 
-  // @note Calculate ego yaw angle on lanelet coordinates
+  /// @note Calculate ego yaw angle on lanelet coordinates
   const double lanelet_yaw = std::atan2(next_point.y - prev_point.y, next_point.x - prev_point.x);
   const double ego_yaw_against_lanelet = vehicle_model_ptr_->getYaw() - lanelet_yaw;
 
-  // @note calculate ego pitch angle considering ego yaw.
+  /// @note calculate ego pitch angle considering ego yaw.
   const double diff_z = next_point.z - prev_point.z;
   const double diff_xy = std::hypot(next_point.x - prev_point.x, next_point.y - prev_point.y) /
                          std::cos(ego_yaw_against_lanelet);
@@ -506,6 +508,20 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet(
       hdmap_utils_ptr_->getCenterPoints(lanelet_pose->lanelet_id));
     if (const auto s_value = spline.getSValue(status.pose)) {
       status.pose.position.z = spline.getPoint(s_value.value()).z;
+      if (consider_pose_by_road_slope_) {
+        const auto rpy = quaternion_operation::convertQuaternionToEulerAngle(
+          spline.getPose(s_value.value(), true).orientation);
+        const auto original_rpy =
+          quaternion_operation::convertQuaternionToEulerAngle(status.pose.orientation);
+        status.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(
+          geometry_msgs::build<geometry_msgs::msg::Vector3>()
+            .x(original_rpy.x)
+            .y(rpy.y)
+            .z(original_rpy.z));
+        lanelet_pose->rpy =
+          quaternion_operation::convertQuaternionToEulerAngle(quaternion_operation::getRotation(
+            spline.getPose(s_value.value(), true).orientation, status.pose.orientation));
+      }
     }
     status.lanelet_pose_valid = true;
     status.lanelet_pose = lanelet_pose.value();
diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/speed_change.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/speed_change.hpp
index 3330ca8fe56..c4ceb892c20 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/data_type/speed_change.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/speed_change.hpp
@@ -24,9 +24,9 @@ namespace traffic_simulator
 namespace speed_change
 {
 enum class Transition {
-  // @todo Add CUBIC transition.
+  /// @todo Add CUBIC transition.
   LINEAR,
-  // @todo Add SINUSOIDAL transition.
+  /// @todo Add SINUSOIDAL transition.
   STEP,
   AUTO
 };
@@ -34,7 +34,7 @@ enum class Transition {
 struct Constraint
 {
   enum class Type {
-    // @todo Add DISTANCE constraint type.
+    /// @todo Add DISTANCE constraint type.
     LONGITUDINAL_ACCELERATION,
     TIME,
     NONE
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
index 9a7d3029d1b..dd923a3fea8 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
@@ -519,8 +519,6 @@ class EntityManager
         entity_status.lanelet_pose = static_cast<LaneletPose>(pose);
         entity_status.lanelet_pose_valid = true;
       } else {
-        entity_status.pose = pose;
-
         /// @note If the entity is pedestrian or misc object, we have to consider matching to crosswalk lanelet.
         if (const auto lanelet_pose = toLaneletPose(
               pose, parameters.bounding_box,
@@ -542,8 +540,15 @@ class EntityManager
             lanelet_pose) {
           entity_status.lanelet_pose = *lanelet_pose;
           entity_status.lanelet_pose_valid = true;
+          /// @note fix z, roll and pitch to fitting to the lanelet
+          if (getParameter<bool>("consider_pose_by_road_slope", false)) {
+            entity_status.pose = hdmap_utils_ptr_->toMapPose(*lanelet_pose).pose;
+          } else {
+            entity_status.pose = pose;
+          }
         } else {
           entity_status.lanelet_pose_valid = false;
+          entity_status.pose = pose;
         }
       }
 
diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
index 24375e4b211..94efa8e3eae 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
@@ -298,8 +298,8 @@ class HdMapUtils
   auto toMapPoints(const lanelet::Id, const std::vector<double> & s) const
     -> std::vector<geometry_msgs::msg::Point>;
 
-  auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &) const
-    -> geometry_msgs::msg::PoseStamped;
+  auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &, const bool fill_pitch = true)
+    const -> geometry_msgs::msg::PoseStamped;
 
 private:
   /** @defgroup cache
diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
index c17cebc71c6..c56b7544b52 100644
--- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
+++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
@@ -1393,7 +1393,8 @@ auto HdMapUtils::toMapPoints(const lanelet::Id lanelet_id, const std::vector<dou
   return ret;
 }
 
-auto HdMapUtils::toMapPose(const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const
+auto HdMapUtils::toMapPose(
+  const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, const bool fill_pitch) const
   -> geometry_msgs::msg::PoseStamped
 {
   if (
@@ -1409,7 +1410,7 @@ auto HdMapUtils::toMapPose(const traffic_simulator_msgs::msg::LaneletPose & lane
     const auto tangent_vec = spline->getTangentVector(pose->s);
     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);
     ret.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(rpy) *
                            quaternion_operation::convertEulerAngleToQuaternion(pose->rpy);
diff --git a/test_runner/random_test_runner/include/random_test_runner/lanelet_utils.hpp b/test_runner/random_test_runner/include/random_test_runner/lanelet_utils.hpp
index 431225d09d3..5203d1068cd 100644
--- a/test_runner/random_test_runner/include/random_test_runner/lanelet_utils.hpp
+++ b/test_runner/random_test_runner/include/random_test_runner/lanelet_utils.hpp
@@ -59,7 +59,8 @@ class LaneletUtils
     double max_distance);
 
   std::vector<int64_t> getLaneletIds();
-  geometry_msgs::msg::PoseStamped toMapPose(traffic_simulator_msgs::msg::LaneletPose lanelet_pose);
+  geometry_msgs::msg::PoseStamped toMapPose(
+    const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, const bool fill_pitch);
   std::vector<int64_t> getRoute(int64_t from_lanelet_id, int64_t to_lanelet_id);
   double getLaneletLength(int64_t lanelet_id);
   bool isInLanelet(int64_t lanelet_id, double s);
diff --git a/test_runner/random_test_runner/src/lanelet_utils.cpp b/test_runner/random_test_runner/src/lanelet_utils.cpp
index 13a0301c656..e8159857e68 100644
--- a/test_runner/random_test_runner/src/lanelet_utils.cpp
+++ b/test_runner/random_test_runner/src/lanelet_utils.cpp
@@ -49,9 +49,9 @@ LaneletUtils::LaneletUtils(const boost::filesystem::path & filename)
 std::vector<int64_t> LaneletUtils::getLaneletIds() { return hdmap_utils_ptr_->getLaneletIds(); }
 
 geometry_msgs::msg::PoseStamped LaneletUtils::toMapPose(
-  traffic_simulator_msgs::msg::LaneletPose lanelet_pose)
+  const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, const bool fill_pitch)
 {
-  return hdmap_utils_ptr_->toMapPose(lanelet_pose);
+  return hdmap_utils_ptr_->toMapPose(lanelet_pose, fill_pitch);
 }
 
 std::vector<int64_t> LaneletUtils::getRoute(int64_t from_lanelet_id, int64_t to_lanelet_id)
@@ -118,7 +118,7 @@ std::optional<traffic_simulator_msgs::msg::LaneletPose> LaneletUtils::getOpposit
   perpendicular_vector.z = 0.0;
   perpendicular_vector = math::geometry::normalize(perpendicular_vector);
 
-  geometry_msgs::msg::Point global_position_p = toMapPose(pose).pose.position;
+  geometry_msgs::msg::Point global_position_p = toMapPose(pose, false).pose.position;
   geometry_msgs::msg::Vector3 global_position;
   global_position.x = global_position_p.x;
   global_position.y = global_position_p.y;
diff --git a/test_runner/random_test_runner/src/test_randomizer.cpp b/test_runner/random_test_runner/src/test_randomizer.cpp
index 66ff94909aa..f47b7ab0746 100644
--- a/test_runner/random_test_runner/src/test_randomizer.cpp
+++ b/test_runner/random_test_runner/src/test_randomizer.cpp
@@ -51,7 +51,7 @@ TestDescription TestRandomizer::generate()
     test_suite_parameters_.ego_goal_lanelet_id, test_suite_parameters_.ego_goal_s,
     test_suite_parameters_.ego_goal_partial_randomization,
     test_suite_parameters_.ego_goal_partial_randomization_distance);
-  ret.ego_goal_pose = lanelet_utils_->toMapPose(ret.ego_goal_position).pose;
+  ret.ego_goal_pose = lanelet_utils_->toMapPose(ret.ego_goal_position, false).pose;
 
   std::vector<LaneletPart> lanelets_around_start = lanelet_utils_->getLanesWithinDistance(
     ret.ego_start_position, test_suite_parameters_.npc_min_spawn_distance_from_ego,
diff --git a/test_runner/random_test_runner/test/test_lanelet_utils.cpp b/test_runner/random_test_runner/test/test_lanelet_utils.cpp
index 0171d21472c..80798159d2a 100644
--- a/test_runner/random_test_runner/test/test_lanelet_utils.cpp
+++ b/test_runner/random_test_runner/test/test_lanelet_utils.cpp
@@ -201,9 +201,9 @@ TEST(LaneletUtils, getLaneletIds)
 TEST(LaneletUtils, toMapPose)
 {
   const traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 10.0);
-  EXPECT_NO_THROW(getLaneletUtils().toMapPose(pose));
+  EXPECT_NO_THROW(getLaneletUtils().toMapPose(pose, false));
   EXPECT_POSE_NEAR(
-    getLaneletUtils().toMapPose(pose).pose,
+    getLaneletUtils().toMapPose(pose, false).pose,
     geometry_msgs::build<geometry_msgs::msg::Pose>()
       .position(geometry_msgs::build<geometry_msgs::msg::Point>()
                   .x(3747.3511648127)
@@ -217,6 +217,26 @@ TEST(LaneletUtils, toMapPose)
     EPS);
 }
 
+TEST(LaneletUtils, toMapPoseWithFillingPitch)
+{
+  const traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 10.0);
+  EXPECT_NO_THROW(getLaneletUtils().toMapPose(pose, true));
+  /// @note orientation data is output as of #1103
+  EXPECT_POSE_NEAR(
+    getLaneletUtils().toMapPose(pose, true).pose,
+    geometry_msgs::build<geometry_msgs::msg::Pose>()
+      .position(geometry_msgs::build<geometry_msgs::msg::Point>()
+                  .x(3747.3511648127)
+                  .y(73735.0699484234)
+                  .z(0.3034051453))
+      .orientation(geometry_msgs::build<geometry_msgs::msg::Quaternion>()
+                     .x(-0.0091567745565503053)
+                     .y(-0.0022143853886961245)
+                     .z(-0.97193900717756765)
+                     .w(0.23504428583514828)),
+    EPS);
+}
+
 TEST(LaneletUtils, getRoute_turn)
 {
   EXPECT_NO_THROW(getLaneletUtils().getRoute(34681, 34513));
diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
index dc95b256dfc..b77ee4fc93f 100755
--- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
+++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
@@ -64,6 +64,7 @@ def launch_setup(context, *args, **kwargs):
     autoware_launch_file                = LaunchConfiguration("autoware_launch_file",                   default=default_autoware_launch_file_of(architecture_type.perform(context)))
     autoware_launch_package             = LaunchConfiguration("autoware_launch_package",                default=default_autoware_launch_package_of(architecture_type.perform(context)))
     consider_acceleration_by_road_slope = LaunchConfiguration("consider_acceleration_by_road_slope",    default=False)
+    consider_pose_by_road_slope         = LaunchConfiguration("consider_pose_by_road_slope",            default=True)
     global_frame_rate                   = LaunchConfiguration("global_frame_rate",                      default=30.0)
     global_real_time_factor             = LaunchConfiguration("global_real_time_factor",                default=1.0)
     global_timeout                      = LaunchConfiguration("global_timeout",                         default=180)
@@ -87,6 +88,7 @@ def launch_setup(context, *args, **kwargs):
     print(f"autoware_launch_file                := {autoware_launch_file.perform(context)}")
     print(f"autoware_launch_package             := {autoware_launch_package.perform(context)}")
     print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}")
+    print(f"consider_pose_by_road_slope         := {consider_pose_by_road_slope.perform(context)}")
     print(f"global_frame_rate                   := {global_frame_rate.perform(context)}")
     print(f"global_real_time_factor             := {global_real_time_factor.perform(context)}")
     print(f"global_timeout                      := {global_timeout.perform(context)}")
@@ -110,6 +112,7 @@ def make_parameters():
             {"autoware_launch_file": autoware_launch_file},
             {"autoware_launch_package": autoware_launch_package},
             {"consider_acceleration_by_road_slope": consider_acceleration_by_road_slope},
+            {"consider_pose_by_road_slope": consider_pose_by_road_slope},
             {"initialize_duration": initialize_duration},
             {"launch_autoware": launch_autoware},
             {"port": port},
@@ -141,6 +144,7 @@ def description():
         DeclareLaunchArgument("autoware_launch_file",                default_value=autoware_launch_file               ),
         DeclareLaunchArgument("autoware_launch_package",             default_value=autoware_launch_package            ),
         DeclareLaunchArgument("consider_acceleration_by_road_slope", default_value=consider_acceleration_by_road_slope),
+        DeclareLaunchArgument("consider_pose_by_road_slope",         default_value=consider_pose_by_road_slope        ),
         DeclareLaunchArgument("global_frame_rate",                   default_value=global_frame_rate                  ),
         DeclareLaunchArgument("global_real_time_factor",             default_value=global_real_time_factor            ),
         DeclareLaunchArgument("global_timeout",                      default_value=global_timeout                     ),
diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml
index 0c63ace8c06..1d11fdf1147 100644
--- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml
+++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml
@@ -85,7 +85,7 @@ OpenSCENARIO:
                                         freespace: true
                                         relativeDistanceType: euclidianDistance
                                         rule: equalTo
-                                        value: 6.2855887168861768 #6.28727
+                                        value: 6.239365667688271166468894080026
                                         Position: &POSITION_1
                                           LanePosition:
                                             roadId: ""
@@ -111,7 +111,7 @@ OpenSCENARIO:
                                         freespace: true
                                         relativeDistanceType: longitudinal
                                         rule: equalTo
-                                        value: 6.2583065215658280 #6.26
+                                        value: 6.211880466908041853457689285278
                                         Position: *POSITION_1
                                 - name: ""
                                   delay: 0
diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml
index 13bce0cd036..7dc89b05e47 100644
--- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml
+++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml
@@ -97,7 +97,7 @@ OpenSCENARIO:
                                         freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used.
                                         relativeDistanceType: euclidianDistance
                                         rule: equalTo
-                                        value: 5.2310695926720685
+                                        value: 5.230991883723311808296330127632
                                 - name: ""
                                   delay: 0
                                   conditionEdge: none
@@ -113,7 +113,7 @@ OpenSCENARIO:
                                         freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used.
                                         relativeDistanceType: longitudinal
                                         rule: equalTo
-                                        value: 5.2283065215597162
+                                        value: 5.228228771542489994317293167114
                                 - name: ""
                                   delay: 0
                                   conditionEdge: none
@@ -129,7 +129,7 @@ OpenSCENARIO:
                                         freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used.
                                         relativeDistanceType: lateral
                                         rule: equalTo
-                                        value: 0.1700000000055297
+                                        value: 0.169999999998253770172595977783
                                 - name: ""
                                   delay: 0
                                   conditionEdge: none