fix(simple_planning_simulator): fix ego sign pitch problem (autowaref…

…oundation#5616) (#1029) * fix ego sign pitch problem * change variable name for clarity * update documentation to clarify that driving against the lane is not supported --------- Signed-off-by: Daniel Sanchez <[email protected]> Co-authored-by: danielsanchezaran <[email protected]>
tier4 · Nov 21, 2023 · f30f4c8 · f30f4c8
1 parent a1af5f4
commit f30f4c8
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md
@@ -95,6 +95,8 @@ Ego vehicle pitch angle is calculated in the following manner.
 
 ![pitch calculation](./media/pitch-calculation.drawio.svg)
 
+NOTE: driving against the line direction (as depicted in image's bottom row) is not supported and only shown for illustration purposes.
+
 ## Error detection and handling
 
 The only validation on inputs being done is testing for a valid vehicle model type.

diff --git a/...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -322,8 +322,9 @@ void SimplePlanningSimulator::on_timer()
 
   // calculate longitudinal acceleration by slope
   constexpr double gravity_acceleration = -9.81;
-  const double ego_pitch_angle = enable_road_slope_simulation_ ? calculate_ego_pitch() : 0.0;
-  const double acc_by_slope = gravity_acceleration * std::sin(ego_pitch_angle);
+  const double ego_pitch_angle = calculate_ego_pitch();
+  const double slope_angle = enable_road_slope_simulation_ ? -ego_pitch_angle : 0.0;
+  const double acc_by_slope = gravity_acceleration * std::sin(slope_angle);
 
   // update vehicle dynamics
   {