Backport: Planning with multiple pipelines in parallel with moveit_cpp #3244

sjahr · 2022-10-17T19:43:23Z

Description

This is a backport of the parallel planning API for moveit_cpp from moveit2#1420.

This PR adds:

A new interface for moveit_cpp to plan with multiple pipelines in parallel with a user-defined stopping criterion and solution selection function
A free function to the robot trajectory package to calculate the length of a given trajectory
Replace PlanSolution with extended MotionPlanResponse to reduce duplicate code
Add PlannerConfigurations for CHOMP, PILZ and trajopt (detailed motivation for this can be found in original moveit2 PR

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);

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

welcome · 2022-10-17T19:43:26Z

Thanks for helping in improving MoveIt and open source robotics!

codecov · 2022-10-17T21:23:34Z

Codecov Report

Base: 62.03% // Head: 61.82% // Decreases project coverage by -0.22% ⚠️

Coverage data is based on head (6aa4ca5) compared to base (4c10a9e).
Patch coverage: 29.37% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #3244      +/-   ##
==========================================
- Coverage   62.03%   61.82%   -0.21%     
==========================================
  Files         377      378       +1     
  Lines       33268    33373     +105     
==========================================
- Hits        20636    20630       -6     
- Misses      12632    12743     +111

Impacted Files	Coverage Δ
...include/moveit/robot_trajectory/robot_trajectory.h	`98.31% <ø> (ø)`
...eit_core/robot_trajectory/src/robot_trajectory.cpp	`67.42% <0.00%> (-1.81%)`	⬇️
.../moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h	`100.00% <ø> (ø)`
moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp	`62.30% <0.00%> (-6.20%)`	⬇️
...clude/moveit/planning_pipeline/planning_pipeline.h	`75.00% <0.00%> (-25.00%)`	⬇️
...cpp/include/moveit/moveit_cpp/planning_component.h	`47.83% <14.29%> (-52.17%)`	⬇️
...ros/planning/moveit_cpp/src/planning_component.cpp	`37.63% <21.63%> (-11.04%)`	⬇️
...anning/planning_pipeline/src/planning_pipeline.cpp	`81.53% <60.00%> (-0.70%)`	⬇️
...lude/moveit/planning_interface/planning_response.h	`100.00% <100.00%> (ø)`
...ude/moveit/planning_interface/planning_interface.h	`69.24% <0.00%> (-15.38%)`	⬇️
... and 16 more

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.

Downgrade log from ERROR to INFO

AndyZe · 2022-10-31T19:21:16Z

We've been using this a lot and I can attest that it's AWESOME! I'm going to review it over at MoveIt2, though, so I don't review twice.

rhaschke

I have a handful of remarks. The most important one is the API incompatibility with existing PlannerManager plugins. While you might break API in ROS2, we shouldn't do so in ROS1.

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

rhaschke · 2022-11-03T19:53:15Z

moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h

@@ -53,7 +53,15 @@ struct MotionPlanResponse

  robot_trajectory::RobotTrajectoryPtr trajectory_;
  double planning_time_;
-  moveit_msgs::MoveItErrorCodes error_code_;
+  moveit::core::MoveItErrorCode error_code_;
+  moveit_msgs::RobotState start_state_;


As far as I could see, both start_state_ and planner_id_ are not actually used. What's the purpose of these fields?

It is possible to pass an optional solution_selection_callback and/or a stopping_criterion_function both use the produced MotionPlanResponses to make a decision and these to additional fields might be useful to make that decision. For example

