fix(simple_planning_simulator): fix ego sign pitch problem

[danielsanchezaran]

Description

There is a bug on the simple_planning simulator when enable_road_slope_simulation is true. Currently, the ego pitch angle is negative when the ego vehicle is on a downward slope and positive when on an upwards slope, which causes trouble when computing the acceleration caused by the slope (the sign is negative when it should be positive and vice versa), causing a wrong simulation output.

The original code:

As you can see on the code picture, if the vehicle is on an upwards slope, the pitch sign is negative but the resulting acceleration will be positive (-9.81 * sin(negative) = positive value) which makes no sense for an upwards slope.

Examples of wrong behavior:
The vehicle goes uphill, but its acceleration is high, it overshoots its goal

uphill_overshoot_goal.mp4

the vehicle wont move downhill

downslope_no_movement.mp4

This PR fixes the mentioned problem. Also, now the vehicle pitch simulation should be correct, even if enable_slope_simulation is disabled.

Tests performed

Tested with uphill and downhill lanelet maps:

Uphill:

cap-.2023-11-17-14-01-09.mp4

Downhill, the ego vehicle moves, no overshoot:

cap-.2023-11-17-13-57-38.mp4

Effects on system behavior

Fixes the acceleration by slope to have the correct sign. Now the simulated ego vehicle can start on a downward slope and it does not overshoot on uphill paths.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Comparison is base (a84c43b) 15.26% compared to head (d0373e1) 15.37%.
Report is 43 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5616      +/-   ##
==========================================
+ Coverage   15.26%   15.37%   +0.10%     
==========================================
  Files        1713     1713              
  Lines      118192   118381     +189     
  Branches    37797    37899     +102     
==========================================
+ Hits        18047    18206     +159     
- Misses      79595    79623      +28     
- Partials    20550    20552       +2     

Flag	Coverage Δ		*Carryforward flag
differential	57.51% <ø> (?)
total	15.26% <ø> (-0.01%)	⬇️	Carriedforward from 913a43b

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.