fix ego sign pitch problem

Signed-off-by: Daniel Sanchez <[email protected]>
autowarefoundation · Nov 17, 2023 · f1a3b8f · f1a3b8f
1 parent a84c43b
commit f1a3b8f
Showing 1 changed file with 3 additions and 2 deletions.
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
@@ -327,8 +327,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_pitch_angle = enable_road_slope_simulation_ ? -ego_pitch_angle : 0.0;
+  const double acc_by_slope = gravity_acceleration * std::sin(slope_pitch_angle);
 
   // update vehicle dynamics
   {