bool stoppingCriterionCallBack( moveit_cpp::PlanningComponent::PlanSolutions const& plan_solutions, moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters const& plan_request_parameters) for (auto const& solution : solutions) { // Stop parallel planning if PILZ found a solution if (solution.planner_id_ == "LIN") { return true; } } return false; }

I agree for the planner_id_, but all plans share the very same start state, don't they?
Please add a comment, e.g. "planner this solution originates from".

That's true, it does not matter for the solution selection. I added it since the MotionPlanResponse message has a similar field and it can be useful when the MotionPlanResponse is processed further independent from moveit_cpp/ the planning_component. Do you think we should remove it or is a comment sufficient?

I'm not sure yet. If the start state is identical for all solutions, saving it here would be a waste of memory.
However, due to different pipeline configs, modifying the start state (or not), the actual start state might differ between solutions (but I don't know by heart). That would be an argument to keep it here...

I'd remove the underscore from the field names but you're right, the API will be broken by this. I can open a PR for moveit_tutorials to fix this. Would MTC be affected?

I'm not only worried about moveit_tutorials or MTC. The problem is that any user code out there accessing those members will be affected by this change! At least we should provide a deprecation period for transition.
My suggestion is to:

remove the underscores

provide additional, deprecated members with the underscore as const references to the new members:
moveit_msgs::RobotState start_state; [[deprecated("Use start_state instead.")]] const moveit_msgs::RobotState& start_state_;
Obviously, those references need to be initialized in the constructor. This will render the struct read-only. So you will need to provide a custom copy constructor and assignment operator as well.

rhaschke · 2022-11-03T19:53:24Z

moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h

@@ -63,7 +71,15 @@ struct MotionPlanDetailedResponse
  std::vector<robot_trajectory::RobotTrajectoryPtr> trajectory_;
  std::vector<std::string> description_;
  std::vector<double> processing_time_;
-  moveit_msgs::MoveItErrorCodes error_code_;
+  moveit::core::MoveItErrorCode error_code_;
+  moveit_msgs::RobotState start_state_;


rhaschke · 2022-11-03T19:56:36Z

moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp

+      const planning_interface::PlannerConfigurationSettings planner_config_settings{
+        group, group, std::map<std::string, std::string>()
+      };
+      pconfig[planner_config_settings.name] = planner_config_settings;
    }
+    setPlannerConfigurations(pconfig);


What's the purpose of setting these empty planner configs?

Isn't it only empty when the robot model does not have any JointModelGroups? The line above adds for each joint model group settings

But those settings are simply empty: <std::string, std::string>()

rhaschke · 2022-11-03T19:58:14Z

moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp

+  void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pcs) override
+  {
+    config_settings_ = pcs;
+  }


Maybe introduce this as the default implementation of PlannerManager?

Personally, I'd like to remind the person that implements a new planner plugin to think about the planner config and forcing them to implement this function would be a good way. But I can make this a default implementation if you don't like that

I understand your reasoning and for MoveIt2 it is probably the right way to go.
For MoveIt1, I personally think we should maintain API stability. Other maintainers might have a different opinion.

Reverting this will be required for CI to pass...

Ok, let's do it that way then 👍

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h

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

Co-authored-by: Robert Haschke <[email protected]>

rhaschke · 2022-11-08T17:18:34Z

This PR had several clang-tidy warnings. Don't you have clang-tidy checks enabled anymore for MoveIt2?

rhaschke · 2022-11-08T17:26:20Z

How did you handle the API-incompatible member renamings in MoveIt2??
PlanSolution's trajectory and start_state members didn't have a trailing underscore, but MotionPlanResponse's members have!
This results in a failure in downstream packages accessing those public members.

sjahr · 2022-11-09T08:41:06Z

How did you handle the API-incompatible member renamings in MoveIt2?? PlanSolution's trajectory and start_state members didn't have a trailing underscore, but MotionPlanResponse's members have! This results in a failure in downstream packages accessing those public members.

Since MotionPlanResponse is a struct and all member variables are public the _ underscore naming convention for private member variables is not correctly used here anyway's. In moveit2 we opened an additional issue to fix this but I can included into this pr for simplicity's sake

sjahr · 2022-11-09T08:47:29Z

Yes, moveit2 uses clang-tidy but the related clang-tidy check is disabled moveit/moveit2#214

rhaschke · 2022-11-09T09:23:51Z

In moveit2 we opened an additional issue to fix this

I still don't know how you solved it in MoveIt2. Please add a reference.

but I can included into this pr for simplicity's sake

Depends on the solution approach you took. I don't yet see how this can be fixed in a fashion not breaking API for somebody.

