7.3.0

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

--

Related Issues