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

sjahr · 2023-10-11T12:53:02Z

Description

Addresses #2408
Requires moveit/moveit_msgs#170

Related PRs:

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 · 2023-10-11T13:58:57Z

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.

moveit_core/CMakeLists.txt

moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml

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.

moveit_planners/stomp/stomp_moveit_plugin_description.xml

moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h

moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h

moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp

moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h

moveit_ros/planning/default_request_adapters_plugin_description.xml

MarqRazz

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

...ros/planning/planning_response_adapter_plugins/res/default_plan_response_adapter_params.yaml

moveit_ros/planning/default_response_adapters_plugin_description.xml

moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h

moveit_configs_utils/default_configs/chomp_planning.yaml

moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h

moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml

mergify · 2023-10-18T15:05:08Z

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]

moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h

moveit_ros/planning/default_response_adapters_plugin_description.xml

sjahr · 2023-10-23T16:42:58Z

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 🤔

moveit_ros/planning/default_response_adapters_plugin_description.xml

MarqRazz · 2023-10-23T17:40:04Z

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 · 2023-10-24T06:50:07Z

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?

moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h

moveit_ros/planning/default_request_adapters_plugin_description.xml

moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h

moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml

moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp

moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp

sea-bass · 2023-10-24T15:45:29Z

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

Move plan_request_adapters into planning interface package Add callRequestAdapterChain function Differentiate between request and response adapters Restore planning_pipeline.cpp Add response_adapters parameter and cleanup adapter chain calls Add response adapter loading chain Address compiler errors Fix parameter type Fix compilation errors Rename planning_request_adapters to *_adapter Cleanup + fix errors Fix plugins and print description Remove unused adapters Planners plan, Adapters adapt ... Format + fix compilation error Simplify by removing callRequest/ResponseAdapterChain Remove unneeded PlannerFn Make clang tidy happy Add loadPluginVector template and make callAdapterChain private Add documentation Remove TODOs Cleanups and move templates into annonymous namespace Add more debug information and fix bugs in adapters Fix bugs Remove commented out CMake lines Update parameter description Apply suggestions from code review Co-authored-by: Abishalini Sivaraman <abi.gpuram@gmail.com> Address review Remove instrospection dir Make parameters read only Make sure that pipeline does not abort if no request adapter is configured Remove anonymous namespace in header Move adapter params into separate namespace Delete outdated unittest Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> ruckig -> Ruckig! Address comments part 2 Remove wrong request_adapters initilization from PILZ config Update migration guide Update logging Remove response argument from PlanningRequestAdapter::adapt Make adapter naming consistent Add pipeline state publisher Pull stacked constraints check out of planning pipeline Update planning request adapters Format! Remove unused params and fix clang tidy Fix error Move validity check and path publisher in separate adapters Fix DISPLAY_PATH_TOPIC Update config yamls Add MoveItStatus and return with request adapter Revert outdated change Request adapters adapt returns void now Planner solve function returns void now Update license in header Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Update migration guide Address review Don't make initialize purely virtual & address review

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

moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h

moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h

sea-bass · 2023-11-13T19:48:09Z

moveit_core/utils/include/moveit/utils/moveit_status.h


