[Planning Pipeline Refactoring] #1 Simplify Adapter - Planner chain

[sjahr]

Description

Addresses #2408
Requires moveit/moveit_msgs#170

Related PRs:

• Update planning pipeline configs moveit_resources#189
• Update tutorials for planning pipeline refactoring moveit2_tutorials#793

How to test

Use moveit/moveit2_tutorials#793 and moveit/moveit_resources#189 and run

ros2 launch moveit2_tutorials move_group.launch.py

and

ros2 launch moveit2_tutorials motion_planning_pipeline_tutorial.launch.py

Expected output

[motion_planning_pipeline_tutorial-1] [INFO] [1699454408.777050834] [motion_planning_pipeline_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to continue the demo
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.087785146] [motion_planning_pipeline_tutorial.remote_control]: ... continuing
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.087971200] [motion_planning_pipeline_tutorial]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.088272213] [motion_planning_pipeline_tutorial]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[motion_planning_pipeline_tutorial-1] [WARN] [1699454410.088287442] [motion_planning_pipeline_tutorial.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.088442316] [motion_planning_pipeline_tutorial]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.088596429] [motion_planning_pipeline_tutorial]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.089114285] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.124115820] [motion_planning_pipeline_tutorial]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[motion_planning_pipeline_tutorial-1] [WARN] [1699454410.124140126] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Invalid max_velocity_scaling_factor 0.000000 specified, defaulting to 1.000000 instead.
[motion_planning_pipeline_tutorial-1] [WARN] [1699454410.124149954] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Invalid max_acceleration_scaling_factor 0.000000 specified, defaulting to 1.000000 instead.
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.125490595] [motion_planning_pipeline_tutorial]: Calling PlanningResponseAdapter 'ValidateSolution'
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.125781979] [motion_planning_pipeline_tutorial]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[motion_planning_pipeline_tutorial-1] [INFO] [1699454410.125976980] [motion_planning_pipeline]: Visualizing the trajectory

Changes

• Simplify pipeline component call chain to a sequence of two vectors + a planner
• Disallow adapters to call the planner
• Remove FixStartStatePathConstraints ➡️ The problem of connecting a start state to the start of a constrained motion should be solved with MTC today
• Add PipelineState publisher which publishes the intermediate state between the pipeline stages.
  • To test it, set the progress_topic some topic name you'd like to see the intermediate the intermediate results published to before the planning pipeline instances are constructed. The parameter is evaluated during the construction of a planning pipeline instance
