ToRoadPosition implementation

[francocipollone]

https://github.com/ToyotaResearchInstitute/maliput_multilane/blob/1518673a0df56ee187497fcde079de9353ae3391/src/maliput_multilane/road_geometry.cc#L41-L56

This logic was changed in maliput_malidrive because of two possible scenarios.

• Let's say that we want to do a ToRoadPosition at the start of lane B. We have lane A and B connected by A-end and B-start.
  doing a ToRoadPosition at the very start of the B lane could lead to ambiguity when:

  • Distance for Lane B is zero and distance for Lane A is less than linear tolerance, let's say half of linear_tolerance
  • Both lateral displacement (R) (which is evaluated if the distance isn't enough, and they are both within lane bounds) is close to zero. However, because of numerical error, the R for Lane A is 1e-7 and the R for Lane B is 1e-6.
  • In this case, Lane A will be selected as the best candidate.

• The other scenario to have in mind is that the distance of all the lanes in a segment to a particular Inertial position will be the same. Given that the distance field from the RoadPositionResult means the distance from the requested inertial position to an inertial position in the segment volume, not the lane volume. Currently, it isn't an issue but when trying to make the logic more robust because of the first scenario, this case needs to be taken into account.

Related issues in maliput_malidrive backend:

• ToRoadPosition wrong behavior maliput_malidrive#173
• Fixes bug at ToRoadPosition query. maliput_malidrive#174
• CheckInvariants() outcome for some RoadGeometries maliput_malidrive#155 (comment)