Add hybrid planning feature #488

sjahr · 2021-06-08T15:13:57Z

Description

This PR adds the new hybrid planning architecture to MoveIt2. The related issues that document the development process are #300 and #433.

To run the demo's you simply need to build moveit2 from source and make sure you use sjahr/moveit_msgs's hybrid_planning branch until the message definitions get merged into ros_planning/moveit_msgs.

The current setup includes a demo to verify the architecture's correct behavior. To run it use the following command:

ros2 launch moveit_hybrid_planning hybrid_planning.launch.py

The expected behavior looks like this:

You can change the configuration of the hybrid planning architecture by switching the different plugins used by the architecture's main components in the associated config file.

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Create tests, which fail without this PR reference
While waiting for someone to review your request, please help review another open pull request to support the maintainers

codecov · 2021-06-08T15:48:34Z

Codecov Report

Merging #488 (f9a4494) into main (0b27304) will increase coverage by 0.32%.
The diff coverage is n/a.

@@            Coverage Diff             @@
##             main     #488      +/-   ##
==========================================
+ Coverage   54.17%   54.48%   +0.32%     
==========================================
  Files         192      192              
  Lines       20182    20448     +266     
==========================================
+ Hits        10931    11139     +208     
- Misses       9251     9309      +58

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo_calcs.cpp	`73.13% <0.00%> (+2.61%)`	⬆️

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 0b27304...f9a4494. Read the comment docs.

AdamPettinger

Went through mostly the local planner stuff, but skimmed the manager and global as well.

This looks great! Very clean, and I think the plugin architecture will allow a lot of flexibility

AdamPettinger · 2021-06-08T17:32:47Z

moveit_ros/hybrid_planning/README.md