-  void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override
+  MoveItStatus(const int code = 0 /*UNDEFINED*/, const std::string& message = std::string(),


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

moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp

moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp

Abishalini · 2023-11-13T21:20:22Z

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

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

sea-bass

Let's do this! 🚢

sjahr mentioned this pull request Oct 11, 2023

Extend Planning Pipeline to allow calling multiple planners in sequence #2408

Closed

9 tasks

sjahr mentioned this pull request Oct 12, 2023

Update planning pipeline configs moveit/moveit_resources#189

Merged

sjahr marked this pull request as ready for review October 12, 2023 15:36

sjahr requested review from tylerjw, henningkayser and sea-bass October 12, 2023 15:36

sjahr commented Oct 12, 2023

View reviewed changes

moveit_core/CMakeLists.txt Outdated Show resolved Hide resolved

sjahr commented Oct 12, 2023

View reviewed changes

moveit_core/CMakeLists.txt Outdated Show resolved Hide resolved

sjahr changed the title ~~WIP: Refactor pipeline chain~~ Refactor pipeline chain Oct 12, 2023

sjahr commented Oct 13, 2023

View reviewed changes

moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml Outdated Show resolved Hide resolved

sjahr commented Oct 13, 2023

View reviewed changes

moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml Outdated Show resolved Hide resolved

Abishalini reviewed Oct 13, 2023

View reviewed changes

MarqRazz reviewed Oct 13, 2023

View reviewed changes

sjahr requested a review from Abishalini October 17, 2023 15:52

sjahr changed the title ~~Refactor pipeline chain~~ [Planning Pipeline Refactoring] #1 Simplify Adapter - Planner chain Oct 17, 2023

sjahr mentioned this pull request Oct 17, 2023

[Planning Pipeline Refactoring] #2 Enable chaining planners #2457

Merged

11 tasks

sjahr requested a review from MarqRazz October 18, 2023 14:57

sjahr force-pushed the pr-refactor_pipeline_chain branch 2 times, most recently from a4d32a5 to 1992fe5 Compare October 23, 2023 11:42

MarqRazz reviewed Oct 23, 2023

View reviewed changes

moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h Show resolved Hide resolved

moveit_ros/planning/default_response_adapters_plugin_description.xml Outdated Show resolved Hide resolved

sjahr linked an issue Oct 23, 2023 that may be closed by this pull request

Throw error if a time parameterization planning request adapter is not loaded #2145

Closed

sjahr commented Oct 23, 2023

View reviewed changes

moveit_ros/planning/default_response_adapters_plugin_description.xml Outdated Show resolved Hide resolved

MarqRazz approved these changes Oct 23, 2023

View reviewed changes

sea-bass reviewed Oct 24, 2023

View reviewed changes

sjahr force-pushed the pr-refactor_pipeline_chain branch from de17a4e to 602e859 Compare November 13, 2023 17:49

Clang-tidy

315e29c

sjahr requested a review from sea-bass November 13, 2023 18:41

sea-bass reviewed Nov 13, 2023

View reviewed changes

Abishalini reviewed Nov 13, 2023

View reviewed changes

moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp Outdated Show resolved Hide resolved

sjahr and others added 5 commits November 14, 2023 17:06

Fix wrong if statement

3332826

Remove moveit status in favor of the error code

f03c6ac

Apply suggestions from code review

e191169

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

Update documentation

0d6c008

Merge branch 'main' into pr-refactor_pipeline_chain

0a0ff26

sjahr requested review from Abishalini, sea-bass and MarqRazz November 14, 2023 16:33

sea-bass approved these changes Nov 14, 2023

View reviewed changes

sjahr added 6 commits November 15, 2023 10:48

Format

15fbf04

Add unittest

910eecc

Fix typo

e8c2d54

Finish unittests

232b479

Merge branch 'main' into pr-refactor_pipeline_chain

9ede94f

Format ..

a4ae7d7

sjahr self-assigned this Nov 15, 2023

tylerjw merged commit beec33d into moveit:main Nov 21, 2023
12 checks passed

tylerjw deleted the pr-refactor_pipeline_chain branch November 21, 2023 15:09

This was referenced Nov 28, 2023

WIP: Add planner chain tutorial moveit/moveit2_tutorials#820

Draft

Update tutorials for planning pipeline refactoring moveit/moveit2_tutorials#793

Closed

sjahr mentioned this pull request Dec 6, 2023

Update tutorials to pipeline refactoring changes moveit/moveit2_tutorials#834

Merged

sjahr mentioned this pull request May 24, 2024

regression in FixStartStateCollision adapter #2849

Closed

chama1176 mentioned this pull request Jul 22, 2024

CheckStartStateBounds does not support REVOLUTE and NOT Continuous type joints #2924

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

sjahr commented Oct 11, 2023 •

edited

Loading

codecov bot commented Oct 11, 2023 •

edited

Loading

Abishalini left a comment

MarqRazz left a comment

mergify bot commented Oct 18, 2023

MarqRazz left a comment

sjahr commented Oct 23, 2023

MarqRazz commented Oct 23, 2023

MarqRazz left a comment

sjahr commented Oct 24, 2023

sea-bass left a comment

sea-bass commented Oct 24, 2023

sea-bass left a comment

sea-bass Nov 13, 2023

Abishalini commented Nov 13, 2023

sea-bass left a comment


		void initialize(const rclcpp::Node::SharedPtr& /* node /, const std::string& / parameter_namespace */) override
		MoveItStatus(const int code = 0 /UNDEFINED/, const std::string& message = std::string(),

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

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

Conversation

sjahr commented Oct 11, 2023 • edited Loading

Description

How to test

Changes

TODOs

codecov bot commented Oct 11, 2023 • edited Loading

Codecov Report

Abishalini left a comment

Choose a reason for hiding this comment

MarqRazz left a comment

Choose a reason for hiding this comment

mergify bot commented Oct 18, 2023

MarqRazz left a comment

Choose a reason for hiding this comment

sjahr commented Oct 23, 2023

MarqRazz commented Oct 23, 2023

MarqRazz left a comment

Choose a reason for hiding this comment

sjahr commented Oct 24, 2023

sea-bass left a comment

Choose a reason for hiding this comment

1: Removing the planner function from adaptAndPlan()?

2. Discussions on PlanRequestAdapter vs. PlanResponseAdapter

sea-bass commented Oct 24, 2023

sea-bass left a comment

Choose a reason for hiding this comment

sea-bass Nov 13, 2023

Choose a reason for hiding this comment

Abishalini commented Nov 13, 2023

sea-bass left a comment

Choose a reason for hiding this comment

sjahr commented Oct 11, 2023 •

edited

Loading

codecov bot commented Oct 11, 2023 •

edited

Loading

1: Removing the planner function from `adaptAndPlan()`?

2. Discussions on `PlanRequestAdapter` vs. `PlanResponseAdapter`