Find nearest waypoint for ResponsiveWait

[Yadunund]

Fix #97

[codecov]

Codecov Report

Merging #98 (5605a36) into main (dbfdc87) will increase coverage by 0.12%.
The diff coverage is n/a.

@@            Coverage Diff             @@
##             main      #98      +/-   ##
==========================================
+ Coverage   22.02%   22.14%   +0.12%     
==========================================
  Files         410      615     +205     
  Lines       32882    49353   +16471     
  Branches    16044    24093    +8049     
==========================================
+ Hits         7243    10931    +3688     
- Misses      17828    26652    +8824     
- Partials     7811    11770    +3959     

Flag	Coverage Δ
tests	22.14% <ø> (+0.12%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Impacted Files	Coverage Δ
...src/rmf_fleet_adapter/jobs/detail/impl_Rollout.hpp
...eet_adapter/rmf_rxcpp/include/rmf_rxcpp/RxJobs.hpp
...t_adapter_python/graph/PyOrientationConstraint.hpp
..._fleet_adapter/src/rmf_fleet_adapter/ParseArgs.cpp
...mf_fleet_adapter/test/phases/test_DispenseItem.cpp
...mf_rxcpp/include/rmf_rxcpp/detail/RxJobsDetail.hpp
.../rmf_fleet_adapter/src/mock_traffic_light/main.cpp
...apter/src/rmf_fleet_adapter/phases/RequestLift.cpp
.../rmf_rxcpp/RxCpp-4.1.0/Rx/v2/src/rxcpp/rx-util.hpp
...os2/rmf_traffic_ros2/examples/participant_node.cpp
... and 1015 more

[Yadunund]

@mxgrey test_rmf_rxcpp seems to be failing in the galactic CI. Is it okay to merge as the changes here are not related to rmf_rxcpp?

[mxgrey]

Looks like the test passed in the original PR, but I agree that the changes in this PR couldn't have impacted the failure. I guess it's just a flaky test 😬 . We can merge this and deal with the bad test in a future PR.