Skip to content

Lane pose calculation when spawning#

If you spawn entities by specifying in the lane coordinate system, lane pose calculation is skipped.
Internally, spawning entities by specifying in the lane coordinate system means specifying LaneletPose as the argument pose of the API::spawn function. In the OpenSCENARIO, it is to execute a TeleportAction by specifying the LanePosition. (e.g)

If you spawn entities by specifying in the map coordinate system, lane pose calculation is performed.

There are several internal implementations of pose calculation in the lane coordinate system implemented in the HDmapUtils class. The one used in this case is here.

In this function, the following procedure is used to calculate the pose in the lane coordinate system.

Search for matching lanes#

This procedure falls back to 2 if 1 fails. If 2 also fails, fallback is performed to 3. If 3 also fails, fallback is performed to 4.

  1. Obtain candidate lanes for matching considering the bounding box.
  2. Obtain lanes in the neighborhood of the Entity as matching candidates without considering the bounding box.
  3. Obtain matching candidates before and after the lane matched by considering the bounding box.
  4. Obtain matching candidates that exist in close proximity (0.1m) to the Entity.

If all cases from 1~4 fail, the pose in the lane coordinate system will be a calculation failure.

Calculate pose in lane coordinate system#

After narrowing down the candidate lanes by the above procedure, the pose of the lane coordinate system is calculated from the coordinates of the intersection of the horizontal bar extended from the Entity and the Catmull-Rom Spline curve constructed from the center line of the lane for each candidate lane.

Lane pose calculation

Vehicle / Ego Entity#

Let L_m be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be t_f and the tread of the rear wheels be t_r.

L_m = max(t_r, t_f) + 2.0

Pedestrian / MiscObject Entity#

Let L_m be the length of the horizontal bar used in the lane coordinate system calculation and width of the bounding box be L_w

L_m = L_w + 2.0