rhaschke

There is still a lot of redundancy. Can you handle the missing config settings in moveitcpp instead?

rhaschke · 2022-11-10T09:29:52Z

moveit_core/planning_interface/src/planning_interface.cpp

+
+  // Construct default configurations for planning groups that don't have configs already passed in. This is necessary
+  // since moveit_cpp can only use planners with a config for a given joint group
+  for (const moveit::core::JointModelGroup* group : robot_model_->getJointModelGroups())
+  {
+    if (config_settings_.find(group->getName()) == config_settings_.end())
+    {
+      planning_interface::PlannerConfigurationSettings empty_config;
+      empty_config.name = empty_config.group = group->getName();
+      config_settings_[empty.name] = empty;
+    }
+  }


Why did you decide to initialize settings for "missing" groups here instead of in PlannerManager::initialize as suggested here?

Sorry, you reviewed to fast, that change did not work. The problem is that initialize() function can easily be overwritten and thus compatibility with moveit_cpp be destroyed. The way I implemented it would have added a default empty config for all planning plugins with setPlannerConfigurations. But that did not work because the robot model is not available in this function.

I think the planner plugins should be modified because the flaw right now is that pconfig is not used correctly (as expected by moveit_cpp) by the plugins. If we hide this implementation detail away in the base class, I fear future plugin implementations will repeat the same error,

pconfig is not used correctly (as expected by moveit_cpp) by the plugins.

Can you please detail this? As far as I can see, moveit_cpp just generates a "shortcut" map from these configs.
If there are no config settings, why should a plugin declare them as empty? I think we should first agree on the semantics, before implementing anything further...

My preference would be to use the current solution and open an issue to re-evaluate the PlannerConfigurationSettings, since it is only properly used by OMPL at the moment. Maybe it would be better to remove/refactor it and the pipeline detection in moveit_cpp but I think this is out of the scope of this PR.

From: moveit/moveit2#1522

When the plan() method of the planning component gets called, all available planning pipelines are searched based on their name [1]. These names are provided by moveit_cpp [2]. The moveit_cpp function that provides the planning pipeline names reads them from an internal map [3]. A planning pipelines needs to have a planner plugin that has a PlannerConfiguration in order to be added to this map [4].

moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp

rhaschke · 2022-11-10T09:34:37Z

moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp

@@ -279,7 +261,7 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations()
      ROS_DEBUG_STREAM_NAMED(LOGNAME, " - " << parameters.first << " = " << parameters.second);
  }

-  setPlannerConfigurations(pconfig);
+  context_manager_.setPlannerConfigurations(pconfig);


Why does the OMPL planner use a different approach to configure settings?
I would expect a similar approach to other planners, i.e. calling setPlannerConfigurations(pconfig).

I like the way, ompl defined a default setPlannerConfigurations implementation to add empty configs for joint model groups and moved that implementation into the base class. But that did not work

sjahr · 2022-11-11T09:35:43Z

moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h

+  moveit_msgs::RobotState start_state;
+  std::string planner_id;
+
+  [[deprecated("Use trajectory instead.")]] robot_trajectory::RobotTrajectoryPtr& trajectory_;


@rhaschke Probably I am doing something wrong, but if I make these references const, the API breaks again. Do I miss something here?

…plementation" This reverts commit c72ca11.

This reverts commit 5fd520f.

This reverts commit 8f96165.

- reverse naming: - underscore_ members stay the default - (deprecated) non_underscore members are added for API compatibility - add deprecated start_state member - avoid code duplication between copy constructor and copy assignment

This is a backport of moveit/moveit2#1710, dropping function getPlanningPipelineNames().

This member is not needed anymore.

rhaschke

If the latest version passes CI, I'm fine with the current state.

sjahr · 2022-11-11T11:25:25Z

👍 Thanks a lot for your help to get this merged!

welcome · 2022-11-11T11:53:28Z

Congrats on getting your first MoveIt pull request merged and improving open source robotics!

jspricke · 2022-11-16T10:14:47Z