+Implementation of MoveIt 2's new hybrid planning architecture. You find more information in the project's issues[#300](https://github.com/ros-planning/moveit2/issues/300), [#433](https://github.com/ros-planning/moveit2/issues/433)  and on the [MoveIt 2 roadmap](https://moveit.ros.org/documentation/contributing/roadmap/).
+
+## Getting started
+1. Build moveit2 from source and make sure you use [this fork](https://github.com/sjahr/moveit_msgs) of moveit_msgs for the hybrid planning specific action definitions


Don't forget to change this when the msgs PR is merged

AdamPettinger · 2021-06-08T17:38:08Z

moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp

+  timer_ = this->create_wall_timer(1ms, [this]() {
+    if (initialized_)
+    {
+      timer_->cancel();


I know we talked about initializing this way with a timer. My understanding is right now this is the easiest way to access ROS node capabilities during initialization if this class inherits from rclcpp::Node, and that this will not be necessary with future lifecycle node work.

Maybe we should leave a TODO in all these initialization timers for after the lifecycle thing is sorted out?

AdamPettinger · 2021-06-08T17:51:35Z

.../hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp

+{
+  auto& clock = *hybrid_planning_manager_->get_clock();
+  RCLCPP_INFO_THROTTLE(LOGGER, clock, 1000, event.c_str());
+  return true;


Is this really desired behavior here? Looks like debugging, maybe it should give a warning that the SinglePlanExecute plugin doesn't handle string feedback?

AdamPettinger · 2021-06-08T17:52:58Z

...ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp

+
+bool ForwardTrajectory::reset()
+{
+  return true;


Just want to make sure it does not need to do anything on reset(), and not that this got overlooked

AdamPettinger · 2021-06-08T18:02:54Z

...planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h

+   * @param new_trajectory New reference trajectory segment to add
+   * @return True if segment was successfully added
+   */
+  virtual bool addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) = 0;


I think this function should include feedback (moveit_msgs::action::LocalPlanner::Feedback), perhaps as the return instead of a bool

AdamPettinger · 2021-06-08T18:17:42Z

moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp

+  if (pass_through_)
+  {
+    // Use reference_trajectory as local trajectory
+    local_trajectory.swap(*reference_trajectory_);


If I set the pass_through parameter true in the config file, I get a segfault, I believe at this line

AdamPettinger · 2021-06-08T18:19:21Z

moveit_ros/hybrid_planning/test/config/hybrid_planning_manager.yaml

@@ -0,0 +1 @@
+planner_logic_plugin_name: "moveit_hybrid_planning/ReplanInvalidatedTrajectory"


Is it possible to combine all these config files into 1 file? Do we even want to?

AdamPettinger · 2021-06-08T18:24:39Z

moveit_ros/hybrid_planning/test/launch/hybrid_planning.launch.py

+    load_controllers = []
+    for controller in [
+        "joint_state_controller",
+        "panda_joint_group_position_controller",


I would move to using the panda_arm_controller here as the default, because that is what the rest of the MoveIt tutorials and demos use

AdamPettinger · 2021-06-08T18:29:09Z

moveit_ros/hybrid_planning/test/config/demo_controller.yaml

@@ -0,0 +1,28 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz


Changing this (to 100) has strange effects on the demo, which I cannot explain. Sometimes the robot moves a bit and then stops

Is this an int type and not a floating point?

AdamPettinger · 2021-06-08T18:34:03Z

moveit_ros/hybrid_planning/test/config/local_planner.yaml

+pass_through: false
+
+# ForwardTrajectory param
+stop_before_collision: false


I think this should be true so the demonstration runs like the provided gif.

If this is false, the demo robot passes through the objects when I try it

moveit_ros/hybrid_planning/test/config/demo_controller.yaml

AdamPettinger · 2021-08-12T18:25:53Z

moveit_ros/hybrid_planning/CMakeLists.txt

+  global_planner/include
+  hybrid_planning_manager/hybrid_planning_manager_component/include
+  hybrid_planning_manager/planner_logic_plugins/include
+  local_planner/local_planner_component/include
+  local_planner/trajectory_operator_plugins/include
+  local_planner/local_constraint_solver_plugins/include


Suggested change

global_planner/include

hybrid_planning_manager/hybrid_planning_manager_component/include

hybrid_planning_manager/planner_logic_plugins/include

local_planner/local_planner_component/include

local_planner/trajectory_operator_plugins/include

local_planner/local_constraint_solver_plugins/include

global_planner/include/

hybrid_planning_manager/hybrid_planning_manager_component/include/

hybrid_planning_manager/planner_logic_plugins/include/

local_planner/local_planner_component/include/

local_planner/trajectory_operator_plugins/include/

local_planner/local_constraint_solver_plugins/include/

Without the trailing slash, these are installed at install/moveit_hybrid_planning/include/include/moveit/..... (notice the extra include) and other packages cannot find them without adding an include in the path

moveit_ros/hybrid_planning/CMakeLists.txt

AndyZe

I'll do another pass after you address this round of reviews. Looks good! I'm excited to see it develop further.

moveit_ros/hybrid_planning/README.md

moveit_ros/hybrid_planning/global_planner/CMakeLists.txt

...al_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h

...al_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h

...ner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h

...planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h

...al_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h

...planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp

...ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_interface.cpp

* Add hybrid_planning architecture skeleton code * Add simple state machines to hybrid planning manager and local planner * Initial global planner prototype implementation * Forward joint_trajectory with local planner * Forward hybrid planning motion request to global planner * Abstract planner logic from hybrid planning manager by using a plugin * Implement single plan execution logic * Add test launch files, RViz and demo config * Add test for motion_planning_request

* Make local planner component generic * Add next_waypoint_sampler trajectory operator * Update hybrid planning test to include collision object * Clean up code and fix minor bugs. * Update local planner component parameter * Add local collision constraint solver * Update planning scene topic name and test node * Fix bugs and runtime errors in local planner component and it's plugins * Add collision object that invalidates global trajectory * Add simple "stop in front of collision object" demo * Add hybrid planning manager reaction to local planner feedback * Fix ament_lint_cmake * Ensure that local planner start command and collision event are send once * Add simple replanning logic plugin * Use current state as start state for global planner * Use RobotTrajectory instead of constraint vector describe local problem * Add PlanningSceneMonitorPtr to local solver plugin * Add local planner frequency parameter * Use PID controller to create control outputs for robot controller * Add hybrid_planning_manager config file * Add more complex test node * Update README * Reset index in next_waypoint_sampler * Use correct isPathValid() interface * Rename path_invalidation flag * Read planning scene instead of cloning it in local planner * Add TODO creator * Rename local constraint solver plugin * Use read-locked access to the planning scene for collision checking * Rename constraint_solver into local_constraint_solver * Add missing pointer initialization

* Export missing plugins * Use std::chrono_literals * Construct smart pointers with std::make_* instead of 'new' * Fixup const-ref in function signatures, reduce copies * Improve planning scene locking, robot state processing, controller logic

* Add forward_trajectory local solver plugin (#359) * Use ros2_control based joint simulation and remove unnecessary comment * Update copyrights * Restructure hybrid planning package * Fix formatting and add missing time stamp in local solver plugin * Remove unnecessary logging and param loading * Enable different interfaces between local planner and controller * Use JointGroupPositionController as demo hardware controller

* Add reset method for trajectory operator and local constraint sampler * Refactor next_waypoint_sampler into simple_sampler * Include collision checking to forward_trajectory and remove unneeded plugin * Fix formatting and plugin description * Update README and add missing planner logic plugin description

…solver

…encies

henningkayser

Here a first round of comments. Also, two things that should be addressed:

Use include guards (i.e. #pragma once) for all headers
Consider using consistent prefixes/postfixes for plugin names. For instance SimpleSampler, SinglePlanExecutiondoes not really tell you what kind of plugin is or does. I'd suggest using postfixes like *PlannerLogic, *TrajectoryOp, GlobalPlanner, etc...

...al_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h

henningkayser · 2021-09-17T10:15:49Z

...al_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h

+  std::unique_ptr<pluginlib::ClassLoader<moveit_hybrid_planning::GlobalPlannerInterface>> global_planner_plugin_loader_;
+
+  // Global planner instance
+  std::shared_ptr<moveit_hybrid_planning::GlobalPlannerInterface> global_planner_instance_;


Use std::unique_ptr?

A unique_ptr leads to an issue when creating the plugin here. I'm not quite sure how to fix this.

global_planner_instance_ = global_planner_plugin_loader_->createUniqueInstance(global_planner_name_);

The error:

error: no match for ‘operator=’ (operand types are ‘std::unique_ptr<moveit_hybrid_planning::GlobalPlannerInterface>’ and ‘pluginlib::UniquePtr<moveit_hybrid_planning::GlobalPlannerInterface>’ {aka ‘std::unique_ptr<moveit_hybrid_planning::GlobalPlannerInterface, std::function<void(moveit_hybrid_planning::GlobalPlannerInterface*)> >’})

pluginlib has its own unique_ptr type which basically provides unique access to the single instance managed by the plugin loader. So you either need to use pluginlib::UniquePtr or create an unmanaged plugin instance. Thinking about this more, I'm not sure if any of these versions is better than simply using shared_ptr, though. @tylerjw any thoughts?

henningkayser · 2021-09-17T10:28:42Z

...ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp

+  last_global_solution_ = planning_solution;  // TODO(sjahr) Add Service to expose this
+
+  // Reset the global planner
+  if (!global_planner_instance_->reset())


Is this case even relevant? I think that resetting a planner should never fail. If possible, reset() should even be noexcept imo and we can remove this exception.

...obal_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h

..._planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h

henningkayser · 2021-09-17T11:21:15Z

..._planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h

+  std::unique_ptr<pluginlib::ClassLoader<moveit_hybrid_planning::PlannerLogicInterface>> planner_logic_plugin_loader_;
+
+  // Planner logic instance to implement reactive behavior
+  std::shared_ptr<moveit_hybrid_planning::PlannerLogicInterface> planner_logic_instance_;


unique_ptr?

Same issue with plugin loading, discussed above

..._planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h

henningkayser · 2021-09-28T20:54:23Z

..._planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h

+   * @param event Basic hybrid planning event
+   * @return Reaction result that summarizes the outcome of the reaction
+   */
+  virtual ReactionResult react(const BasicHybridPlanningEvent& event) = 0;


not sure if react() is a good function name here. Maybe use something like processEvent() or similar

...planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp

tylerjw · 2021-10-01T16:08:49Z

.github/workflows/ci.yaml

@@ -8,7 +8,7 @@ on:
  pull_request:
  push:
    branches:
-      - main


You don't need to change the branch here to trigger a CI run, you can do that just by pushing to your branch with an open PR.

By doing this you will cause CI to run twice for each time you push. Once for your push, and another because there was a push into a branch with a open PR.

I'll add a TODO to remove this. As I cannot directly push on the feature branch, I need to open PRs against it and from my understanding, without this change, these PRs wouldn't get checked by CI

.github/workflows/format.yaml

moveit2.repos

moveit_ros/hybrid_planning/CMakeLists.txt

moveit_ros/hybrid_planning/test/hybrid_planning_test_node.cpp

tylerjw · 2021-10-01T19:10:43Z

moveit_ros/hybrid_planning/test/hybrid_planning_test_node.cpp

+
+  HybridPlanningDemo demo(node);
+  std::thread run_demo([&demo]() {
+    rclcpp::sleep_for(5s);


this seems fairly fragile, have you tested this on slow machines?

moveit_ros/hybrid_planning/test/hybrid_planning_test_node.cpp

tylerjw · 2021-10-01T19:17:24Z

The lack of unit tests to test the functionality inside the core classes of this is sad. Please consider adding unit tests to this to verify the logic in the core classes of this.

moveit_ros/hybrid_planning/test/launch/hybrid_planning.launch.py

AndyZe · 2021-11-17T20:10:26Z

I'm working on addressing the code reviews but I can't push to this branch. I'll probably just open a new PR like Tyler did already with #763

Overall, this still works with a few tweaks but bit rot is starting to set in. Would like to get it merged sooner rather than later.

sjahr · 2021-11-18T15:01:03Z

I'm working on addressing the code reviews but I can't push to this branch. I'll probably just open a new PR like Tyler did already with #763

Overall, this still works with a few tweaks but bit rot is starting to set in. Would like to get it merged sooner rather than later.

@AndyZe Thanks for taking this up! I've also addressed a couple of things today and reopened a PR to add those things to the feature branch to make them visible in this PR. Feel free to add your changes to PR #811 or incorporate them in your own PR

AndyZe · 2021-11-18T15:29:13Z

Closing in favor of #811. I'll still track whether the comments get addressed here

* Add detail on custom IK plugin param declaration. MoveIt Servo dispenses with the `automatically_declare_parameters_from_overrides` machinery discussed here: https://github.com/orgs/ros-planning/discussions/1486 However, unlike a kinematics plugin loaded by a `move_group` node, this seems to mean that the kinematics plugin code is responsible for declaring any custom parameters on `/servo_node` that it needs to load from a MoveIt Config's `kinematics.yaml` Maybe this is not the case for configs generated with `MoveItConfigsBuilder`, but it was my experience with the launch strategy used by https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/main/ur_moveit_config * Typo on line number * Fix code tag formatting * getROSParam code tag * Whitespace cleanup Co-authored-by: AndyZe <andyz@utexas.edu>

henningkayser requested review from henningkayser and AdamPettinger June 8, 2021 15:34

AdamPettinger reviewed Jun 8, 2021

View reviewed changes

moveit_ros/hybrid_planning/test/config/demo_controller.yaml Show resolved Hide resolved

AdamPettinger reviewed Aug 12, 2021

View reviewed changes

AdamPettinger mentioned this pull request Aug 17, 2021

Servo as local planner demonstration PickNikRobotics/UR10e_welding_demo#28

Merged

henningkayser force-pushed the feature/hybrid_planning branch from 2cd7a46 to 41eed5b Compare August 24, 2021 13:27

AndyZe reviewed Sep 10, 2021

View reviewed changes

moveit_ros/hybrid_planning/CMakeLists.txt Outdated Show resolved Hide resolved

AndyZe reviewed Sep 14, 2021

View reviewed changes

AndyZe reviewed Sep 20, 2021

View reviewed changes

...ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_interface.cpp Outdated Show resolved Hide resolved

sjahr and others added 19 commits September 28, 2021 15:46

Add TODO to use lifecycle components nodes to trigger initialization

2fa7bcc

Return a reaction result instead of bool on react()

078cea2

Set invalidation flag to false on reset() in ForwardTrajectory local …

f0db150

…solver

Return local planner feedback from trajectory operator function calls

cbb694f

Fix segfault caused by passing through the global trajectory

edf8d7f

Update comment, unify logging and add missing config parameters

ec1382c

Update to rolling

35cb14e

Add generic global planner plugin, support MotionSequenceRequest (#585)

ccf7bce

Fix hybrid planning include folders (#675)

f9511e9

Order stuff in the CMakeLists.txt and remove control_box package

c2d0485

Update README

4ba7cbb

Move member initialization to initializer lists

f3eafdc

Remove control_box include dependency

9e77879

Replace "loop-run" with "iteration"

7518a48

sjahr added 6 commits September 28, 2021 15:46

Remove cpp interface class constructors and destructors

b918ac4

Use joint_state_broadcaster, clean up test node, add execution depend…

bdc5357

…encies

Use only plugin interface header files and add missing dependencies

dab4bf9

Clean up constructor and destructor definitions

bbf3ccc

Declare missing parameter in moveit_planning_pipeline_plugin

1d9b8a2

Move rclcpp::Loggers into anonymous namespaces

4409935

henningkayser force-pushed the feature/hybrid_planning branch from 341db91 to f9511e9 Compare September 28, 2021 15:59

sjahr mentioned this pull request Sep 28, 2021

Add hybrid planning tutorial moveit/moveit2_tutorials#97

Merged

2 tasks

Switch CI branches to feature/hybrid_planning

f9a4494

henningkayser reviewed Sep 28, 2021

View reviewed changes

tylerjw reviewed Oct 1, 2021

View reviewed changes

sjahr mentioned this pull request Oct 4, 2021

[Hybrid Planning] WIP: Address review #718

Closed

henningkayser reviewed Oct 5, 2021

View reviewed changes

moveit_ros/hybrid_planning/test/launch/hybrid_planning.launch.py Show resolved Hide resolved

sjahr mentioned this pull request Oct 20, 2021

Hybrid Planning Feature #433

Closed

7 tasks

henningkayser self-assigned this Oct 22, 2021

sjahr mentioned this pull request Oct 25, 2021

[WIP] ROS2 Port moveit/moveit_calibration#101

Open

tylerjw mentioned this pull request Oct 29, 2021

Hybrid Planning #763

Closed

6 tasks

sjahr mentioned this pull request Nov 18, 2021

Hybrid Planning: Add review changes to feature branch and rebase #811

Closed

3 tasks

AndyZe closed this Nov 18, 2021

This was referenced Nov 18, 2021

Add hybrid planning #813

Merged

Hybrid planning: eliminate timers in constructors #814

Open

AndyZe mentioned this pull request Jan 18, 2022

[hybrid planning] Add action abortion and test; improve the existing test #980

Merged

henningkayser deleted the feature/hybrid_planning branch May 24, 2022 12:23

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add hybrid planning feature #488

Add hybrid planning feature #488

sjahr commented Jun 8, 2021 •

edited

codecov bot commented Jun 8, 2021 •

edited

AdamPettinger left a comment

AdamPettinger Jun 8, 2021

AdamPettinger Jun 8, 2021

AdamPettinger Jun 8, 2021

AdamPettinger Jun 8, 2021

AdamPettinger Jun 8, 2021

AdamPettinger Jun 8, 2021

AdamPettinger Jun 8, 2021

AdamPettinger Jun 8, 2021

AdamPettinger Jun 8, 2021

tylerjw Oct 1, 2021

AdamPettinger Jun 8, 2021

AdamPettinger Aug 12, 2021

AndyZe left a comment

henningkayser left a comment

henningkayser Sep 17, 2021

AndyZe Nov 17, 2021 •

edited

henningkayser Nov 17, 2021 •

edited

henningkayser Sep 17, 2021

henningkayser Sep 17, 2021

AndyZe Nov 18, 2021

henningkayser Sep 28, 2021

tylerjw Oct 1, 2021

sjahr Oct 2, 2021

tylerjw Oct 1, 2021

tylerjw commented Oct 1, 2021

AndyZe commented Nov 17, 2021 •

edited

sjahr commented Nov 18, 2021 •

edited

AndyZe commented Nov 18, 2021

		@@ -0,0 +1 @@
		planner_logic_plugin_name: "moveit_hybrid_planning/ReplanInvalidatedTrajectory"

Add hybrid planning feature #488

Add hybrid planning feature #488

Conversation

sjahr commented Jun 8, 2021 • edited

Description

Checklist

codecov bot commented Jun 8, 2021 • edited

Codecov Report

AdamPettinger 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

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

AndyZe left a comment

Choose a reason for hiding this comment

henningkayser left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

AndyZe Nov 17, 2021 • edited

Choose a reason for hiding this comment

henningkayser Nov 17, 2021 • edited

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

tylerjw commented Oct 1, 2021

AndyZe commented Nov 17, 2021 • edited

sjahr commented Nov 18, 2021 • edited

AndyZe commented Nov 18, 2021

sjahr commented Jun 8, 2021 •

edited

codecov bot commented Jun 8, 2021 •

edited

AndyZe Nov 17, 2021 •

edited

henningkayser Nov 17, 2021 •

edited

AndyZe commented Nov 17, 2021 •

edited

sjahr commented Nov 18, 2021 •

edited