-
Notifications
You must be signed in to change notification settings - Fork 504
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
[Planning Pipeline Refactoring] #1 Simplify Adapter - Planner chain #2429
Conversation
Codecov ReportAttention:
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. |
moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h
Outdated
Show resolved
Hide resolved
moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h
Outdated
Show resolved
Hide resolved
moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/default_request_adapters_plugin_description.xml
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
Outdated
Show resolved
Hide resolved
...ros/planning/planning_response_adapter_plugins/res/default_plan_response_adapter_params.yaml
Outdated
Show resolved
Hide resolved
moveit_ros/planning/default_response_adapters_plugin_description.xml
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml
Show resolved
Hide resolved
This pull request is in conflict. Could you fix it @sjahr? |
a4d32a5
to
1992fe5
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
Show resolved
Hide resolved
moveit_ros/planning/default_response_adapters_plugin_description.xml
Outdated
Show resolved
Hide resolved
@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
Outdated
Show resolved
Hide resolved
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???
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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! |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
Outdated
Show resolved
Hide resolved
moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h
Outdated
Show resolved
Hide resolved
moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h
Outdated
Show resolved
Hide resolved
moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/default_request_adapters_plugin_description.xml
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp
Show resolved
Hide resolved
moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp
Outdated
Show resolved
Hide resolved
Testing with the Kinova config from the tutorials, I just found a bug with the default Pilz config that gets loaded by |
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
de17a4e
to
602e859
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
Outdated
Show resolved
Hide resolved
moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h
Outdated
Show resolved
Hide resolved
|
||
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(), |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
Outdated
Show resolved
Hide resolved
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>
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Let's do this! 🚢
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
and
Expected output
Changes
PipelineState
publisher which publishes the intermediate state between the pipeline stages.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 instancemoveit::core::MoveItStatus PlanningRequestAdapter::adapt(..
Plan request adapters return a status containing error informationvirtual void solve(MotionPlanResponse& res) = 0;
uses the response to communicate success or failurevirtual void adapt(..., planning_interface::MotionPlanResponse& res)
also uses the response for communicationTODOs