Hybrid planning architecture#311

Merged

henningkayser merged 23 commits intomoveit:feature/hybrid_planningfrom

sjahr:pr-hybrid_planning_dev

Jan 6, 2021

Contributor

sjahr commented Nov 19, 2020 •

edited

Loading

Description

Implementation of hybrid planning architecture described in #300

For testing, this custom version of the moveit_msgs package is necessary in order to get the new action definitions.

After building, run

ros2 launch moveit_hybrid_planning hybrid_planning.launch.py

to start the hybrid planning component container and send test hybrid planning 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

sjahr force-pushed the pr-hybrid_planning_dev branch from 5d14f7c to 4664e29 Compare

November 23, 2020 12:05

sjahr mentioned this pull request

Hybrid Planning feature #300

Closed

8 tasks

sjahr force-pushed the pr-hybrid_planning_dev branch from db110b9 to 2896079 Compare

December 3, 2020 09:40

sjahr added 12 commits

December 7, 2020 16:39


          Add hybrid_planning skeleton code

e5617b9


          Add simple state-machine to hybrid planning manager

e3cce1e


          Add global trajectory topic and local planner internal state-machine

010353e


          Move hybrid_planning_namespace as subnamespace into moveit namespace

02618e3


          Add first global planner prototype implementation

a55e8b8


          Move launch files into test dir and add global planner demo config

1da98ac


          Refactor test action client

962adb5


          Forward hybrid planning motion request to global planner

b0346a2


          Fix global planning and add test motion_planning_request.

0008bef


          Add rviz config

8ec8633


          Rename test executable and two action interfaces

75f7119


          Forward joint_trajectory with local planner

5a99eb5

sjahr force-pushed the pr-hybrid_planning_dev branch from 78065a0 to 5a99eb5 Compare

December 7, 2020 15:40

sjahr changed the base branch from main to feature/hybrid_planning

December 10, 2020 13:21

sjahr force-pushed the pr-hybrid_planning_dev branch 2 times, most recently from bc378d8 to 5f7f099 Compare

December 11, 2020 08:36


          Restructure hybrid_planning package structure and clean-up global pla…

3b2c467

…nner

sjahr force-pushed the pr-hybrid_planning_dev branch from 5f7f099 to 3b2c467 Compare

December 11, 2020 15:59

Contributor Author

sjahr commented Dec 11, 2020

@henningkayser I've fixed clang_format and ament_tidy errors, but CI currently fails because I've used new action interface definitions. What do you think is the best way to handle this?

Contributor

felixvd commented Dec 14, 2020

Generally we merge the moveit_msgs PR first and then confirm that CI succeeds.

henningkayser reviewed

View reviewed changes

moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp Outdated Show resolved Hide resolved

moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp Outdated Show resolved Hide resolved

moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp

+                }
+                // Load planning pipelines
+                for (const auto& planning_pipeline_name : config_.pipeline_names)

Member

henningkayser Dec 16, 2020

Looking at this, we might want to just use MoveItCpp in the future in order to reduce redundancy. Of course, we would only load planning pipelines and PSM, nothing else.

moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp Outdated Show resolved Hide resolved

moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp

+                last_global_solution_ = planning_solution;  // TODO Add Service to expose this
+              };
+              moveit_msgs::msg::MotionPlanResponse GlobalPlannerComponent::plan(moveit_msgs::msg::MotionPlanRequest planning_problem)

Member

henningkayser Dec 16, 2020

As already discussed, we should consider moving the actual implementation into its own class.

moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp

+                // Clone current planning scene
+                planning_scene_monitor_->updateFrameTransforms();
+                planning_scene_monitor_->lockSceneRead();  // LOCK planning scene
+                ::planning_scene::PlanningScenePtr planning_scene =

Member

henningkayser Dec 16, 2020

Cloning planning scenes is really costly. Since we are maintaining our own planning scene here, we could simply lock the scene and pass it directly to the planning pipeline.