Document API changes relevant to the user in the MIGRATION.md notes

I think it would be a good idea to add some notes on the API change there. @sjahr

v4hn · 2023-01-20T12:22:14Z

@rhaschke @sjahr I might misunderstand something just perusing this thread, but I just ran into these deprecations and they are the inverted logic of what was discussed throughout this thread. It recommends the historical trailing_ notation over clean public field names. Was that intended?

rhaschke · 2023-01-21T15:22:07Z

This PR removed the rather new data structure PlanningComponent::PlanSolution using member names w/o an underscore and replaced it with PlanSolutions, which is essentially a vector of MotionPlanResponse. The latter in turn used very similar members, but with a trailing underscore.
Due to this incompatibility of member names between PlanSolution and MotionPlanResponse we had to break API for one of them. I suggested not modifying this older data structure and sticking to the more widely used member names with the trailing underscore. All MoveItCpp code already using the new member names thus exhibits the deprecation warnings now.

v4hn · 2023-01-23T21:31:51Z

Thanks for the explanation. Not great to keep the legacy annotation _ when code is reworked anyway, but fair enough.

rhaschke · 2023-01-24T07:19:51Z

Our naming rules enforce the underscore:
https://github.com/ros-planning/moveit/blob/e4ede1f2a88a763554ee5e62b39ef0bd70bbc061/.clang-tidy#L45-L46

Why do you claim this notation legacy?

v4hn · 2023-01-25T10:49:21Z

Why do you claim this notation legacy?

Somewhat a mix of personal opinion, conventions I know and the class/struct dichotomy I guess. (MotionPlanResponse is a struct)
From my perspective the suffix should only be added to protected and private members (public members in classes should be shunned anyway).
E.g. google's code conventions splits structs and classes based on data access & functionality and only class members are expected to have the suffix.
I'm surprised clang-tidy does not seem to support StructMemberSuffix separately.

rhaschke · 2023-01-25T10:55:51Z

There are Public, Protected, and Private variants for all rules. So we could remove the suffix for public members...
https://clang.llvm.org/extra/clang-tidy/checks/readability/identifier-naming.html#cmdoption-arg-publicmembersuffix

I have triggered a corresponding build: https://github.com/ros-planning/moveit/actions/runs/4005320877
Let's see how many API changes this will trigger...

See moveit/moveit#3244

* Fix old-style-cast * Fix deprecated member names (due to moveit/moveit#3244) * Enable -Werror

sjahr added 5 commits October 8, 2022 12:15

Backport parallel planning feature from MoveIt2

d48fb27

Backport: moveit2#1522 Add planner configurations to CHOMP and PILZ

9a1ab3e

Fix not executed trajectory

be2f00b

Format

ac3365e

Make successful planner id accessible and make getSolutions) const

d66dd22

sjahr requested review from jschleicher, mamoll, v4hn, mlautman, rhaschke and felixvd as code owners October 17, 2022 19:43

stephanie-eng and others added 2 commits October 28, 2022 14:47

Downgrade log from ERROR to INFO

c720af2

Merge pull request #1 from stephanie-eng/patch-3

7273f71

Downgrade log from ERROR to INFO

rhaschke requested changes Nov 3, 2022

View reviewed changes

Fix clang-format

59fa76c

rhaschke mentioned this pull request Nov 3, 2022

Remove unused members from MotionPlanDetailedResponse sjahr/moveit#2

Closed

sjahr and others added 3 commits November 7, 2022 11:12

Apply suggestions from code review

a01cf6e

Co-authored-by: Robert Haschke <[email protected]>

Backport review from moveit2

58f02fe

Restore const functions and make active_ flage mutable

e41580c

sjahr requested a review from rhaschke November 7, 2022 10:16

rhaschke added 2 commits November 8, 2022 10:37

Revert to non-abstract virtual method setPlannerConfigurations()

3692f77

Apply clang-tidy suggestions

67912c9

sjahr force-pushed the pr-add_parallel_planning branch 2 times, most recently from 279038b to c72ca11 Compare November 10, 2022 09:36

