Make planning pipeline respect the allowed_planning_time

[TomCC7]

Description

@sjahr WIP PR to solve #2581. Several notes:

1. Modified allowed_planning_time directly to change the timeout of each planner
2. Currently the timeout for each planner is naively evenly distributed as discussed in the issue.
3. Haven't thoroughly tested the modification. I wonder if is there an easy way to do this. I saw a tutorial here but it seems outdated.

Checklist

• [x] Required by CI: Code is auto formatted using clang-format
• [x] Create tests, which fail without this PR reference
• [ ] While waiting for someone to review your request, please help review another open pull request to support the maintainers

[codecov[bot]]

Codecov Report

Attention: Patch coverage is 96.55172% with 1 lines in your changes are missing coverage. Please review.

Project coverage is 43.03%. Comparing base (d962501) to head (167fb5e). Report is 34 commits behind head on main.

:exclamation: Current head 167fb5e differs from pull request most recent head af3e30e. Consider uploading reports for the commit af3e30e to get more accurate results

Files	Patch %	Lines
...planning_pipeline/test/planning_pipeline_tests.cpp	90.00%	1 Missing :warning:

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2692      +/-   ##
==========================================
- Coverage   50.74%   43.03%   -7.71%     
==========================================
  Files         392      692     +300     
  Lines       32553    56351   +23798     
  Branches        0     7277    +7277     
==========================================
+ Hits        16517    24247    +7730     
- Misses      16036    31941   +15905     
- Partials        0      163     +163     

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

[TomCC7]

Thanks for the review! Based on your feedback I would propose that we don't enforce and check for timeout after each step but only modify the corresponding remaining time to guide the planners and adapters to follow the time limit.

We can do a single check after response adapters finished and modify the error code accordingly. In this way even if the timeout is reached the function will also finish so that users can still get the planning result. Does that sound good?

[sjahr]

Thanks for the review! Based on your feedback I would propose that we don't enforce and check for timeout after each step but only modify the corresponding remaining time to guide the planners and adapters to follow the time limit.

We can do a single check after response adapters finished and modify the error code accordingly. In this way even if the timeout is reached the function will also finish so that users can still get the planning result. Does that sound good?

The check in every iteration makes sense to me, so I wouldn't change that. If the first out of ten pipeline elements uses the full time budget, there is no point in running the other elements before reporting the error. I think we should always check if the full time budget is exhausted but we don't care so much if a planner uses the allocated fraction of the budget or a bit more as long as the overall planning time remains within the budget

[TomCC7]

@sjahr sorry for the late update. I revised my implementation based on your feedback. Here's the update:

1. after checking the timeout a moveit::core::MoveItErrorCode::TIMED_OUT error code is sent, which will be catched by error code checks later
2. now the unused time by previous planner will be automatically released for later planners to use
3. added timeout check in the unit test files and verified that it's working

I think if you are good with the current time distribution strategy this pull request is ready for review and merge!

[TomCC7]

hi @sjahr, just checking in, is there anything else I need to do on this PR?

[EzraBrooks]

@TomCC7, I apologize for the delay, we've been pretty absorbed with bugfixes for multi-arm and mobile manipulator configurations lately.

@sjahr @henningkayser is there anything else we need changed on this PR before we give it a last round of testing and merge it?