moveit_ros/hybrid_planning/hybrid_planning_manager/src/hybrid_planning_manager.cpp Show resolved Hide resolved

moveit_ros/hybrid_planning/hybrid_planning_manager/src/hybrid_planning_manager.cpp Show resolved Hide resolved

moveit_ros/hybrid_planning/hybrid_planning_manager/src/hybrid_planning_manager.cpp Outdated Show resolved Hide resolved

moveit_ros/hybrid_planning/local_planner/include/moveit/local_planner/local_planner_component.h

+              // TODO Use lifecycle node?
+              enum class LocalPlannerState : int8_t
+              {
+                ABORT = -1,

Member

henningkayser Dec 17, 2020

Do we have a concise documentation for the states and command flags you're introducing? To me it seems like the logic should be moved to different headers and source files so that we can refactor it more easily in the future.

sjahr added 4 commits

December 20, 2020 12:17


          Rename action interfaces

06857f2


          Temporary use fork of moveit_msgs repository

aee6cb6


          Rename callback functions and append 'Callback' to their names

88aa5ad


          Remove unnecessary parameter declarations

6ecd547

codecov bot commented Dec 20, 2020 •

edited

Loading

Codecov Report

Merging #311 (d96e1e5) into feature/hybrid_planning (0bd801f) will not change coverage.
The diff coverage is n/a.

@@                   Coverage Diff                    @@
##           feature/hybrid_planning     #311   +/-   ##
========================================================
  Coverage                    48.00%   48.00%           
========================================================
  Files                          157      157           
  Lines                        14843    14843           
========================================================
  Hits                          7124     7124           
  Misses                        7719     7719

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 0bd801f...d96e1e5. Read the comment docs.


          Add initialized function to hybrid planning manager and fix clang-format

6b2b454

sjahr force-pushed the pr-hybrid_planning_dev branch from 64b4a81 to b8fae87 Compare

January 6, 2021 13:57


          Apply clang-tidy fixes and remove invalid "react" function call

d96e1e5

sjahr force-pushed the pr-hybrid_planning_dev branch from b8fae87 to d96e1e5 Compare

January 6, 2021 15:29

sjahr changed the title ~~WIP: Hybrid planning architecture~~ Hybrid planning architecture

henningkayser approved these changes

View reviewed changes

Member

henningkayser left a comment

lgtm

henningkayser merged commit 910cd87 into moveit:feature/hybrid_planning

henningkayser pushed a commit to henningkayser/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

55e9538

* 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

henningkayser pushed a commit to henningkayser/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

7ca84f0

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

6bd97e9

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

6ddc8c2

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

133f135

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

433f436

* 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

henningkayser pushed a commit that referenced this pull request


          Hybrid planning architecture (#311)

a98e8ec

* 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

henningkayser pushed a commit that referenced this pull request


          Hybrid planning architecture (#311)

1e12a9d

* 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

sjahr deleted the pr-hybrid_planning_dev branch

May 20, 2021 09:06

sjahr restored the pr-hybrid_planning_dev branch

May 20, 2021 09:08

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

9e75efb

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

890aca8

* 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

henningkayser pushed a commit to henningkayser/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

71dfcb5

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

1953be0

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

89b51a9

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

270a2f6

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

35e0605

* 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

sjahr added a commit to sjahr/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

6ce2e1a

* 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

AndyZe pushed a commit to AndyZe/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

44cc36a

* 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

AndyZe pushed a commit to AndyZe/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

457eade

* 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

AndyZe pushed a commit to AndyZe/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

4000ea3

* 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

AndyZe pushed a commit to AndyZe/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

fef3702

* 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

AndyZe pushed a commit to AndyZe/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

0add007

* 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

AndyZe pushed a commit to AndyZe/moveit2 that referenced this pull request


          Hybrid planning architecture (moveit#311)

8e488ef

* 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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet