Add OSM destination check

[simsurace]

It is possible to plan a route to a destination dest for which dest[3] is not between zero and road_length(dest[1], dest[2], model), which is not caught by the subsequent shortest path algorithm. When such a route is executed, the agent will snap to a position that is not on the graph after completing the route.

With this PR, we check that the destination is valid and if it is not, we exit the plan_route! function early and return false.

Discovered in #1023.

[simsurace]

If this PR is welcome, I will add some unit tests.

[Tortar]

thanks! This is surely useful to avoid the inconsistencies you described in #1023, I approve but I will wait till the unit tests being added before merging

[codecov-commenter]

Codecov Report

All modified and coverable lines are covered by tests :white_check_mark:

Project coverage is 86.50%. Comparing base (8b5b456) to head (4b61243). Report is 105 commits behind head on main.

:exclamation: Current head 4b61243 differs from pull request most recent head 169f1e0. Consider uploading reports for the commit 169f1e0 to get more accurate results

Additional details and impacted files

```diff @@ Coverage Diff @@ ## main #1024 +/- ## =========================================== + Coverage 70.12% 86.50% +16.38% =========================================== Files 42 39 -3 Lines 2718 2550 -168 =========================================== + Hits 1906 2206 +300 + Misses 812 344 -468 ```

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

[Tortar]

The tests are very good, thank you!