Exit loop when position can't change.

[uavster]

Description

Fixes #2066. Infinite looping happens at Trajectory::integrateForward when three things concur:

• The velocity at a trajectory point is zero
• Any maximum acceleration component is zero
• The corresponding component in the tangent at the point of the trajectory is not zero.

The changes in this PR are:

• Break the loop when velocity and acceleration are zero
• Unit test that times out without the fix and passes with it
• Unit test that ensures the loop is not broken if the tangent is zero even if the maximum acceleration is

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

[codecov]

Codecov Report

Patch coverage: 100.00% and project coverage change: +0.02% 🎉

Comparison is base (a2f0183) 50.71% compared to head (46b7944) 50.73%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2307      +/-   ##
==========================================
+ Coverage   50.71%   50.73%   +0.02%     
==========================================
  Files         386      386              
  Lines       31914    31922       +8     
==========================================
+ Hits        16182    16192      +10     
+ Misses      15732    15730       -2     

Files Changed	Coverage Δ
...cessing/src/time_optimal_trajectory_generation.cpp	86.37% <100.00%> (+0.02%)	⬆️

... and 1 file with indirect coverage changes

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

[sea-bass]

Also, what happens in the case where time_step_ is zero? Should that be handled here as well?

Though I guess that will necessarily make path_vel zero so it's probably okay.

[uavster]

Ah right! My original fix does not catch that because time_step==0 makes velocity, but not acceleration. I've added an extra case at the constructor to check for that.

[sea-bass]

gah. The clang-tidy checks are failing because somehow MoveIt doesn't use the Google style of constant casing. Sorry :/

https://github.com/ros-planning/moveit2/actions/runs/5893944176/job/15986542597?pr=2307#step:12:9198

[uavster]

gah. The clang-tidy checks are failing because somehow MoveIt doesn't use the Google style of constant casing. Sorry :/

https://github.com/ros-planning/moveit2/actions/runs/5893944176/job/15986542597?pr=2307#step:12:9198

Oh, hahah. Linter-settled discussion.

[sea-bass]

Oh, hahah. Linter-settled discussion.

It's specifically that in the bodies of functions, where things aren't constants, you have to use snake_case, whereas camelCase is okay for e.g. constexprs in anonymous namespaces.

So if you just use names like max_velocity instead of kMaxVelocity in the tests, should be good to go

[uavster]

Oh, hahah. Linter-settled discussion.

It's specifically that in the bodies of functions, where things aren't constants, you have to use snake_case, whereas camelCase is okay for e.g. constexprs in anonymous namespaces.

So if you just use names like max_velocity instead of kMaxVelocity in the tests, should be good to go

done

[uavster]

Oh, hahah. Linter-settled discussion.

It's specifically that in the bodies of functions, where things aren't constants, you have to use snake_case, whereas camelCase is okay for e.g. constexprs in anonymous namespaces.
So if you just use names like max_velocity instead of kMaxVelocity in the tests, should be good to go

done

BTW, are you ok with inlining the instantiations of Path and Trajectory?

[sea-bass]

BTW, are you ok with inlining the instantiations of Path and Trajectory?

That's fine!