Fix findWayPointIndicesForDurationAfterStart method when out of bounds

[XingxinHE]

Description

The fix is to address the bug raised in issue #3612 which causes UB. The change has been

• clang-format/tidy with docker image moveit/moveit2:rolling-ci
• run unit test with moveit_core/robot_trajectory/test/test_robot_trajectory.cpp

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

Misc.

Actually, the wording "Create tests, which fail without this PR" is not strictly correct. Without the fix, the unit tests will "pass", but it passes with undefined behavior. Meaning, all the EXPECT_xx after the call findWayPointIndicesForDurationAfterStart will be omitted and the google test doesn't actually check it. I once plan to use the EXPECT_EXIT but UB doesn't always result in a SIGSEGV.

In my perspective, I prefer the .at() over [index]. See the guideline. But I guess using .at( might cause a little bit performance degrade in real time control of robot? If we use the [index] approach, then the unit test should run with AddressSanitizer or something. Since I am not quite familiar with the CI/CD of moveit2, therefore I defer whether to keep current implementation or change to .at() to @nbbrooks .

[XingxinHE]

The change in line 539 is not enough to handle the case that waypoints is empty. i.e. index an empty deque with [0]

[XingxinHE]

Hey @nbbrooks , this pr is ready for review.

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.
✅ Project coverage is 46.27%. Comparing base (760277f) to head (8313b62).

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3626      +/-   ##
==========================================
+ Coverage   46.19%   46.27%   +0.09%     
==========================================
  Files         720      720              
  Lines       59181    59208      +27     
  Branches     7594     7596       +2     
==========================================
+ Hits        27333    27394      +61     
+ Misses      31681    31647      -34     
  Partials      167      167              

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[nbbrooks]

This is looking good considering the unclear intended behavior from the original function description and the current [old] error handling patterns in the codebase. As I see it, the expected output with edge cases should be:

// If waypoints are empty: before == 0, after == 0, blend == 0 (invalid - empty trajectory)
// If target duration < 0: before == 0, after == 0, blend == 0 (invalid - before trajectory has started)
// If target duration > total duration of waypoints, before == size() - 1, after == size() - 1, blend == 1 (after trajectory has completed)
// If 1 waypoint and target duration == total duration of waypoints, before == 0, after == 0, blend == 1 (at single point trajectory completion)
// Else before == last waypoint index with total duration < target duration, after == before + 1, blend == fraction of target duration between total duration @ before and total duration @ after
//   Therefore, if 2 waypoints and target duration == total duration of waypoints, before == 0, after == 1, blend == 1 (at trajectory completion)

Since you are being so thorough, here are the last changes I suggest:

1. Change access to use at() as you noted. I would not expect this to be in a real-time loop or have significant impact.
2. Consider updating your test names to be more descriptive of the corner case they are checking (my suggestions are RobotTrajectoryFindWayPointIndicesBetweenWaypoints, RobotTrajectoryFindWayPointIndicesAtLastOfManyWaypoints, RobotTrajectoryFindWayPointIndicesAfterLastWaypoint, and RobotTrajectoryFindWayPointIndicesEmptyWaypoints)
3. Add two more tests to check the remaining cases: RobotTrajectoryFindWayPointIndicesBeforeFirstWaypoint and RobotTrajectoryFindWayPointIndicesAtLastOfSingleWaypoint or something to that effect.

[XingxinHE]

Nice! Then I change the unit test to use EXPECT_NO_THROW and future mistakes will be caught. I've updated the tests and docs as well.