rhaschke reviewed Nov 10, 2022

View reviewed changes

rhaschke mentioned this pull request Nov 10, 2022

Cleanup lookup of planning pipelines in MoveItCpp moveit/moveit2#1710

Merged

Deprecate fields with _

1d7af17

sjahr commented Nov 11, 2022

View reviewed changes

rhaschke added 9 commits November 11, 2022 11:31

Revert "Remove setPlannerConfigurations(...) duplicates of default im…

3831147

…plementation" This reverts commit c72ca11.

Revert "Format"

ca484b6

This reverts commit 5fd520f.

Revert "Re-add planner configs to CHOMP, PILZ and TrajOpt"

7075798

This reverts commit 8f96165.

Revert changes to MotionPlanDetailedResponse

19816c7

Fix MotionPlanResponse

71b1a14

- reverse naming: - underscore_ members stay the default - (deprecated) non_underscore members are added for API compatibility - add deprecated start_state member - avoid code duplication between copy constructor and copy assignment

Ignore deprecation warnings in constructor

9d9df23

Cleanup lookup of planning pipelines

08a48b3

This is a backport of moveit/moveit2#1710, dropping function getPlanningPipelineNames().

Remove MoveItCpp::groups_pipelines_map_

aeb568c

This member is not needed anymore.

fixup! Cleanup lookup of planning pipelines

6aa4ca5

rhaschke approved these changes Nov 11, 2022

View reviewed changes

rhaschke merged commit 5e6f817 into moveit:master Nov 11, 2022

rhaschke mentioned this pull request Jan 25, 2023

Fix public member naming #3317

Closed

rhaschke added a commit to rhaschke/moveit_tutorials that referenced this pull request Jan 31, 2023

Fix deprecated member names

ebdca6c

See moveit/moveit#3244

rhaschke added a commit to moveit/moveit_tutorials that referenced this pull request Jan 31, 2023

CI: Fix compiler warnings and enable -Werror (#752)

3e78922

* Fix old-style-cast * Fix deprecated member names (due to moveit/moveit#3244) * Enable -Werror

v4hn mentioned this pull request Mar 21, 2023

clarify which projects are moveit 1 and 2, add ompl project moveit/moveit.ros.org#784

Merged

2 tasks

Backport: Planning with multiple pipelines in parallel with moveit_cpp #3244

Backport: Planning with multiple pipelines in parallel with moveit_cpp #3244

Conversation

sjahr commented Oct 17, 2022

Description

Usage example:

Checklist

welcome bot commented Oct 17, 2022

codecov bot commented Oct 17, 2022 • edited Loading

Codecov Report

AndyZe commented Oct 31, 2022 • edited Loading

rhaschke left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sjahr Nov 7, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke commented Nov 8, 2022

rhaschke commented Nov 8, 2022

sjahr commented Nov 9, 2022

sjahr commented Nov 9, 2022 • edited Loading

rhaschke commented Nov 9, 2022

rhaschke left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sjahr Nov 10, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sjahr Nov 10, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

sjahr commented Nov 11, 2022

welcome bot commented Nov 11, 2022

jspricke commented Nov 16, 2022

v4hn commented Jan 20, 2023 • edited Loading

rhaschke commented Jan 21, 2023

v4hn commented Jan 23, 2023

rhaschke commented Jan 24, 2023 • edited Loading

v4hn commented Jan 25, 2023 • edited Loading

rhaschke commented Jan 25, 2023 • edited Loading

codecov bot commented Oct 17, 2022 •

edited

Loading

AndyZe commented Oct 31, 2022 •

edited

Loading

sjahr Nov 7, 2022 •

edited

Loading

sjahr commented Nov 9, 2022 •

edited

Loading

sjahr Nov 10, 2022 •

edited

Loading

sjahr Nov 10, 2022 •

edited

Loading

v4hn commented Jan 20, 2023 •

edited

Loading

rhaschke commented Jan 24, 2023 •

edited

Loading

v4hn commented Jan 25, 2023 •

edited

Loading

rhaschke commented Jan 25, 2023 •

edited

Loading