feat: planning manager

[takayuki5168]

This PR is to improve the whole planning architecture to solve the following issue.

• Time synchronization problem: Since each planner module is getting data separately (i.e. dynamic_object), it is unclear which module is using the data when generated. This makes it difficult to ensure consistency when validating the result of the planning module.

Approach to solve the issue:

• make a planning manager to manage the data used in the planning module.
• the manager access to each modules through service with the data and will get a result of each planning.
  • accordingly, all subscribers in each planning modules are removed.
• the validation can be implemented since all data used in planning are managed appropriately in one node.

Related Issue(required)

Description(required)

• Planning Data
  • Requests to each planning module (behavior_path, behavior_velocity, obstacle_avoidance, ...) has this planning data since data for planning will be synchronized over all planning modules.
• Planning Manager
  • Each subscriber to get planning data (dynamic object, ego pose/twist, FOA, ...) has its own callback group whose type is MutuallyExclusive
    • so that callback functions to get each planning data can be executed at the same time.
    • Note that the same callback functions (two callback functions for dynamic object) cannot be called since type is not Reentrant.
  • Each server to request planned path/trajectory or validated result to planning modules has its own callback group whose type is MutuallyExclusive
    • so that callback functions to request planned path/trajectory to planning modules
    • Note that the same callback functions (two callback functions for dynamic object) cannot be called since type is not Reentrant.
  • The main function is called in a fast cycle (i.e. 1000Hz) with MutuallyExclusive.
    • spawn new planning (not a thread) in a desired planning Hz (i.e. 10Hz) with a unique ID
      • This requests to the first planning (behavior_path)
    • Whether planning in a each module is done (asynchronized) is monitored in Planning Manager
      • When a planning finished, send the next planning request (behavior_path -> behavior_velocity -> obstacle_avoidance -> ...)
      • When all the plannings finished, send all the validate requests to all the planning modules
        • Note that for validation, there is no order (planning has an order like behavior_path -> behavior_velocity -> ...), so we can request to all the planning modules in parallel.
      • When all the validation finished, publish necessary topics and disable monitoring of planning with the ID

Review Procedure(required)

TBD

Related PR(optional)

PR for autoware launcher

Pre-Review Checklist for the PR Author

PR Author should check the checkboxes below when creating the PR.

• Read commit-guidelines
• Assign PR to reviewer

If you are adding new package following items are required:

• Documentation with description of the package is available
• A sample launch file and parameter file are available if the package contains executable nodes

Checklist for the PR Reviewer

Reviewers should check the checkboxes below before approval.

• Commits are properly organized and messages are according to the guideline
• PR title describes the changes

Post-Review Checklist for the PR Author

PR Author should check the checkboxes below before merging.

• All open points are addressed and tracked via issues or tickets

CI Checks

• Build and test for PR / build-and-test-pr: Required to pass before the merge.
• Build and test for PR / clang-tidy-pr: NOT required to pass before the merge. It is up to the reviewer(s). Found false positives? See the [guidelines][clang-tidy-guidelines].
• Check spelling: NOT required to pass before the merge. It is up to the reviewer(s). See here if you want to add some words to the spell check dictionary.

[takayuki5168]

cc @TakaHoribe @yukkysaito

[takayuki5168]

I'll close this PR since we will discuss the planning manager architecture again.