Feature/ego slope

[hakuturu583]

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 and disabled defaultly to ensure backward-compatibility.

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.

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

Destructive Changes

There is no destructive changes because this function is disabled defaultly.

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.

[HansRobo]

I close this because I cannot assign @hakuturu583 to reviewer in this pull-request.
(he is marked the auther of this pull-request because he made this )