Parallel planning pipelines

[sjahr]

Description

This PR adds a first implementation of @mamoll proposal in #1200 to run multiple planning pipelines in parallel to improve the path quality by creating multiple solutions and choosing the most suitable one. I extended the MoveItCpp interface with an additional function that allows processing multiple planning requests in parallel in separate threads. Additionally, I've refactored the kinematics_metrics package in moveit_core to a more general package called metrics which contains several metric functions for robot trajectories. These implementations were mostly extracted out of the benchmarks package to become more visible and usable at runtime outside of the benchmarking context. I'll probably separate this change in a second PR once this is not WIP anymore.

Tutorial

→ https://moveit.picknik.ai/main/doc/how_to_guides/parallel_planning/parallel_planning_tutorial.html

Usage example:

config.yaml

one:
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: ompl
    planner_id: "RRTConnectkConfigDefault"
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0

two:
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: ompl
    planner_id: "PRMstarkConfigDefault"
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0

three:
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: pilz_industrial_motion_planner
    planner_id: "LIN"
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0

demo_node.cpp

    planning_component->setGoal(some_goal_state);
    planning_component->setStartStateToCurrentState();

    moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request;
    multi_pipeline_plan_request.load(node, {"one", "two", "three"});

    auto plan_solution = planning_components->plan(multi_pipeline_plan_request);

TODO

• Write unittests for robot_trajectory_metrics
• Add default stopping criteria --> Finish when all pipelines finished
• Make plan() thread safe

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

Base: 51.00% // Head: 50.97% // Decreases project coverage by -0.03% ⚠️

Coverage data is based on head (b18ec00) compared to base (afef073).
Patch has no changes to coverable lines.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1420      +/-   ##
==========================================
- Coverage   51.00%   50.97%   -0.02%     
==========================================
  Files         378      378              
  Lines       31649    31649              
==========================================
- Hits        16138    16131       -7     
- Misses      15511    15518       +7     

Impacted Files	Coverage Δ
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	78.79% <0.00%> (-1.13%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	45.29% <0.00%> (-0.43%)	⬇️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[mamoll]

some planners in OMPL (asymptotically optimal ones) can produce more than one solution before exiting. These could also be retrieved and added to planning_solutions in case the criteria the callback uses for selecting the best solution are different than the optimization objective used by planner.

[sjahr]

Do you know if multiple solutions are accessible through the interface that is provided by

PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters)

[mamoll]

I don't know. My comment isn't that important. The way to get to these solutions is first check that dynamic_cast<OMPLPlanningPipelinePtr>(planning_pipeline_ptr> != nullptr (or something like that). Next, you have to get the ModelBasedPlanningContextPtr. Next, you loop over each solution in getOMPLSimpleSetup()->getProblemDefinition()->getSolutions(). This can be done at some later point.

[tylerjw]

Here are some general comments on this. Do you have a minimal example of using this somewhere so it can be tested?

[AndyZe]

I think a good default planner setup would be:

Priority 1. Pilz
Priority 2. PRM-something
Priority 3. RRTConnect

Select Pilz by default, if it succeeded. Because it follows straight lines that are nice and predictable

RRTConnect is kind of a last-ditch method. Nobody wants that solution, unless the other planners fail

[henningkayser]

Sorry for letting this get stale for so long. I'm generally approving this implementation with some smaller requests.
Long-term, I would really like to iterate on this feature to make it more of a default. Some ideas:

• All plan requests (even single ones) should run in a thread with futures for synchronization
• Multi-plan-requests would simply run multiple plan requests and keep track of futures
• Things like terminate() shouldn't require another pipeline lookup. The pipeline or terminate function should be stored with the requested plan instance/thread.
• If possible, building of requests and parsing of responses should be in separate (free?) functions for improved readability

[henningkayser]

terminate() returns true, even if the planner is not running. https://github.com/ros-planning/moveit2/blob/main/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h#L128
We should check for false, though.

[sjahr]

Good point, is it enough to just check the activity of the planner instance itself or do we need to make the whole pipeline more interrupt-able, for example how long can planning_scene->isPathValid(...) take? The isActive() function checks on the pipeline level while terminate() only checks on the planner level & only aborts the planner

[AndyZe]

Here are some mostly-nitpicky comments, apologies for that. I'll give it a test now.

We've been using this in ROS1 and it's AWESOME!

[AndyZe]

I gave it a final test with the WIP tutorial. LGTM!

moveit/moveit2_tutorials#493