Hybrid Planning

[tylerjw]

Description

Here is a PR for the hybrid planner. I plan on doing the work to clean this up so it can be merged.

Here is the original hybrid planner PR: #488

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

Merging #763 (6769fe8) into main (48625f6) will increase coverage by 0.69%.
The diff coverage is n/a.

❗ Current head 6769fe8 differs from pull request most recent head 24f8abd. Consider uploading reports for the commit 24f8abd to get more accurate results

@@            Coverage Diff             @@
##             main     #763      +/-   ##
==========================================
+ Coverage   54.87%   55.56%   +0.69%     
==========================================
  Files         196      198       +2     
  Lines       21358    21420      +62     
==========================================
+ Hits        11719    11900     +181     
+ Misses       9639     9520     -119     

Impacted Files	Coverage Δ
...sion_distance_field/collision_env_distance_field.h	4.55% <0.00%> (-4.54%)	⬇️
...rajectory_processing/src/ruckig_traj_smoothing.cpp	80.62% <0.00%> (-4.08%)	⬇️
...on_distance_field/collision_distance_field_types.h	59.41% <0.00%> (-1.37%)	⬇️
moveit_ros/moveit_servo/src/servo_calcs.cpp	68.86% <0.00%> (-0.51%)	⬇️
...mpl_interface/src/model_based_planning_context.cpp	50.00% <0.00%> (-0.44%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	46.34% <0.00%> (-0.40%)	⬇️
moveit_ros/moveit_servo/src/collision_check.cpp	85.25% <0.00%> (-0.23%)	⬇️
.../collision_detection_fcl/src/collision_env_fcl.cpp	89.68% <0.00%> (-0.19%)	⬇️
...cessing/src/time_optimal_trajectory_generation.cpp	76.72% <0.00%> (-0.15%)	⬇️
moveit_ros/planning/rdf_loader/src/rdf_loader.cpp	23.47% <0.00%> (ø)
... and 22 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 48625f6...24f8abd. Read the comment docs.

[tylerjw]

I am really confused as to how this would affect the servo tests in any way. None of the servo PRs seem to be failing but this one is.

[JensVanhooydonck]

I am having the following issues with this branch:

• Multiple consecutive requests are not possible. In single_plan_execution.cpp the call_once of the local planner ensures that the local planner is only executed once. However, this is not reset when the local planner was successful. Replacing the std::once_flag with a Boolean and resetting after stopping the local planner seems to solve this. However I'm not sure this is thread safe.
• When the global planner does not find a solution (Not possible or collision after smoothing) this crashes the Hybrid Planner Component container. In other words: if the initial path is not possible, it will not try again, or return that this path is not possible, but the hybrid container will stop.
• It is not possible to pass a ompl_planning.yaml file to change the ompl planning parameters for the global planning.
• The following parameters in the MotionPlanRequest are ignored:
  • max_velocity_scaling_factor
  • max_acceleration_scaling_factor
  • max_cartesian_speed
  • workspace_parameters.*
  • start_state.*

[AndyZe]

I find this branch has broken the demo. One of the reasons, at least, is that the trajectory controller does not come up.