Feature/multi level lanelet support

[SzymonParapura]

Description

Abstract

This pull request introduces support for multi-level lanelets (e.g., bridges over roads) by improving lanelet matching and collision solving mechanisms. Previously, lanelet matching relied on 2D position checks, which could lead to incorrect detections and false-positive collisions when entities were on different altitudes.

Background

In multi-level environments – such as maps featuring bridges over roads or tunnels beneath roads – the previous lanelet matching and collision detection algorithms relied solely on 2D proximity checks. This approach considered only the X and Y coordinates of entities, ignoring their altitude (Z-axis).

As a result, entities located on different levels (e.g., a vehicle on a bridge and another vehicle on the road below) could be incorrectly matched to the same lanelet. This caused:

• False-positive detections: Entities were mistakenly identified as obstacles, even though they were on different levels.
• Unnecessary slowing or stopping: The Ego vehicle would slow down or stop due to these incorrect detections, reducing driving efficiency and simulation realism.

To resolve these issues, 3D lanelet matching and collision-solving mechanisms have been introduced.

Details

Enhancements to Lanelet Matching

3D Matching Support

• The toLaneletPose method has been updated to incorporate the altitude (Z-axis) of entities when performing lanelet matching. This helps ensure that entities are correctly matched to lanelets on the appropriate level.

New altitude_threshold Parameter

• Added a parameter altitude_threshold to define the maximum altitude difference allowed when matching an entity to a lanelet.

Improvements to Collision Solving

BehaviorTree Updates

• The BehaviorTree logic now considers the altitude of potential obstacles. This allows NPCs to ignore entities that are on a different vertical level. Link to introduced changes.

Ego Plane Determination

• A mechanism for determining the Ego plane was introduced. Entities that fall below this plane (e.g., vehicles under a bridge) are excluded from detection. Link to introduced changes.

Geometry Enhancements

• Introduced a Plane structure and methods to calculate offsets relative to this plane. Link to introduced changes.
• Updated quaternion operations for more efficient altitude calculations. Link to introduced changes #1 and Link to introduced changes #2

Fix traffic_source class

• The TrafficSource::makeRandomPose method does not account for the Z-axis when generating random positions. As a result, the generated position may be incorrect and an entity may not match to the lanelet. To fix this issue, the Z-coordinate is now assigned based on the altitude of the first nearby lanelet found. Link to introduced changes

Fix Unit tests

• Adjusted unit tests to reflect recent changes related to 3D matching and collision solving. Link to introduced changes #1 and Link to introduced changes #2 and Link to introduced changes #3

References

Internal link 1
Internal link 2

Destructive Changes

--

Known Limitations

--

[github-actions]

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

• Is this pull request written in a way that is easy to read from a third-party perspective?
• Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
• If this pull request contains a destructive change, does this pull request contain the migration guide?
• Labels of this pull request are valid?
• All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
• The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

[SzymonParapura]

There is no regresssion

[sonarqubecloud]

Quality Gate passed

Issues
5 New issues
0 Accepted issues

Measures
0 Security Hotspots
34.4% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud