Consider road slope in distance measurement and entity poses

[HansRobo]

Description

Abstract

• consider z-axis in euclidean distance evaluation
• fit pitch angle of entity poses to HDMap
• add consider_pose_by_road_slope flag to switch function
• update new scenarios and tests

Note: All the features can be toggled by consider_pose_by_road_slope.

Background

One of the characteristics of scenario_simualtor_v2 is that it is a simple and lightweight simulator.
Therefore, we have mainly dealt with simulations on a 2D plane without considering the Z direction or pitch/roll axes.

However, as the behavior on slopes is now being checked in autoware developing, simulation on a 2D plane alone is no longer able to cover the evaluation of Planning/Control, which is the main evaluation target of scenario_simualtor_v2.

So, we started to support simulation on 3D pose.

Details

Note: I would like to thank @dmoszynski for his great efforts for this pull-request.

Entity pose pitch-fitting to HDMap

As long as lane matching is successful, scenario_simulator_v2 tries to constrain the pose on the HDMap.
Specifically, the situation is as follows.

• spawning entity to position: OpenSCENARIO AddEntityAction
• teleporting entity to position: OpenSCENARIO TeleportAction
• setting goal: OpenSCENARIO AcquirePositionAction
• setting route
  • OpenSCENARIO FollowTrajectoryAction
  • OpenSCENARIO AssignRouteAction

The z fitting to HDMap was already performed in scenario_simulator_v2 until now.
But the entity pose fitting was not complete until now: pitch and roll angle were not fitted to HDMap.
In this pull-request, pitch-fitting is implemented.

With OpenSCENARIO WorldPosition / RelativeWorldPosition / RelativeObjectPosition, scenario_simulator_v2 will be performing both of z-fitting and pitch-fitting.
With OpenSCENARIO LanePosition, scenario_simulator_v2 will be performing only pitch fitting.
( Other position types are not supported in scenario_simulator_v2 now )

The pitch-fitting is also affected to bounding-box-based distance evaluation(freespace=true), because the orientation of entity bounding-box can be changed by pitch-fitting.

Entity that specified pitch=0 and consider_pose_by_road_slope=False

vokoscreenNG-2023-12-20_19-23-04.mp4

Entity that specified pitch=0 and consider_pose_by_road_slope=True

wolrd_fix.mp4

Distance evaluation

The euclidean distance evaluation in OpenSCENARIO DistanceCondition / RelativeDistanceCondition / ReachPositionCondition is performed in 2D plane previously.
In this pull-request, I implemented 3D euclidean dintance evaluation.

References

INTERNAL INVESTIGATION LINK

Regression Test: Pass

Destructive Changes

behavior changes

Some of OpenSCENARIO Actions, Conditions, traffic_simulator::API functions have destructive changes in terms of the behavior.
Please see detail section for a detailed explanation of the behavior changes.
All bahavior changes are caused by sloped or uneven roads on the map.
There should be almost no difference on a flat map, but there are some changes, so you may need to modify the scenario.
However, these changes correct what was wrong in the past, so it is recommended that you accept them.
If you really want to use the old behavior, please use the version before this pull-request or specify consider_pose_by_road_slope=False.

Please see sample scenario diffs in this pull-request for bahavior changes of conditions.
(for example: https://github.com/tier4/scenario_simulator_v2/pull/1209/files#diff-8ad8028efaba14a312f2bc2036165f514f69f3078760046016ce5826204993f2R88)

API schema changes

In this pull-request, some of API functions were added a boolean argument to toggle its behavior.
But the changes have backward-compatibility in terms of API interface, because all the additional argument have default value.

Known Limitations

No roll-fitting to HDMap

In this pull-request, orientation fitting is implemented for pitch angle only.
This is because it follows the implementation of simple_planning_simulator in autoware.universe.

No orientation-fitting to HDMap in OccupancyGridSensor

The GridMap output from scenario_simulator_v2 is always horizontal regardless of the road slope.
This is because current implementation of autoware.universe does not consider the roll and pitch axes.

[github-actions]

This PR edits vehicle model that is copied from simple_planning_simulator. Please consider making changes to the original code to avoid confusion or consult developers (@hakuturu583, @yamacir-kit and @HansRobo ).

=> (@HansRobo): With this change you don't have to worry about that. Because this changes only edit additional part of scenario_simulator_v2 in vehicle model files.

[HansRobo]

This is re-opened pull-request of #1103