Fix longitudinal dintance calculation with lane-change in HdMapUtils::getLongitudinalDistance

[HansRobo]

Description

Abstract

Fix longitudinal dintance calculation with lane-change in HdMapUtils::getLongitudinalDistance

Details

In previous implementation of HdMapUtils::getLongitudinalDistance, the function performed wrong longitudinal distance when the route has a lane change between first and second lanelet.

The calculation method is as described in the documentation, but the implementation contained a miss, so I will fix it in this pull-request.

scenario test behavior

I added the bugged case to ByEntityCondition.EntityCondition.DistanceCondition.Shortest.yaml
Before this pull_request, the relative longitudinal distance between Npc1 and Npc4 is calculated to about 113, as executed here.
But after this pull-request, it is calculated to about 54, as defined in the test scenario.

References

Regression Test: OK

Destructive Changes

None

Known Limitations

None

[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.

[hakuturu583]

@HansRobo Thank you!
Code is looks good to me.
Could you write test scenarios written in C++ and add it to the test in github actions?

[HansRobo]

OK, I'll add C++ unit test
⇒ done