• Make the moveit error code the sole source to report success or failure
  • moveit::core::MoveItStatus PlanningRequestAdapter::adapt(.. Plan request adapters return a status containing error information
  • PlannerContext virtual void solve(MotionPlanResponse& res) = 0; uses the response to communicate success or failure
  • PlanRequestAdapter virtual void adapt(..., planning_interface::MotionPlanResponse& res) also uses the response for communication

TODOs

• Write Unit tests
• Create humble release + branch for moveit_resources
• Update setup assistant
• Add instructions for migration guide

[codecov]

Codecov Report

Attention: 45 lines in your changes are missing coverage. Please review.

Comparison is base (f79a070) 50.83% compared to head (a4ae7d7) 50.93%.
Report is 1 commits behind head on main.

Files	Patch %	Lines
...anning/planning_pipeline/src/planning_pipeline.cpp	70.94%	25 Missing ⚠️
...mpl_interface/src/model_based_planning_context.cpp	57.50%	17 Missing ⚠️
...clude/moveit/planning_pipeline/planning_pipeline.h	84.22%	3 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2429      +/-   ##
==========================================
+ Coverage   50.83%   50.93%   +0.10%     
==========================================
  Files         391      388       -3     
  Lines       32198    32237      +39     
==========================================
+ Hits        16366    16416      +50     
+ Misses      15832    15821      -11     

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

[Abishalini]

I briefly read through this PR and had some cursory suggestions. I will give this another read at the end of the day for a more thorough review.

[MarqRazz]

I was able to build and run the examples and it works as expected. Great work @sjahr!

[mergify]

This pull request is in conflict. Could you fix it @sjahr?

[MarqRazz]

I assume you are still updating the dependencies because right now I get this error...

[motion_planning_pipeline_tutorial-1] terminate called after throwing an instance of 'rclcpp::ParameterTypeException'
[motion_planning_pipeline_tutorial-1]   what():  expected [string_array] got [string]

[sjahr]

ros2 launch moveit2_tutorials motion_planning_pipeline_tutorial.launch.py

@MarqRazz I cannot reproduce that issue. Are you using moveit/moveit_resources#189 and moveit/moveit2_tutorials#793? Maybe you need to wipe your workspace and rebuild it 🤔

[MarqRazz]

ros2 launch moveit2_tutorials motion_planning_pipeline_tutorial.launch.py

@MarqRazz I cannot reproduce that issue. Are you using ros-planning/moveit_resources#189 and ros-planning/moveit2_tutorials#793? Maybe you need to wipe your workspace and rebuild it 🤔

So I just did a clean build and it is still failing. Below is what my workspace looks like. I'm testing this in Humble so maybe that is causing issues???

=== ./moveit2 (git) ===
On branch pr-refactor_pipeline_chain
Your branch is up to date with 'sebj/pr-refactor_pipeline_chain'.
nothing to commit, working tree clean
=== ./moveit2_tutorials (git) ===
On branch pr-update_for_refactroing_planning_pipeline
Your branch is up to date with 'origin/pr-update_for_refactroing_planning_pipeline'.
nothing to commit, working tree clean
=== ./moveit_msgs (git) ===
On branch ros2
Your branch is up to date with 'origin/ros2'.
nothing to commit, working tree clean
=== ./moveit_resources (git) ===
On branch pr-update_pipeline_planner_configs
Your branch is up to date with 'origin/pr-update_pipeline_planner_configs'.
nothing to commit, working tree clean
=== ./moveit_task_constructor (git) ===
On branch ros2
Your branch is up to date with 'origin/ros2'.
nothing to commit, working tree clean
=== ./moveit_visual_tools (git) ===
On branch ros2
Your branch is up to date with 'origin/ros2'.
nothing to commit, working tree clean
=== ./pick_ik (git) ===
On branch main
Your branch is up to date with 'origin/main'.
nothing to commit, working tree clean
=== ./ros2_kortex (git) ===
On branch main
Your branch is up to date with 'origin/main'.
nothing to commit, working tree clean
=== ./rviz_visual_tools (git) ===
On branch ros2
Your branch is up to date with 'origin/ros2'.
nothing to commit, working tree clean

[MarqRazz]

I'm not sure what I changed other than restarting my computer and the crash is now gone. I tested with Cyclone and FastDDS, I thought the RMW might be causing issues, and everything worked as expected.

[sjahr]

I'm not sure what I changed other than restarting my computer and the crash is now gone. I tested with Cyclone and FastDDS, I thought the RMW might be causing issues, and everything worked as expected.

Glad it worked! I have not tested it on humble but on rolling, so good to know it works on humble too. Thanks!

[sea-bass]

I have 2 high level comments from reading this.

1: Removing the planner function from adaptAndPlan()?

At a glance, it seems okay to remove this and simply have the adapters mutate the original request/response... but it does seem weird to me that replacing a call to the planner's solve with simply true will keep exactly the same functionality.

... but maybe it does work out? It seems like the previous "adapter chain" implementation would basically call the adapters recursively so it likely has the same effect as what you changed it to, but way more confusing than your cleanup!

2. Discussions on PlanRequestAdapter vs. PlanResponseAdapter

We spoke offline about the existence of the separate classes, and how you've had to create a templated function to run adapt() on both types, plus a lot of copypasta. I do like that one implementation holds the request const and the other holds the response const, though!

Another alternative would be to have a single PlanAdapter base class with empty implementations of, e.g., adaptRequest() and adaptResponse() methods that can be separately overridden. This way, you can have a single adapter that works as both request/response adapter, and also gets rid of code duplication and the need for templates.

Don't have a strong opinion here, but as we discussed I wanted to float the option?

[sea-bass]

Testing with the Kinova config from the tutorials, I just found a bug with the default Pilz config that gets loaded by MoveItConfigsBuilder. If you remove the following line, it does what we want:

https://github.com/ros-planning/moveit2/blob/main/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml#L2

[sea-bass]

I can only see small things with comments at this point. Once your small PR to moveit_msgs is merged and you make that change here, LGTM

[sea-bass]

Cool, I approved that PR so feel free to make the change here afterwards

[Abishalini]

Should we consider merging moveit/moveit_msgs#171 and remove the MoveItStatus struct?

[sea-bass]

Let's do this! 🚢