From 36687f1666211edf5283bdd345242d83154c0440 Mon Sep 17 00:00:00 2001
From: yamazakiTasuku <100691117+yamazakiTasuku@users.noreply.github.com>
Date: Thu, 13 Apr 2023 12:15:11 +0900
Subject: [PATCH 01/15] refactor(pure_pursuit): delete default values (#2942)

* delete param

Signed-off-by: yamazakiTasuku <tasuku.yamazaki@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: yamazakiTasuku <tasuku.yamazaki@tier4.jp>
Co-authored-by: yamazakiTasuku <tasuku.yamazaki@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../pure_pursuit_lateral_controller.cpp       | 28 +++++++++----------
 .../src/pure_pursuit/pure_pursuit_node.cpp    |  9 +++---
 2 files changed, 18 insertions(+), 19 deletions(-)

diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp
index 609850c89a7ea..cb812202d8651 100644
--- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp
+++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp
@@ -67,25 +67,25 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node)
   param_.max_steering_angle = vehicle_info.max_steer_angle_rad;
 
   // Algorithm Parameters
-  param_.ld_velocity_ratio = node_->declare_parameter<double>("ld_velocity_ratio", 2.4);
-  param_.ld_lateral_error_ratio = node_->declare_parameter<double>("ld_lateral_error_ratio", 3.6);
-  param_.ld_curvature_ratio = node_->declare_parameter<double>("ld_curvature_ratio", 120.0);
+  param_.ld_velocity_ratio = node_->declare_parameter<double>("ld_velocity_ratio");
+  param_.ld_lateral_error_ratio = node_->declare_parameter<double>("ld_lateral_error_ratio");
+  param_.ld_curvature_ratio = node_->declare_parameter<double>("ld_curvature_ratio");
   param_.long_ld_lateral_error_threshold =
-    node_->declare_parameter<double>("long_ld_lateral_error_threshold", 0.5);
-  param_.min_lookahead_distance = node_->declare_parameter<double>("min_lookahead_distance", 4.35);
-  param_.max_lookahead_distance = node_->declare_parameter<double>("max_lookahead_distance", 15.0);
+    node_->declare_parameter<double>("long_ld_lateral_error_threshold");
+  param_.min_lookahead_distance = node_->declare_parameter<double>("min_lookahead_distance");
+  param_.max_lookahead_distance = node_->declare_parameter<double>("max_lookahead_distance");
   param_.reverse_min_lookahead_distance =
-    node_->declare_parameter<double>("reverse_min_lookahead_distance", 7.0);
-  param_.converged_steer_rad_ = node_->declare_parameter<double>("converged_steer_rad", 0.1);
-  param_.prediction_ds = node_->declare_parameter<double>("prediction_ds", 0.3);
+    node_->declare_parameter<double>("reverse_min_lookahead_distance");
+  param_.converged_steer_rad_ = node_->declare_parameter<double>("converged_steer_rad");
+  param_.prediction_ds = node_->declare_parameter<double>("prediction_ds");
   param_.prediction_distance_length =
-    node_->declare_parameter<double>("prediction_distance_length", 21.0);
-  param_.resampling_ds = node_->declare_parameter<double>("resampling_ds", 0.1);
+    node_->declare_parameter<double>("prediction_distance_length");
+  param_.resampling_ds = node_->declare_parameter<double>("resampling_ds");
   param_.curvature_calculation_distance =
-    node_->declare_parameter<double>("curvature_calculation_distance", 4.0);
-  param_.enable_path_smoothing = node_->declare_parameter<bool>("enable_path_smoothing", true);
+    node_->declare_parameter<double>("curvature_calculation_distance");
+  param_.enable_path_smoothing = node_->declare_parameter<bool>("enable_path_smoothing");
   param_.path_filter_moving_ave_num =
-    node_->declare_parameter<int64_t>("path_filter_moving_ave_num", 25);
+    node_->declare_parameter<int64_t>("path_filter_moving_ave_num");
 
   // Debug Publishers
   pub_debug_marker_ =
diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp
index 59a89c9159672..254b83bccbc34 100644
--- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp
+++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp
@@ -63,14 +63,13 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options)
   param_.wheel_base = vehicle_info.wheel_base_m;
 
   // Node Parameters
-  param_.ctrl_period = this->declare_parameter<double>("control_period", 0.02);
+  param_.ctrl_period = this->declare_parameter<double>("control_period");
 
   // Algorithm Parameters
-  param_.lookahead_distance_ratio =
-    this->declare_parameter<double>("lookahead_distance_ratio", 2.2);
-  param_.min_lookahead_distance = this->declare_parameter<double>("min_lookahead_distance", 2.5);
+  param_.lookahead_distance_ratio = this->declare_parameter<double>("lookahead_distance_ratio");
+  param_.min_lookahead_distance = this->declare_parameter<double>("min_lookahead_distance");
   param_.reverse_min_lookahead_distance =
-    this->declare_parameter<double>("reverse_min_lookahead_distance", 7.0);
+    this->declare_parameter<double>("reverse_min_lookahead_distance");
 
   // Subscribers
   using std::placeholders::_1;

From b8857a262efeb3ce45ae519ac968b0d634ac5450 Mon Sep 17 00:00:00 2001
From: "awf-autoware-bot[bot]"
 <94889083+awf-autoware-bot[bot]@users.noreply.github.com>
Date: Thu, 13 Apr 2023 03:23:05 +0000
Subject: [PATCH 02/15] chore: sync files (#3387)

Signed-off-by: GitHub <noreply@github.com>
Co-authored-by: shmpwk <shmpwk@users.noreply.github.com>
---
 .github/PULL_REQUEST_TEMPLATE/small-change.md    | 6 ++++++
 .github/PULL_REQUEST_TEMPLATE/standard-change.md | 8 ++++++++
 2 files changed, 14 insertions(+)

diff --git a/.github/PULL_REQUEST_TEMPLATE/small-change.md b/.github/PULL_REQUEST_TEMPLATE/small-change.md
index 527c8ed81fded..2ff933c43a349 100644
--- a/.github/PULL_REQUEST_TEMPLATE/small-change.md
+++ b/.github/PULL_REQUEST_TEMPLATE/small-change.md
@@ -9,6 +9,12 @@
 
 Not applicable.
 
+## Effects on system behavior
+
+<!-- Describe how this PR affects the system behavior. -->
+
+Not applicable.
+
 ## Pre-review checklist for the PR author
 
 The PR author **must** check the checkboxes below when creating the PR.
diff --git a/.github/PULL_REQUEST_TEMPLATE/standard-change.md b/.github/PULL_REQUEST_TEMPLATE/standard-change.md
index cfdf7101b5afd..7aedefd0a7bf1 100644
--- a/.github/PULL_REQUEST_TEMPLATE/standard-change.md
+++ b/.github/PULL_REQUEST_TEMPLATE/standard-change.md
@@ -14,6 +14,14 @@
 
 <!-- Write additional information if necessary. It should be written if there are related PRs that should be merged at the same time. -->
 
+## Interface changes
+
+<!-- Describe any changed interfaces, such as topics, services, or parameters. -->
+
+## Effects on system behavior
+
+<!-- Describe how this PR affects the system behavior. -->
+
 ## Pre-review checklist for the PR author
 
 The PR author **must** check the checkboxes below when creating the PR.

From aeaa1f7b4e36e37df6dd5dcf2895a6f814f509cd Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Date: Thu, 13 Apr 2023 12:39:42 +0900
Subject: [PATCH 03/15] refactor(behavior_path_planner): unify module manager
 param getter (#3379)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../src/behavior_path_planner_node.cpp        | 119 +++---------------
 1 file changed, 20 insertions(+), 99 deletions(-)

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index 6b6e51cd868f1..d602e40bda1b8 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -328,108 +328,29 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
 
   p.verbose = declare_parameter<bool>("verbose");
 
-  {
-    const std::string ns = "pull_out.";
-    p.config_pull_out.enable_module = declare_parameter<bool>(ns + "enable_module");
-    p.config_pull_out.enable_simultaneous_execution_as_approved_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
-    p.config_pull_out.enable_simultaneous_execution_as_candidate_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
-    p.config_pull_out.priority = declare_parameter<int>(ns + "priority");
-    p.config_pull_out.max_module_size = declare_parameter<int>(ns + "max_module_size");
-  }
-
-  {
-    const std::string ns = "pull_over.";
-    p.config_pull_over.enable_module = declare_parameter<bool>(ns + "enable_module");
-    p.config_pull_over.enable_simultaneous_execution_as_approved_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
-    p.config_pull_over.enable_simultaneous_execution_as_candidate_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
-    p.config_pull_over.priority = declare_parameter<int>(ns + "priority");
-    p.config_pull_over.max_module_size = declare_parameter<int>(ns + "max_module_size");
-  }
-
-  {
-    const std::string ns = "side_shift.";
-    p.config_side_shift.enable_module = declare_parameter<bool>(ns + "enable_module");
-    p.config_side_shift.enable_simultaneous_execution_as_approved_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
-    p.config_side_shift.enable_simultaneous_execution_as_candidate_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
-    p.config_side_shift.priority = declare_parameter<int>(ns + "priority");
-    p.config_side_shift.max_module_size = declare_parameter<int>(ns + "max_module_size");
-  }
-
-  {
-    const std::string ns = "lane_change_left.";
-    p.config_lane_change_left.enable_module = declare_parameter<bool>(ns + "enable_module");
-    p.config_lane_change_left.enable_simultaneous_execution_as_approved_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
-    p.config_lane_change_left.enable_simultaneous_execution_as_candidate_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
-    p.config_lane_change_left.priority = declare_parameter<int>(ns + "priority");
-    p.config_lane_change_left.max_module_size = declare_parameter<int>(ns + "max_module_size");
-  }
-
-  {
-    const std::string ns = "lane_change_right.";
-    p.config_lane_change_right.enable_module = declare_parameter<bool>(ns + "enable_module");
-    p.config_lane_change_right.enable_simultaneous_execution_as_approved_module =
+  const auto get_scene_module_manager_param = [&](std::string && ns) {
+    ModuleConfigParameters config;
+    config.enable_module = declare_parameter<bool>(ns + "enable_module");
+    config.enable_simultaneous_execution_as_approved_module =
       declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
-    p.config_lane_change_right.enable_simultaneous_execution_as_candidate_module =
+    config.enable_simultaneous_execution_as_candidate_module =
       declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
-    p.config_lane_change_right.priority = declare_parameter<int>(ns + "priority");
-    p.config_lane_change_right.max_module_size = declare_parameter<int>(ns + "max_module_size");
-  }
-
-  {
-    const std::string ns = "external_request_lane_change_right.";
-    p.config_ext_request_lane_change_right.enable_module =
-      declare_parameter<bool>(ns + "enable_module");
-    p.config_ext_request_lane_change_right.enable_simultaneous_execution_as_approved_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
-    p.config_ext_request_lane_change_right.enable_simultaneous_execution_as_candidate_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
-    p.config_ext_request_lane_change_right.priority = declare_parameter<int>(ns + "priority");
-    p.config_ext_request_lane_change_right.max_module_size =
-      declare_parameter<int>(ns + "max_module_size");
-  }
-
-  {
-    const std::string ns = "external_request_lane_change_left.";
-    p.config_ext_request_lane_change_left.enable_module =
-      declare_parameter<bool>(ns + "enable_module");
-    p.config_ext_request_lane_change_left.enable_simultaneous_execution_as_approved_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
-    p.config_ext_request_lane_change_left.enable_simultaneous_execution_as_candidate_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
-    p.config_ext_request_lane_change_left.priority = declare_parameter<int>(ns + "priority");
-    p.config_ext_request_lane_change_left.max_module_size =
-      declare_parameter<int>(ns + "max_module_size");
-  }
-
-  {
-    const std::string ns = "avoidance.";
-    p.config_avoidance.enable_module = declare_parameter<bool>(ns + "enable_module");
-    p.config_avoidance.enable_simultaneous_execution_as_approved_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
-    p.config_avoidance.enable_simultaneous_execution_as_candidate_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
-    p.config_avoidance.priority = declare_parameter<int>(ns + "priority");
-    p.config_avoidance.max_module_size = declare_parameter<int>(ns + "max_module_size");
-  }
+    config.priority = declare_parameter<int>(ns + "priority");
+    config.max_module_size = declare_parameter<int>(ns + "max_module_size");
+    return config;
+  };
 
-  {
-    const std::string ns = "avoidance_by_lc.";
-    p.config_avoidance_by_lc.enable_module = declare_parameter<bool>(ns + "enable_module");
-    p.config_avoidance_by_lc.enable_simultaneous_execution_as_approved_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
-    p.config_avoidance_by_lc.enable_simultaneous_execution_as_candidate_module =
-      declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
-    p.config_avoidance_by_lc.priority = declare_parameter<int>(ns + "priority");
-    p.config_avoidance_by_lc.max_module_size = declare_parameter<int>(ns + "max_module_size");
-  }
+  p.config_pull_out = get_scene_module_manager_param("pull_out.");
+  p.config_pull_over = get_scene_module_manager_param("pull_over.");
+  p.config_side_shift = get_scene_module_manager_param("side_shift.");
+  p.config_lane_change_left = get_scene_module_manager_param("lane_change_left.");
+  p.config_lane_change_right = get_scene_module_manager_param("lane_change_right.");
+  p.config_ext_request_lane_change_right =
+    get_scene_module_manager_param("external_request_lane_change_right.");
+  p.config_ext_request_lane_change_left =
+    get_scene_module_manager_param("external_request_lane_change_left.");
+  p.config_avoidance = get_scene_module_manager_param("avoidance.");
+  p.config_avoidance_by_lc = get_scene_module_manager_param("avoidance_by_lc.");
 
   // vehicle info
   const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();

From 4f0769fd3daa561b6bd3642f14a43cf3a6670290 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com>
Date: Thu, 13 Apr 2023 13:05:04 +0900
Subject: [PATCH 04/15] feat(default_ad_api_helpers): support goal modification
 for rviz (#3370)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../ad_api_adaptors/README.md                 | 13 ++++++-----
 .../launch/rviz_adaptors.launch.xml           |  3 ++-
 .../ad_api_adaptors/src/routing_adaptor.cpp   | 22 +++++++++++++++----
 .../ad_api_adaptors/src/routing_adaptor.hpp   |  6 +++--
 4 files changed, 31 insertions(+), 13 deletions(-)

diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md
index eea7ff124ef23..b0dd92df3a95b 100644
--- a/system/default_ad_api_helpers/ad_api_adaptors/README.md
+++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md
@@ -20,9 +20,10 @@ When a goal pose topic is received, reset the waypoints and call the API.
 When a waypoint pose topic is received, append it to the end of the waypoints to call the API.
 The clear API is called automatically before setting the route.
 
-| Interface    | Local Name       | Global Name                           | Description                 |
-| ------------ | ---------------- | ------------------------------------- | --------------------------- |
-| Subscription | ~/input/goal     | /planning/mission_planning/goal       | The goal pose of route.     |
-| Subscription | ~/input/waypoint | /planning/mission_planning/checkpoint | The waypoint pose of route. |
-| Client       | -                | /api/routing/clear_route              | The route clear API.        |
-| Client       | -                | /api/routing/set_route_points         | The route points set API.   |
+| Interface    | Local Name         | Global Name                           | Description                                        |
+| ------------ | ------------------ | ------------------------------------- | -------------------------------------------------- |
+| Subscription | ~/input/fixed_goal | /planning/mission_planning/goal       | The goal pose of route. Disable goal modification. |
+| Subscription | ~/input/rough_goal | /rviz/routing/rough_goal              | The goal pose of route. Enable goal modification.  |
+| Subscription | ~/input/waypoint   | /planning/mission_planning/checkpoint | The waypoint pose of route.                        |
+| Client       | -                  | /api/routing/clear_route              | The route clear API.                               |
+| Client       | -                  | /api/routing/set_route_points         | The route points set API.                          |
diff --git a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml
index 8e9773b07443f..02f873813520c 100644
--- a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml
+++ b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml
@@ -9,7 +9,8 @@
       <remap from="~/partial_map_load" to="/map/get_partial_pointcloud_map"/>
     </node>
     <node pkg="ad_api_adaptors" exec="routing_adaptor" name="routing_adaptor">
-      <remap from="~/input/goal" to="/planning/mission_planning/goal"/>
+      <remap from="~/input/fixed_goal" to="/planning/mission_planning/goal"/>
+      <remap from="~/input/rough_goal" to="/rviz/routing/rough_goal"/>
       <remap from="~/input/waypoint" to="/planning/mission_planning/checkpoint"/>
     </node>
   </group>
diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp
index c899b673e131e..4823955474d06 100644
--- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp
+++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp
@@ -21,10 +21,14 @@ namespace ad_api_adaptors
 
 RoutingAdaptor::RoutingAdaptor() : Node("routing_adaptor")
 {
-  sub_goal_ = create_subscription<PoseStamped>(
-    "~/input/goal", 5, std::bind(&RoutingAdaptor::on_goal, this, std::placeholders::_1));
+  using std::placeholders::_1;
+
+  sub_fixed_goal_ = create_subscription<PoseStamped>(
+    "~/input/fixed_goal", 3, std::bind(&RoutingAdaptor::on_fixed_goal, this, _1));
+  sub_rough_goal_ = create_subscription<PoseStamped>(
+    "~/input/rough_goal", 3, std::bind(&RoutingAdaptor::on_rough_goal, this, _1));
   sub_waypoint_ = create_subscription<PoseStamped>(
-    "~/input/waypoint", 5, std::bind(&RoutingAdaptor::on_waypoint, this, std::placeholders::_1));
+    "~/input/waypoint", 10, std::bind(&RoutingAdaptor::on_waypoint, this, _1));
 
   const auto adaptor = component_interface_utils::NodeAdaptor(this);
   adaptor.init_cli(cli_route_);
@@ -65,12 +69,22 @@ void RoutingAdaptor::on_timer()
   }
 }
 
-void RoutingAdaptor::on_goal(const PoseStamped::ConstSharedPtr pose)
+void RoutingAdaptor::on_fixed_goal(const PoseStamped::ConstSharedPtr pose)
+{
+  request_timing_control_ = 1;
+  route_->header = pose->header;
+  route_->goal = pose->pose;
+  route_->waypoints.clear();
+  route_->option.allow_goal_modification = false;
+}
+
+void RoutingAdaptor::on_rough_goal(const PoseStamped::ConstSharedPtr pose)
 {
   request_timing_control_ = 1;
   route_->header = pose->header;
   route_->goal = pose->pose;
   route_->waypoints.clear();
+  route_->option.allow_goal_modification = true;
 }
 
 void RoutingAdaptor::on_waypoint(const PoseStamped::ConstSharedPtr pose)
diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp
index 717a3ef762503..6c0bbf72e40a7 100644
--- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp
+++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp
@@ -39,7 +39,8 @@ class RoutingAdaptor : public rclcpp::Node
   component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
   component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
   component_interface_utils::Subscription<RouteState>::SharedPtr sub_state_;
-  rclcpp::Subscription<PoseStamped>::SharedPtr sub_goal_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_fixed_goal_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_rough_goal_;
   rclcpp::Subscription<PoseStamped>::SharedPtr sub_waypoint_;
   rclcpp::TimerBase::SharedPtr timer_;
 
@@ -49,7 +50,8 @@ class RoutingAdaptor : public rclcpp::Node
   RouteState::Message::_state_type state_;
 
   void on_timer();
-  void on_goal(const PoseStamped::ConstSharedPtr pose);
+  void on_fixed_goal(const PoseStamped::ConstSharedPtr pose);
+  void on_rough_goal(const PoseStamped::ConstSharedPtr pose);
   void on_waypoint(const PoseStamped::ConstSharedPtr pose);
 };
 

From 3c9aae4bf9f38c4d2dbf11778db2e066b5fa27a3 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Thu, 13 Apr 2023 14:07:16 +0900
Subject: [PATCH 05/15] chore(behavior_velocity_planner): divide build target
 for faster recompile (#2775)

* chore(behavior_velocity_planner): divide build target for faster recompile

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* cmake became extremely slow

Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>

* avoid usage of ament_auto

Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>

* build test fails

Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>

* fixed type. test compiles as well. no error on runtime

Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>

---------

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>
---
 .../behavior_velocity_planner/CMakeLists.txt  | 65 +++++++++++++------
 1 file changed, 45 insertions(+), 20 deletions(-)

diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt
index 870791444adf3..ba89fe87b4db1 100644
--- a/planning/behavior_velocity_planner/CMakeLists.txt
+++ b/planning/behavior_velocity_planner/CMakeLists.txt
@@ -11,6 +11,35 @@ find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED COMPONENTS common filters)
 find_package(OpenCV REQUIRED)
 
+# concatenate include_dirs
+set(${PROJECT_NAME}_INCLUDE_DIRS
+  ${${PROJECT_NAME}_FOUND_INCLUDE_DIRS}
+  ${BOOST_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${tf2_geometry_msgs_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+list(REMOVE_DUPLICATES ${PROJECT_NAME}_INCLUDE_DIRS)
+
+# concatenate link libraries
+set(${PROJECT_NAME}_LINK_LIBRARIES ${${PROJECT_NAME}_FOUND_LIBRARIES} ${PCL_LIBRARIES})
+list(REMOVE_DUPLICATES ${PROJECT_NAME}_LINK_LIBRARIES)
+
+# utilization library
+add_library(behavior_velocity_planner_utilization
+  SHARED
+  src/utilization/path_utilization.cpp
+  src/utilization/util.cpp
+  src/utilization/debug.cpp
+)
+target_include_directories(behavior_velocity_planner_utilization
+  SYSTEM PUBLIC
+  ${${PROJECT_NAME}_INCLUDE_DIRS}
+)
+target_link_libraries(behavior_velocity_planner_utilization ${${PROJECT_NAME}_LINK_LIBRARIES})
+
+# scene modules libraries
 set(scene_modules
   detection_area
   blind_spot
@@ -26,36 +55,30 @@ set(scene_modules
   out_of_lane
 )
 
+set(behavior_velocity_planner_scene_modules behavior_velocity_planner_utilization)
 foreach(scene_module IN LISTS scene_modules)
   file(GLOB_RECURSE scene_module_src "src/scene_module/${scene_module}/*")
-  list(APPEND scene_modules_src ${scene_module_src})
+  add_library(behavior_velocity_planner_${scene_module}
+    SHARED ${scene_module_src}
+  )
+  target_include_directories(behavior_velocity_planner_${scene_module}
+    SYSTEM PUBLIC
+    ${${PROJECT_NAME}_INCLUDE_DIRS}
+  )
+  target_link_libraries(behavior_velocity_planner_${scene_module} ${${PROJECT_NAME}_LINK_LIBRARIES} behavior_velocity_planner_utilization)
+  list(APPEND behavior_velocity_planner_scene_modules behavior_velocity_planner_${scene_module})
 endforeach()
 
-ament_auto_add_library(behavior_velocity_planner SHARED
+# node library
+add_library(behavior_velocity_planner SHARED
   src/node.cpp
   src/planner_manager.cpp
-  src/utilization/path_utilization.cpp
-  src/utilization/util.cpp
-  src/utilization/debug.cpp
-  ${scene_modules_src}
 )
-
 target_include_directories(behavior_velocity_planner
   SYSTEM PUBLIC
-    ${BOOST_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-    ${tf2_geometry_msgs_INCLUDE_DIRS}
-    ${EIGEN3_INCLUDE_DIR}
-)
-
-ament_target_dependencies(behavior_velocity_planner
-  Boost
-  Eigen3
-  PCL
-  message_filters
+  ${${PROJECT_NAME}_INCLUDE_DIRS}
 )
-
-target_link_libraries(behavior_velocity_planner ${PCL_LIBRARIES})
+target_link_libraries(behavior_velocity_planner ${${PROJECT_NAME}_LINK_LIBRARIES} ${behavior_velocity_planner_scene_modules})
 
 rclcpp_components_register_node(behavior_velocity_planner
   PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode"
@@ -72,6 +95,7 @@ if(BUILD_TESTING)
   target_link_libraries(utilization-test
     gtest_main
     behavior_velocity_planner
+    ${${PROJECT_NAME}_LINK_LIBRARIES} ${behavior_velocity_planner_scene_modules}
   )
 
   # Gtest for occlusion spot
@@ -86,6 +110,7 @@ if(BUILD_TESTING)
   target_link_libraries(occlusion_spot-test
     gtest_main
     behavior_velocity_planner
+    ${${PROJECT_NAME}_LINK_LIBRARIES} ${behavior_velocity_planner_scene_modules}
   )
 endif()
 

From dbbcc26576be265ec905c17017d06466520c07f3 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Thu, 13 Apr 2023 15:09:36 +0900
Subject: [PATCH 06/15] fix(behavior_path_planner): fix pull over deceleration
 when stopped (#3377)

* fix(behavior_path_planner): fix pull over deceleration when stopped

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add commnet

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* typo

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../pull_over/pull_over_module.cpp            | 23 ++++++++++++++-----
 1 file changed, 17 insertions(+), 6 deletions(-)

diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
index 0f51b5ea64a42..97e6069f7d1d5 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
@@ -839,7 +839,7 @@ PathWithLaneId PullOverModule::generateStopPath()
   // if stop pose is closer than min_stop_distance, stop as soon as possible
   const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose);
   const auto min_stop_distance = calcFeasibleDecelDistance(0.0);
-  if (ego_to_stop_distance < min_stop_distance) {
+  if (min_stop_distance && ego_to_stop_distance < *min_stop_distance) {
     return generateFeasibleStopPath();
   }
 
@@ -1093,9 +1093,15 @@ boost::optional<double> PullOverModule::calcFeasibleDecelDistance(
     return 0.0;
   }
 
-  const auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints(
+  auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints(
     v_now, target_velocity, a_now, -a_lim, j_lim, -1.0 * j_lim);
 
+  if (!min_stop_distance) {
+    return {};
+  }
+
+  min_stop_distance = std::max(*min_stop_distance, 0.0);
+
   return min_stop_distance;
 }
 
@@ -1118,17 +1124,22 @@ void PullOverModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLan
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
 
   for (auto & point : path.points) {
-    const auto distance_to_stop = std::max(
+    const double distance_to_stop = std::max(
       0.0, calcSignedArcLength(path.points, point.point.pose.position, stop_pose.position));
     const float decel_vel =
       std::min(point.point.longitudinal_velocity_mps, static_cast<float>(distance_to_stop / time));
     const double distance_from_ego = calcSignedArcLengthFromEgo(path, stop_pose);
     const auto min_decel_distance = calcFeasibleDecelDistance(decel_vel);
-    if (!min_decel_distance) {
+
+    // when current velocity already lower than decel_vel, min_decel_distance will be 0.0,
+    // and do not need to decelerate.
+    // skip next process to avoid inserting decel point at the same current position.
+    constexpr double eps_distance = 0.1;
+    if (!min_decel_distance || *min_decel_distance < eps_distance) {
       continue;
     }
 
-    if (0.0 < *min_decel_distance && *min_decel_distance < distance_from_ego) {
+    if (*min_decel_distance < distance_from_ego) {
       point.point.longitudinal_velocity_mps = decel_vel;
     } else {
       insertDecelPoint(current_pose.position, *min_decel_distance, decel_vel, path.points);
@@ -1138,7 +1149,7 @@ void PullOverModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLan
   const double stop_point_length = calcSignedArcLength(path.points, 0, stop_pose.position);
   const auto min_stop_distance = calcFeasibleDecelDistance(0.0);
 
-  if (*min_stop_distance && min_stop_distance < stop_point_length) {
+  if (min_stop_distance && *min_stop_distance < stop_point_length) {
     const auto stop_point = util::insertStopPoint(stop_point_length, path);
   }
 }

From 4095f28585447e9e20eef0fa9ff979cecbb0ea79 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com>
Date: Thu, 13 Apr 2023 15:12:01 +0900
Subject: [PATCH 07/15] feat(tier4_adapi_rviz_plugin): add adapi rviz plugin
 (#3380)

* feat(tier4_adapi_rviz_plugin): add adapi rviz plugin

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: fix copyright and name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

---------

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 common/tier4_adapi_rviz_plugin/CMakeLists.txt |  23 ++++++++++++
 .../icons/classes/RouteTool.png               | Bin 0 -> 18815 bytes
 common/tier4_adapi_rviz_plugin/package.xml    |  30 ++++++++++++++++
 .../plugins/plugin_description.xml            |   7 ++++
 .../src/route_tool.cpp                        |  34 ++++++++++++++++++
 .../src/route_tool.hpp                        |  34 ++++++++++++++++++
 6 files changed, 128 insertions(+)
 create mode 100644 common/tier4_adapi_rviz_plugin/CMakeLists.txt
 create mode 100644 common/tier4_adapi_rviz_plugin/icons/classes/RouteTool.png
 create mode 100644 common/tier4_adapi_rviz_plugin/package.xml
 create mode 100644 common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml
 create mode 100644 common/tier4_adapi_rviz_plugin/src/route_tool.cpp
 create mode 100644 common/tier4_adapi_rviz_plugin/src/route_tool.hpp

diff --git a/common/tier4_adapi_rviz_plugin/CMakeLists.txt b/common/tier4_adapi_rviz_plugin/CMakeLists.txt
new file mode 100644
index 0000000000000..7b3f909115dff
--- /dev/null
+++ b/common/tier4_adapi_rviz_plugin/CMakeLists.txt
@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_adapi_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+add_definitions(-DQT_NO_KEYWORDS)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/route_tool.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+)
+
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package(INSTALL_TO_SHARE icons)
diff --git a/common/tier4_adapi_rviz_plugin/icons/classes/RouteTool.png b/common/tier4_adapi_rviz_plugin/icons/classes/RouteTool.png
new file mode 100644
index 0000000000000000000000000000000000000000..6a67573717ae18b6008e00077defb27256415663
GIT binary patch
literal 18815
zcmc#)<y(|rv>ig}7!VL~0BI3P>4u>jX{5WQyQGE==@gXiZcsv`yJ3)ShVHnpzyIKV
zx%UIl!@PCQ*=Oy&*IwtGaAid)9L!gkAP@*gMjEOL0wKly`$K;YTnXsp8Ulf2BxRtY
zY95&f%V-`irs2ZJD-E_>W|QJ#$f(lDGUzn9FhfdJ6ls+f7g13tl1h)8=nGVpD&-eU
zEXJbkUMvxiN-bt%sdy?0Ws#;2U}Tww$D9k?tJrv3b90N4w6q7mY>Vu>j)cq`Gq>&g
ztxVotd7x=_%0QrfCTU6#h!Tqc9R&LG1r-?ts`)>?Y`h5pRS_lWag8OXawfXJHk9@r
zwSCxzbPRm-g9n1hq$!Q)8a9)#!i(M_cz&h|6vnpsyKVD7twGSL0-4KL6Q=BD1ZeP-
zzfzYzWVM-q+?d)>KwJl;5;rOea<iu~C{e?<&L3i=M;@#9Qds={La>VP-p8?mb$R~c
z!r4STVAB1bJ?W`Q1N^wk6a>;QVI%JxOu}Vf{^G|)Co3MGH{(9RMV<;nHU4Za)%KSC
z`;I=`QN*K~)?CjlT%q<P24yM)xECXN=&hBtyV<rLaBLhd`^T9W=avk?Bcdlw`8-U8
z?WY`6c0}G=Qbh>3CJ0<hR^~JNLCcs63>d$G%J_7^;$JI4Ms3f{6oyJDbjK_N!4ju2
z&$?3P$rm$WTureE-aq`r%H%qf&xx}1+^E65XE7*U*@ASHv#;8ex#{Xeuo?I&+x*og
zCp;~E10-qE582|U!y4K~`SF#B31Gjpj3?LDSx6hf<#=NRr5x>o;P$^l9osYrEfL<G
zF7}re{Z$$90wO_7Y@Y4<Z2W77Z74{a-<}y$Hf$VRr?zF=grjr6wNy!8(W@Wz^%-ph
zZkzqPJ(4Fn`us7>{WYjQydU9;g?K5#3k=W*FphTCuCHewl5&`Uu9F)(f>MMOxB>pE
z88huBN|GrNq|eH~M(ELpl-tdZkMhs%;OV+^2?6{7^3=m>h4(Sn1AF4SLTeDL;O9g!
zBWaM6c%X0DechlP8B_BUQ^1_B5ImG0q$&AGba@Q1!Sx#uv;b)FNlKJ%FF!gu4goAi
z+jugMiX+lt3%Db%$x}1fa)yh%A&xxGW-<QF8^(|V^xKJ7SKpHx=(o^$#(pAoYnij!
z*O+(5_#nR7Kd&;}DsIP;6@bz1#)-Hv7Tex>Lle9BaRZcr(Jl8YJvmXVoM8b*d?nDn
zr>xqjT|4gjOmgjyb!-I_L=1WSO(};jF5)F9(KK9k$6juv0YKyrQRHz_DK~8jI2)+w
zm-Yt$QfvYLhOutzxp@d+SUG4qO=vJ%C2^D=7atD;N<S3i@$pk@bt70s3SJE=qHXy6
z6J_pELzop)jy?|<6qNJ5UN=83h%?PD=2Iz0c?$q_p;ZXh6wZ76te*@-=p^8`yzcWH
z5$OPG_CitfP%!d8hHB>#3;^c;DQzmR&&!oPfkcBJT}x-4FY#kT02m@DxoqS-{xT|$
z3PAj4Y06h5S$bR{FjZhmf2ImPCOB3Cm{OLqda0jhqvfLSxmZ+?A2<^h<jw?o{SSWI
zC3to>+z64sYjMk*OaM^512bTNz1yJX#3oL;p>F~$69vB)J^K&J0kyCuHxo2qc9?N7
zpx^)Vh!`XhEMW3-8hQ#=jbwx3L_Yk3cspR5f1h~;US}c=&En=n$AFqJRa-Trw&}AF
z#AgA`>wx2@JWAg4l9l5}1GY|0Z9CNsXNdgw(z**Vj@S4BOp0I}AA5Z|Zl~>Z3kVbj
zqQ+>~?`j3P7S~|HAAK#cX8I@4)zn@Yx%DiDRJ@~c4bTC4y<!=z(5*4@DiAdf40ya0
zJ~-JPgwFiF`R*|G6c_msOR4phVYotMl!LH64w&{V=9vhZ&h?Udj+f%mN*{mR=P+W}
zbDqfO%QDbm!0_Kl=F&cr#fk860DeHF>SPXJL&So)v8RA@6#{J%M_y$DCRzdd0*FPR
zEH--gPG5+|{w5Yegr~;hMSkzb)vYj91mGV5xcRrf*}fIu`8(Gk(oJHQTMWSTlpe3W
z7U|f@97Ozui1Pqn>xtK!34&Gp1$<#-lg>4RGs7K!6%!+v&|>f%Vhw^400u&sGY~q5
zJ^R2hK)+x%+?N_ujGMp<Dp0eO;o6>pd5=D@{V_pS_0HC2*7x*eApoKv>S`$MhIW>f
zMXBEn^5P16FfqDGOxdOTX$;a&7%;G-28$P)$1&8Il!2^?K;(-9A|F432ZYVYM^|}I
z3>OV1q`?mb9wc7}dJzwJ)_0FSe8o7Cm(><8hzS0<IS6|s9(eO5g0kxw0XlVc2ujs8
z83+Y{wOo0)wuhDp2qQQ!c;c<QP98xh0CGxsn%*RA-C=&%=K!NN##)OPoUtQ^EyT00
z0wA@54UQRGH+TN<8`7D6Rj>{vBcWu7VZ9!xVd$<bT9YQ9>!*%0qZqPEJb=%hJ$Z_L
zz~Miu9bQOq9)39Df;+w>C4k8SnA^IR``!NH4|SUf61t|;@Y}Cs7L7w2kT8`ufki#>
z(|*WcvwwZbRQK{xx%^U^9it@ui(i`PZ(a!{U!?$lf%er!QwQi?Jq@BTnXsQ0^KiZy
z)@H)?s!l`=|Lg4jvo;>IF<edA$bHW~L#Q+*CNKfLgetmlN6^nF_m+XIrQE?|x3+cG
zS2Xw`ymFz-L;TM{*QL%lEdE?GX)gx(aj8Wq+P~QL&!4b)3L1!B5d~bTsd*<*_xo-P
z#p~sq#q=_aA*;aV!46J<Tg1TfJo}JABq_>s7CP;9l8V4UpAo>sjIq#`%v=kuRv~Dh
zsiW)pz26t!@MhZgf>?_yj35jsQu16~;-8_l{<d{cK+A(yL5-u&B<}2SOgDO*$cwn8
zhs({meH0PlG6g;p8>?rj9%$d(V-9L6Mv^t{|GGZ5-C`W&WqNh29e6d|hsvmvzE$*g
zQmyMXe|TD_9?2}u%kdXf_x#(4xd=B<yzN|epSr&o>4sAmzB{R*=+ZDMv*{0=ZZw`U
z1zHnRLzGA%_kVvk=&UnLH`<f^UU2sEeMA1V^8mTb)n&mly!q*>*nK6OLz)IZ+1RaP
z$_a6)pzGO}t0_(S2Ee<UC6HQlG`5`=%CqB_byMvjZpFQ;%3F|cv-{s;JLVCdj@@og
zx0QZ1*W=|FMl|ED8%dCT1O*T;+r^OM`?FQ1gaF^mspipanOyZVmSk~mB~B|DUGy|x
z=04)jewaGwd;JETa3+UKUa)R6S_)`d%3~q1KRLD6v^fDo5K}KBIkvu9njC5UHf2t~
z<vRr}Kg5rC8|Clt__rS{h?nP>N@?u0pWGf^QOT;Fy&fIf+061Vv&k6#eQ9E?CJbN7
zR~KK=eS#kf?q9C)BZqF)@T7l`iF(-E2|R4|?91b}4Ez)Kk2FyYsGq$iY-#*_u@~1}
zergZr={<!9bq_-_X0J{nkOGJtboN_4`o2kdv?Trk#wAc<wJT?xz23rnlXRtc^s0D^
zw+9y9kl|f#v*vGcM!wP?VJ%=AV5FY#zScPdSIW)*X?3>EWSme9lc1Lvw@G~X6iq*U
zJ>6TJJ1l)Wu8Wk+%I_jO@X>ttg#2Gtvr6H&5?wENhU3wfOD|1HK$EE0JWP<pDqDW)
zz~rFbevqu6p7Aw4{k4fNBDb_$K{az{1i3qXu2ub<Y+qmJH$SeRC^Gop3@}mAZrj%n
zAm*5sIc$82^~z7h%^G!g3q<YM*s-wRatp%wT|K;YMMXt(QuMuwxe*@c$QaNcK$^&y
z{#5cXqLOm+-rUarb$1JqmJX-96TtFBtUlhRTnBIZ4~d6jdkP5)3rD!idl_y*0?6k$
zA~4HGT&7zpD)!-B>Io?o%@w7QX>1z=lRIcDYmm^RdaH!J$U0qdKjR{v?Nj@k8=mLg
z!QaffX+{G^hFLx>yzv{!<edz~CS}ht(P{}u+i|VxDM<U;y3!F1OD}be31U%tQ!24o
z9*c$Zv{-tC*9htI!(``_jVgwp_h_{eydT-K#v_XI>{4I7+*?C}Mw;W;Akw&+^oGNB
zKbo1D;RxOCe8*HY+Wq2Z{a&$bq>kL0E*5uqV%bZU97(Rgg}axR>R3#V{3&SGo$cMU
zo>il+6}(CD_7>Sn+RG>LD*ZH>q}A^P_|jLN;2-ZSZ)e^-CR7g&UCtF)C)#ml2sjLp
zna0G_?3W<_14PCTp7u%y`1jJLdPCb;WW==}ZA1fH$8?tCsR>bJF<V)9>?vJ$I|J{?
zl<X%dM;ja%Xt7=Cc$hz2Y<wCKV)y+?!u*d7-01=oYl_i-yh*WRYC}tp?&K7Aenu`M
z-^3mU?QSJ1ny_1{_2q(F$WAr7t`EdWM1Rra-ap@+bg+1ta6}n6)kTm&Op&I2?{BU5
zeMUi5uiLwE!mhKv*SH{w`-)vcXGofgDyh+W$2uvaU;st@tC!>TYZ~d#P!IC7nc5MI
zRsbBkP4l%D7PaBB{&6DKt4yR^NwBqPM*lr^I~U=AH?+LJ*&C$ZAXr^~8P$6rvc|OQ
z@5*T^JL<hlL#M5i96romye}wXP#JXqJ8unXXQZVS&-3aCyb1gi{GOLzh^qMHL;XN!
z8)f(M`h=}`lySPiw;of@jN^0NF!*8m^{Xi*<@yY(lMQ=~$o>t$TP5T0t(=wiG^sXg
zEd~AVuZ<W(f9Oh6a;8E~=m0`%F=g%B!%O7@OzEue{(2i=w83Eg@?*i0?#=RAHGf0&
zI`q3byD#I8ziu?;-*L+=k2?d%Yuw!4CR@u@l4&}z`TW6JxC_X5-!yJGp4VUe$mhek
z=yLAa^vvF{<j?l80h`n|Eo{}0er9!*=8p_V&yjd9sh5plv02m_@h)mJ8%!1~6_ai$
z7zB%0SN9aq{%)kW!lQ~%|9%h5n>Vrio-s!T&LrqYTve>D*P81_=>7;qF3oJb%9-_$
zl7tF5{GK`$Y)Po@y_Df6U|SUSg7!xbv_*4yVe!Q#Ebc;ZcwoSK)rHUB2ho1zr!2(h
z%dVk7oadz}SQ849EKz@)p|3b8py_+bZq+KNZ|#0KdDLC_ZhWXy#9XXyyOh3<qkKqE
zpJFrCYstA~A&AB)Fp2xJ=ty%$U@B!*SAp)MT!`3&pa2^tz=BeW(ga==_IO353n1HS
zV3jvDFGZy^`EwPt#ob{3)`(1#baMy{>&D_KBd$d5r~dZ&@?gHCJhG5mniAQ@R>y{O
zFvmlJ?JS~a-%K~9k;T?<cd-H^*@r|6NKC2O@0$faMD5QL<a9p^b+fOSyGZ4*7*Ur^
z%6FHheDb{l6MbFTr>h@u&=-btl<6W(Mr?#p_#Nvx_0v`ilrR<$^+%&Dej}o4vNPn*
z;owRb6rB0y*XFvkd6OhCQqGB6I?r)a!X@j6i&CJJ1K0a?>M~fR$7l@HLqkBtR{DNi
z#>_e~=e6CHkiV3$L3^TB!NYiWD6Xr+?LjRJ218fc9?44c9VrfHEASeRf4u5=;4Pz2
zbmAarlXGC+<}+MNXuEB2%{j_j84W=YGt*J6aB$pdMP%f^yA?e@n6EWm{~fLLbbmH#
zB#6dX=5Pv<(zsg%*jkAKL5r0knZM%tivqc1CatP(x82cXPdwWwcmjs-W@q|ImYY99
z<xHCr(Or!*RC&738!k@t&3L)E5-N75*~@f;y7{@UTx}Mi(Ro<O?{yngqrY6x91u#o
zjBL6WtGfY7mwdJmW;JMnfVXEaI&?nxvtW&83)*---fXAE{!ZiCNT~7+&sdEr+54DY
z8a{5RFtOf!tM^AivtGUY;LDdUDzz4)%<6}zeE2gMG{uW$t<wwp-Nx*mnoFPTi5vP&
zw;pfKo>YwlE~Y-hze~lk`x3mjPB!27mzk`xg^y;8^_L_ahVv6~)|t~mfqeU``w(TZ
zxrTjOPssfK`oySR$6>Am1>m)tK+)!_P=0!6FfqlwTf!xubMJn*u$lh(uwJ8c<*Bgn
zDIx;XIk9`S^Zs-&ZN{2BW%h21L^x#Hy{t&3#$u%2EIrNNs9uMG<IV3{`!n+4o*prK
zoy_fUZOtM$W>jS4;G4WkVIc79S?9<_)}5sGh^o2GGu01pfp6?z7wr_`sf6M}tS@hT
zNCXk5MDOIEc)b#>BL5Byp4r;f?uV$qGqs%c8FpR1U>>e-v`u`iQ65*US#Q)pH<!ul
ztWoc<A{0J-Q%A!@C@r>|_z0GX?kiG%%(LOU!%Ckq@v`f@JK|pfX2{xiK)~<5soB5N
zZ^n6hgiX)Z0q$xxBO+$zshAz%JT;G?R^ui!yaQF=CYolEefA_D418x5EBtuv6S_T#
zt>8C5ZK^P2{Tk3jP{74C9i5Z*XB90Em|L^1Lp$Ty?ZVFx%zQ38_!1Hlo_wx*Jxm>5
z;cSTnM62^vn<U--v(FnxD_RN+s_9xI(oN~oh3_5%_J4+Be)q?H#KF^p7}Zuka8USN
zuWA%3<vecfjDK%U%TBGWvI(5~m|-9b9k)&rE|J38Qd+EhSf@C;g8Lh&`6Ci&<>W0!
zv(<%*u%dQ#ZEdDm`^UPysNb89-plAwdG@K0)y_jBD-qUQUN%)_5(TsN78%{K<br#;
zH6GQwOU=$D<!j~YMkyk%wGaD>)f*=5FXVTI-5jC-M0r)_e2{Uu>Io;LFZ)WTSayCh
zH$y%7yq?2_WZ>t-lJJ(ErM5;8)CMsVjU&indTZFQ!V%ji9)yN-+O%Xp_zMN?x2c&~
z$h26bQIZIOY$0|Hl-A{A>HW>D%Vl99Us^*}%6q+6(o_lL&Dq|Mo_&23{zG+CyVGoa
zZdPuB-3?-rE;hgTQB^{1j%hnpE81vUZxe{q<Pn{-2L8(zC-gb@Cms0<n>7+r62)9R
zQjuMBfm(#Ms?@4Z$;Ik^iw9oWqRW`6isW|}$3~+7uuRp_uSRo5+uoV%o;%fYOqe&(
z|2-;yrIE5RJ-g8wo6hVoIj1wlF#nl)eXp0d4XYB-b^eG|Cv?<)1s7iS_r5%WWTvIf
z2b#SW2vI-|e@Dud15=%5ScGn5xI-4JpMXeprsukuG_&3pJ9vw|;{D{iSX$ZsyN-QS
zI<i)gVs1t`NfV$k4nQ7l0eCfWWEy#Eb?c~A<EuUACY!l@{TVFHk+zGO2q2u?zqL}=
z)YKiOo3uG`bd1*e-6f|*69<qHZ-9)HwBKp5HcTznSP9lgbQaOdC3^@1F)nUE%j4<6
zuQB8BJOINt@=!9JjJ-{+L&@Q;ZR3Mt_)FDfi`0rTp<eAIlvW~EJ%^*{i+w>kazCCe
z<EP8PK$HtrRn;RSQaqMWB}$-_DgsKW?de-xT`u`k09-?LyFNf>5tjTIqBD|W;A`f4
z(o2%2vBi}U($ZixbedSg2KMZ`WSTOQSc*AmMXpEHQN>d_Ts3UpJGs9*!n#G=T_Pyr
zRXRI_^rJ$49%InZRr>IOdrQ?H&qWVYSv0t<)_MjA=@l6!>}m{uAv-nCbu4CkNL1I0
zszx2HgbeMhEPPFWT{;-iq%i3PmyXoFdjD&`g5k23{NZw;w9<>q|Iu^oZgl-bE6dSw
z^mlKsUR{#$i%`XX%3&%n7Tqbvr-=iv3Vn!kYG;%8(aknQHmT914|hK(l9+AT<Iif<
z_=T^WOS`P?-+uQ6n4EfA)PUH7m*QX#yxir1v}cZ~Lgqm(mHybZvXgnQ<#4Y0I}bDS
zk@W@jT3;1L&jE~^H}ZM8YU#ylRGnvvB8!FJ!y_Wi;)TDJ<EY>j)ON8U+A7k}qtQoh
z(o1n!2r2de-qqvX8LPa^T2)TVwotK{{5PUlaF}h}t$?CyJIDXX_j2AsLij`a@Rsi6
z<mC6M=m-km6(rF)f?7U6oG7t-F<onC;xDQ5dUBi_KtSzXcC#~)|C&?ta`K}2i0LiY
z@+yq=VWr&Hr=PF<Cttm7(SG3-$inuAr*WsT-iW+I>T9`WmSJ<RCqQgX7x1k4G3xvD
z_%P3$VpHR#a$+`zN&qP0pUP_#pdA?Pk*OW^&aKNIJjN&|e+8kb-weTh_RIhDMv`#&
zk|s>@(5~J3(YDA@J|o3An#c=nA#EWWa`laHX*T$SjobapA&u?(UWP+Q{C%@7Q}QA^
zJNv{`cm#_$-9Pc`0QTcIA;7WaQ;?NPKQ)wA2wg2XAF*`a%1+DL{Ei|8qxs#0>_+qE
zIygdQ6ZJGXrnY0=Ug)~OeO>jx(RvJib8E2<`5+hRj<=UQ`XSK&;bJyBjKUK-vv>=H
zGCKa(@!Sj)ME<{<u$T=2EzRWAt6beCfAzTx8+?A&#h7nvY13&uPa-n4&g00SDxlZu
zvCqD7Vx7Y)ZHP~M12K{LL9=g0TVB||9_L8K&_P33p9Jy!fVkGZcX*R-hF^$TtBUdm
zV!Quqr(RBGVq)$m=z?Jr>YC-0A1b(Bqx63NF8LaQ+pwr7++?Y;83W-@6>t<h=^@<8
zxjZyk$RDpR$}QKFNoHoOa%|Umb(D>v_S?kk#VM|MVs2LY-DySvyPfa--K5g8|I^)y
zxR)i5=c%cxpXe{y&Af@{;>*~{b02i&{aB_$54p;>&#@87qjme%iw$;g@1y3hdBV!k
zZH`JGW7<V*8R%r_C*$#3)%7%t@gwrXj8|*RQVJ-8=^5(>Tng^Ak~%}#;Q-2Kv@J$5
zPoiIQ%EmkU-JkTeJJ;0SF{mVKykranq(ebDsHb&e)<5FL`@L~x6=ryQdf$-7vCQ0<
zVKw1(9v>zMv~Hr4MzS?k%k=W&z-eNFmOLHeG{ZZM>d@B7MqQoA_xwa^NGPaDE9_=G
zqs6AdhWYsONzd@+OaTr-8XOBq(wvBuPJ?Raqs7jrj-Q(vB?55luOh~nXxNd`{VH~I
zM;8yWZJr7r(}dDk^xNhxwsalXR!sA1N(T6xx8%3<WKC{~GE5G+6dJ^yyA7Q*4q7y>
z9i?;He4?%(U&RqXs^zm7y_}z_XZ`rKJWdqi+!lDPbqjFCc03mCyq$|H4*?12d18Xu
zsc9-IL6-9k#5yU9z|37?M|bFc*A_tKQa1XE*M2$dVt-b%RtQm5BcL!+`EtBIz{=SB
z>EVhR7s6-iy&6K2n(c0w;sbP}fDO(N@z^>W*1LXDq9TP$lSj^&IGFi{KpGIP-++mV
zR2tI$TwO=$)6FcJ+Rs%5-rd7wzS5;BWefCwh}pkOiJM7&g&1R6o^Ft!tBsfjK$~LV
zchl!;(BZ>dy(Cy;<ivD^&{JqUchFm|6D3%bX}!DhP7%6HO<xf|yquLagrPdQ_3`Tp
z7UFdlO3s_TC%b2pm5#0nO5=7QzpBXmNqAgiCYRQLzBlWvV;;LxlqNd_-O%;%)#MnQ
z*3a>~wVJ*0yFzq6!EcXR5$rY?4gI54mUi2JbS2$QJGSQ5Xyxt;M>AF4Sv@28`bN@}
zlqwK4bFtGi`_*QHwN9+3#N)ub$|i36r9T2zdP63$?>Q-i9luzG2$gB(w!3~+Q&2P3
ztg~7hwvPLAu**HR*1BKO`siW+cbQb!0_qL(aGzg2%;p*MKf-C??dsxN<>Sk(?%vrz
zzc_PV+EeGDFeZ;i84*ZtW=H3=#2j|W^|ND<(N6Ys^vYL`H`p!um`KLX43QJ|#XvZ&
z5VNDI>gv4*nZ&heTY8$rMpSG1;mfQ=IFry0%W*2X;r!dHgLxC(0(<v?oDVh$#*mq2
z-YbORkKTh0XZg32cPw!~(Qsc|mJ}5!+tkhS2R@IHHRG@Vz;e7uFSqSE`p}>fUSoX7
zwNjEXD0t!s_`m5$=5BFW$;8yBAcYLBacVr_p>vrk2KwkD((ynWWJf^m6M0>Rs^j&K
zcuG{Bb;k<W8mINp8_fT9U~{w!xh8MT%_#Q#PE&BT(iMXBpxbD#{>~FsXL?edF#}7S
zHZDLmq@^Y_Y4fI*ecn|MwalQi<F>pJKC!`&+AEi=Oxumg^tYvS0s6v}&dED>#4Eap
zAJNq9yf;-G>9>swxt+fKLmkVyKVy>_Z%8^@9<W&dPrh1gban<>4F5{sE|(%yu=RF}
zU|f+?>1X;0!Jp9#;p0r9dszlE)NBFz%Xb>g4B!|F;iIe%Rte_9u|W9MNnz3b-6obM
zRP{c^sD1ZD1@8DThqDat@NK>7r_aG&(BpD|3!JTXho#kHn#b%IMn={-%208He(I{j
z8s5K{sLB<8*d9?YNvdlAkE1Xq4M&^5V`}zED!eajyno=M;-hVhny2RT;+R}Z9r4hs
zHg0|z%N0-eaMaHz%=Q`>bdt9ENtBg7zrZD9*0FRwt#|ss^{%z@XNh|0y-*Iz&V$F+
z5b1tlj^6+i1CSkW24b4a6t9GRb-61JetI<D1ODyJWjpt6i<g8T{wR64GIEtbI^ga)
z`y;TlDL}RDuFEasinFi&$7SyC2_b!3tYCxMSxzqy<_}6WE6s)Y_Nt2oQ-z}c1y}I0
z4lDn5S_J0GLcNVd76PaZ&2IIL<vnx5H(!|*D~37z`oS#U>WG3TakcE8lU{iJ1*WfY
zhT3M?F_AEMwx8^<aSxv=T}ahtArdsY_gJYm>2o>J9?B73d2M-Ao0yblIV=w|pGk7+
zChG0Dy~rHOK1Z}(>0iZDOF^?7HV2aYQZkl<Z1umY4gSUD9a8jA@RT!9HU2O;^901k
zC4=JD*%2FPkB>SUv5SgTzl!7E#o4p(rOC24S<TdZO<t^UI_KF6y$;v?8LO84Ju&ag
zgX-ku<d$XZchv#(_RJQiG)pxqHFq=PkTgLghq<a>cRx|^VD=~3JdRynp4w?)W(5A0
z)_C3;5w{#ut;AeK9af+D3$6?#Btje>?{BQL*L-vD2I8<YM399(IWI={k6#8E`!!H<
zj5{x@+oi?^cpd*jYaZoE*{dBDs%;0zpFD^3{yp{}OmSEpAw;GLPq1>Ik7Vg!@QnLe
zppa1pgsr!=`wq1_mTHZ}C3xl@K9Vf_gQQRAD_36GdaZ87H=x1+REu&}7i5gjPv`MP
z?=NN3^VvFySA4Is?wUDmnv~`eOyF%d=CNUipe$ZOJ}FZgXz3+QW*Yt>xpMko-JL05
zks6_=q<8Zb!SRc%GSdhvd#^TbuH?h6Nef0sA3Wjvs`Q+zGw6AI#HO>wHLBBX;iyJI
zFZ5A$b~Ji_KEX=v=l0q7@oKkklDfM5JEnZw<(BE1>)c*>-8TRsJgfKBDNYu#C!HGU
zbLvbP6<#?=;4iU0T&Pb*LBnMo{A)%V&E)8oVNDoVfNMGIGqDC~Tscy8s*HW5h(C^+
zG%$nRO&>=ccGarn_c)}SF|TH0#$7Cx8+pN%20+veAhSU?RF1MOcSo&5t!{_L=^j!S
zwpKQodunYpRvLi#(<FsdJK7C6<3>OCA4LtJoKXSCHK;j8k9qyr5?e~KQ|t>1+SXe)
z>)&;E*Mlsb=gfL7YJkKP1nqI#dXEOw6i8@(OKrTvK3jR+J9~Xn-iY+EwA~bLWlsRR
z23NE2O*dU)=1+e<1VW`GQj_6h3MbAnyDHL5QY*f|gl9W++|d2#xN2PS<3Ii9G|AA=
zOQyQ5g5I}YLVzr($N9aHm+H&mo*)(pv+|j@cP5Nnz1nAZ%$JsKX2hLXNhUIS$$QnG
z1AL<~%s^invTXLbb=gI`FD6^i`_j_7{ay3FG_nui=Feq_SET$RM11j`*GRQ*M5-@2
zd2E<{+liz^%K)8H_K-`#W19`%J018J8$M5t{XUL%67!Kv?T5Y6%HoSPfj9xo@SRNA
zO&i%cnz)EbdPRxH)73v%VP9f)rk1ApgoAeZVYB+))9p8Bqd4&bBR~*`oNY>09O~N2
z!Rp>7KnHX=U8>;-697~ZJ_sN<+Irs|Hl_`;G1z694OixT`*V|Cz_YBGLc(tM@Kohm
z>JD&MYd3Xf+B|B;<jS{3Ujf&7`g~|^v03?qe!oJBF%IXAYOO$4%D#eo=P*m@Jv*C_
zW0CMV?WdOyGv$5F<>lk$xwEPD9_~%=V(vx(C@a6Y-H$#=bfVOn57$&>rkwrVs4#bi
zIXh7e(IM6w&y9tw+~I(;y6HV5=yM_(oXxC~r9=|;e>##2d}?n{dgmZtm$kdn2_X2$
zZn1&1<iZi&=6ROdD8|=lWgA(SDlYrUcai&ovnc210f#3*l6+(<Jue@wR&-aq&J@fs
z+5r8!yv!^pj&V&ih4YNWeAZ;)_Gzz_MbBooLfv_!Zyo9)-+RhQe?TCe*L?alFI1)v
zfNa0ZW#j1foyiozpmWVRz)=<+Z$@2*H0>c*eQwvMIIRLX^A-fxxJ(8WBPe?||LSuy
z{t{iR`A!>-41uqe*4_8|FWHQLq|ffyeq*aJzDYNo!z||t!@KwR;k$ICGOlv)`wp!Q
zFe7=&V)-C1g>f{5f5E>X@5^pc*;|x_)H3t0Wj(Hdf}t6f7dj;Lm3y|b61$Fgis3HI
zZZTE=iadp@o=Y|={({-`cL3Mp!SitW{cQ0t++xA?d#mq<7jHsy-_<SnoR3R+dtDw_
zS&e-QtPz*Bc0ApXaR3BF1tKD%P{q0F>FH#^te&BG6e9=TM4K?yE4$gLtJB{QzeetB
zryUa6*x1vKi=Ur?^*FjI-@8jse%C$IlbLGM)=y!06b)*c>gqrMkCt)IoHxxwf;h>K
zI$ZxUI0Qd&(@HM3+nD1SE<}@A-dyZ1FM9v|iBvCi@E-ah@k7-!4x7-5u`g+?MqO*l
zm)P-Zzu$iR6iAlY+w9>D6kT>tx3it8Zf2XNw7OwQn=-dqz_;&l(OUI~)JrvPrtJfn
zwVGJjkAIYJ@iHeSyf)eOw~G0ZJld2UGPIIZu3h{4455%Qbz6lZW74FexmYeYM;8aI
z$K-h3Y^GFFCuw>-Y<yJ`U}?L~6CZZA90qi<ht7vfvdO|~plXpH=ZY*Em;_W(DGvP)
zjsVG%&u&R!F&H^V?2VNK?nLm?(oG0<D1j%h0cyUHC1jVLUWh(eQF_+J6^M!r;X5ay
zx>X)_9?Ig^9A4wx1xQ2#eYVG!-LK}|bv$R8@eqlJgC_fD*}q9KhIE1f<?X>_i#O7_
zT)%x`)T#K2OJP6!v6Vb?t~cc+MQ<JL<Cr+!xI_H`M`ggz6=v&djqjNIs05$ShPhWt
zHRhl0xB2QD`=`pJD14_}?t3r_%HvAC8|xYXk<&#C+D6^Q;S5D9pgy+$S2+)*amaU|
zh2cVKn04x4+U~MJw<j|;t;AE!TOYOkLR&4uYzu5U4_6vT&!#>q6?;as!glC6mA<(e
z<%lFRX`h0@<?1a<EB;T*+-?V!TMz9zj@QRVgArzfso9gpT$95ni_Z_KNXOo6$F_H9
z_ttq|*^jO(3H)-&I&D2_8T7ipb~Nw5yR6QScOC8n7WE)uMy=FsfLN(f+FLvPhfE@z
z=n{kGrzty*@aMlj>1C67z0NHWh|ZjuVZ|V8t@FP@kIUYdtPp)2I9s3B_BXeBp}-ZP
z5|l~AJ-|-~e7adHXKm6rZHQ~nBqb!qRwUhuE_?<ZcDAfmYLF%?O8F6OleN3Gqf^!=
zO)yKCtl2&$0qk!v!0$d4*F0WS`g`gFRezNt`T2?&e;H80b+r+>tsh^Kl=Hlz{jJv5
z%a`iA?S8PdtMf0~>M^6t2;7z3^z?rn(s|R}N+Jo!JO1-*rxQyqqw0F6X>RM!ACLOT
zz2%2LmeJ%LT4#%}#EGCHI_{2UWy@EDy<D!{^rq{)F02=T#ey@^z4nOON7b}tj!SlP
zrC<7$DRfa3qlb_Gl$d?eBa*exb2~?1$m4`BkDVcz#+&=mqIhaS#tvu9x;lN$V%ue4
z#n=`g7=6)X+;9?WUhk`;3x@WY`0Ok}?-_Q}Klq)aeGT*$dw<>hONJs|(RvT?Z?St5
zJe~uJKn9oH8ehO#Qs>h$z{xbsOPOK-DR@vG_GmD%r#Dqp<DBtZzETt@xZ58t7p*(W
z{9|Eo&1xS#QRL>IINVFeAg~=yeiV@S@ycUgHP|m(|5vgek*ZEH4RdwK{d$6jL>+4X
z`4ChMuWjS9nJF{!ij95ygn<6)v21l;z0gZ@UjZab`9s9KK++(ZU<Qy+r~T3z5OXSa
z>}hS#jlPp@YHsdX+N+j<%}bAm_k9)T8@LAgyH4JzSyw>)>X<QXuHb`rdwDo`fWW<e
zaLMO38T)>O5eYZ-f(KZq4CZ8KAE}&p33a#nAw#ZL!zhkYZa0_fy&Jh12UfTAj4W?<
z)O2(X-CC!M1+7n}8-n0j-m&cbyUhc#29lDYkyrH{#~wfcNz*@%?SyYHR3jK#qYkg`
zfOxt;Xp>!NetI3?-j2$*c6E0peFJ1k0qtRyN~0J}eb0QIcI)6yPMaCaqc?e?=5%L0
z!wWTLgHwtNdLFYf->C5f&&TSGJKKO9egTz|q7H=dL_|b<lB^gC#Um>SuP9CLa~>76
znCtPqJkWLkR->ldp7iOcx({hE83#s3x$hnxA+ElF{8rgnRi*q<k61@I+vilKTf=;Q
zem>nM5H$vfR4=(~l-hR-WQi)59iT6|TZNDzuRd5R$FCqNF)uguYRz#H!g{_d<p^bq
ztZ}|F`aycNue*41<c%O@cJ#lP)+A3tV_NEOB}azb0mjS5zI-+^JS>46@8|;<Q{JGa
zq`anP0?@70hVNRQ^PircmP92tg<+HCZE@n&2^o9NiheT*Lc=@yT+LE3xOr=$21dw=
zR!RY;y_zci7+O3Gu<U|Jcf)^@jlAZ}vinN8`K<cireyaAl;?l1?w(Z62JbEx9c)@!
z#qj!?9M>6d)C31g+?wwl9UWUN!_7@iU(y8@`IP%@=plBH%=02yJ&ynGhwto!JlUOX
z4ZlnR=UHTX(m73bJ`MY1G35^fepmv;GCh*P+1Gq7e{QeWW1q~jSlc~K9=z&1x!<tZ
zpdJge083T}XG%gq%6tsI_NWti!Q*wY*XtA7l7LGtP}GVDq-LM+y&WRebEjyqo0J^h
z?;ygP{Z@2&xJZBV@Yt<iIV%kGgAtw-po06gf5RL2__AB+l4BpF#p6ol%ws1v%q62q
zv*J^+`Dqblo)7#2=j9l%>R5Sw+>Pg|4d7B*QMP1eov^xZ{boH28L>~l{Zhb#6){LZ
zm@6K(L>wtG?EU!}!s|J9nZo{zoX5(&q`-6B*F~kgn1RWbz(S1+KjtWyba%eqCgn)S
zFN@z*^(@_A3M$`75z6`%5E_kQVPm~8vUW(0-)Xpgi@MOw2BBVq0JDHsQp!G~mGIp`
zt+g8jO^5Tn$1Dr}bH1cx#Vy}6vXegp0~5m=T_@{(WO?uY<)bNy2_cW;Z}e~9$_)RA
z&R+=3N)8DzIPSQK_PD=ZR~&XP^$^GwPJke6@_WV`6v&1R5CO#>3rm*Q23Pe8MP7zS
zN4F<m4LkIFZ);mB-`wOv?l<XyfkR(~xG&l@T6u+k1wxw_1UD=l;|>FUff1Z$-rRg+
zUOSJkl&izPHIn6>?4GZlL7(#}f01PSSIocavf<qzn6%h!|2rAt7PU*+-u`F~y-itj
zGZo0&a&k(&_Kk~Ajhzp}S*fWz>%MkVJQEWW_pySvMFl!k06}^LD&vwY0rFM7Sdo{5
z!^6JOK>wyp&Wqx=>r``x(>dQU>14zTUKY_wnm<dBva%{(z_aKCs|X<YP`k9cLviHF
zG)j-cT&yo%@;d!^r<``|R8cd*4M>ZX{if`ugWtEtf5;FbfaR!RPMaJd=iiHkN)+{o
zw=7_sEj@pbvWiNnS(}3oHAP5B$P(ALz1hl~T>aEY-XZhaZD8r8xs7nX)xEONDCjv;
zOUTNmo&)Iqji8`liKzLOKAgKFJ{IZWuJ!WaoUryyAsQa}xqK??8(ZCEx7KBV%c+g}
zKb}#TJ3DI#oj_gNy)Lv4->#&&w<8@3lV0qv{JbmRz6E=*>$kXAjlB;|IzAm_X;uCB
zv75{DlZUCPX_L_PUzBOWE8nZ-zMA|iD#R`FU!}FRHDgRnO#Q#U-C^A-YHAzgg4T%^
zfT5UUrP9zgG!1;~y?UlKy%*}N2#Hw+^T|{6+|stQd7Vod9abu&XJ-I|3zTY<JK<1p
z4P|6y4YO-IP~}147tekgY;f6_D=8=l{=PHEE2QS8FSK6m3aRK0!;|xq6}q@XJetX+
zXl!k6W^F#W@9Z|!`QFuz3Ou-MHp}jk$&&MY{wZiZ#n3oxzMfRXwk<9?r><mFTwKfz
z5%hXtGDymiCjV$Y>{NPshIIhMP4(e)?%0Uv@SDj|pppr_Jb?SU%^oes09s*d=hLG%
ze`5g7Pg|}#S=k6d+Fsg@%%xAxPEJlx7nk#=$7MiN?f*(I2$<XjD2uy+LB$LJDGlm~
zeUk%qNs5yeL6W8%sC{+>fs2BE8qna6zlDpw#AvWn%g%t!02^>Yj~m`Jc$>iH%XJ+e
zZKMxoZxEm;V0$ox2<jq;palKJ8w}r32i^h01B>?Us2H~)s3I>H<)HIzL9C2*B2%2%
zuoz$qGG)~AC_jk0_89l)-w}Kjrep=sU*p1KA%bo<^)Jj!7x@I^8ms<D0nxHldm>O>
z4>V)boi5cC6{?m%cYKi1zpQXO7Sj9MjcR~0ZwA;qj*h|~Zix)e+JeBwbFm_`UgUBj
zgZ3CD_k(a;jH;dGQ*p3Bd%O`Fzi<iAb;U!<KOl)f<UG;v6_0U)%h#b((w4?30W~rB
z<2**F#H<1Cln0ym<II2+UMjOtd4&Pncv1DV4i4$^khD%AA}a6V1m?s^h$(~#RFJ^+
ze<A1rNNV^h!Y+E%VZ`XdXXBVi$~~x5eEq=YSYWHJuv9ohI*fT8cr3<_CQsKB4O{{2
z@U3K^x($V;pWvd36t;lJ(b|I0HFvt!_Fsbq|L~)OvTpek&<rdCk4a>pXyFXu?vx-t
zRRt(cFcEnGVV%fzDOH|TTM%e0EhS#yS{w_?&t<&+7NqhCIwh;+O2Q)sTsc~YfC3bU
zq)x}416#3Ue7ioXI|D~PEaz~kUWr15fPZx;b=F9Tzwr0+<AZQob6`ttK><&|tGXq@
z-!p+0U!6Y(5C2L=UkFUuSvc=t6~zQS+H_VW)Q7oK9zW$`$z@cCY7+m<S&H4r72N5X
zD42x4kb`D<pdo)l@`ml`wDHW6yBJw%z5+wGAgBS)>xgHfQ2uQhZ-Bg<!7@5R1vYX2
z+mVhCsk1}Zd4s=ov<3l9{rh=XR5hqW0d(>6F)mmEz|j=0p4I}11k?as_b+mA<T_MM
z8Y%@eR4-1Fe3?a^7oRHd%m@f~VV6L+D3G~T8>%#j1I8iXEwQXk2s&yQGt+}5bJPGb
zYs;@L`=D=*mVrBcjz(@?+%AG&0^6ZbXHPVwwQtYzgjukZEJ2bVC7?Lfli&RQ%4ql>
zZ3Bjstq?yQBdHTP-^B*^9^)d5i0V_-08a5k6bqU|cZ>@XF)H;$15M;U%QGN@6}<!e
z;l)HE%x?{XDjh@&^Bew^{l*Z)Ox1$aFP_61rP=BCChrM<90ZIp*zi4gd4wN54oy$%
z_UbLzbptRKcHoQ@8P&o7KmLE0xpUls6Gegn8@UcZ&kg=^_@Kyj1dZHFLnXR4M91C&
z9UEmSn~4nB7EvOHo{kB(LAgU2jn<2^0sp{Xg;2Gi=)TyYXsL?o1$++#wuNr2LQsPN
ztNyNm0iSlAgKZeSwhVk3nFMnqR3S!)fUL*$v?P$QLGSTRP7JkJEayt0=ST=j5nkY_
zSnZcoX~X>Z0a%$-dB3&%`n1N7*~lrtIJ9})K!Z&`SK<Vs(7HXMcdM%8p)9~4(Gda#
zfY}m(iCB*Sw$6kDheIgwXB06xM(~btQUAMMLYW5!AXwFi+~3_a7Gh@GNua_(#kc`M
z56&lqe$enpMM4q7>bd?mA;INup)SGh_}|FuM4}K3U^9Fb8+f3I)=d<w007lg2(h$B
z6H4g?vdO<`*%Qa25y+&bq6h2!=Eq0JT7>OLQaPRBqE3YvwXIT1%t;9*xKkQh>Kn}+
z;Q<4<$i;f+^O`#E1n{wLZ`^)rpKs3y{DkQ8RN<llMw{SqMIbtmX~2;{&R6*3dcafS
zUReDoz^SjjV8C(y9}j%}@6MPRZ+aZbQ!&!zF;U6B$)j2Ad{Tt=g<Dd94=MAOp&Oeo
z=|Lb%C<&EU5UeEp56c>lH1vu<1qB`m4Ar|D>VgG6BuM+f>~<dpBd5#@ZF_nN0vUcM
z13wUptrdCRD?&{?pHx7gZxz3w=cQlz*Spr$SC=tBpqH>Dm_|t=53>L+p^5?Ap9utF
zppJ$KnZyZ<P}~K_e*%Gi#>s@R1KmMUXW&i^`seS3FLP#h)<V8>k*_YGfPxP+Lf^-x
z_sPQj+kVNDcrI(6;l2Q!R|cD$^7tp7bf7;!t@&f~h=LG73c#aHVNd&EsO|JDJ9OOx
z4GDygn<z^OoV`f8<}ZX<B7qeM;Rag4?(EUwmYCpS!sHL}d2hhWlqY~^1bpu&XiW#a
zRfB3Enkr9%VDK>sW{dr3{vF5xpd#k62wqCZIlHJHXZfU5XlU_yV<>Rk0;H^GfkD(N
zC8;^<sY*xZS2%&c`T5`6h`z1PY)#~)cn8}dOxyMhgH;gS)V>cd!U6lfJ^KX0@2Cm4
zd=8d)ev_63t*c7*w-JRN1{3v+Dme+gIiEr$x928QNy2pGAlduak_Nks{AB%r4d$fH
zGhrW1qRbq8d%BIF&*L}E-fw%oD&cP&m%}Rqb%t5mz_v?@_2O7|JOgdKNWi}S@&06z
zP{QC}7)~36DkZQSelp`cI3;!}dXmDmA1C4Sk2IJkXca2?Rq1CeU5pr=Z$Eb|MA79r
zy57rE7PN*8KqGRYA39>(eT2c|(NGsf<M}}EeD65_EAV(9)Fmh4?#vV;$a2C)-Vo<g
zB{j4z1?E?=c^_!w!~w3aSbyz+Kb}tcq#q0Gz;E7jr=aqZyY9@u3i5xBVTQU+v=`|Q
z9v6mLe*Ns5L?G?+tgAha_WnsmjG&aD!t8HrW3)R}21BmyuFSI$_rl<_2l2qOastl^
zix)ha3ZoGMR|z_PPyFGCXb*CyXN+m{UE4Hny%w=?&?7?0gWk~3kCEWTi$;0L`6Q~r
z<TG651B+?kN4*Ro<AZnjj&A(+lY%s;)`rPwN^}G9q%S3}x>r{%mU54|>P>rf=dU$a
zVl0M^67U>rcEgp*DN>+P_xfhO3&BA|ja^IR9nZ)3c|`h+cLeVW#xuq+3uv}nG%A6$
z#JM=Sf#pn?8f_ORN$Xqfs`b>*IN!zY+eSWe1i~INHf-G~8xDL(&1R?cv=sOgWv}k5
zvteF@Dw-i34yuxg_Ahk2WZ_t?;YIDiHl87$aT0m1go-04L)r0u>_1x^n=umxKkx@T
zXKB)YVqzl*IYMPkmI~jo$xVn-{?=xiJ8`YE6AE&DmVLZJomVQhqu;I!nEhjZSW}A&
z9aUJDnK_|~5?EL>2L>l;*|=qJIbzC{IPFmO%Mz2JRMpR3A?xzA_EP8?g@56F6FM$q
zslHsKdNUki-|0=HpNsVPE(Hds%j;8TT#?q*M<cT|rOH$3@=mvB%<9ZgeoSqFe;sUB
zDT7WKY>Z~(-<~C%b_A2|X}`fHdwvLeljj~n5@a2L|D4uGt&g#XQX&30uL+<ae4igT
zz3h$%_a?EE*3lE6{9SQ2;xj}lzOufu&KjF7e&HTR(nsQ)^kw_`Wkg5@<6N9<z8)QR
zmk0rdp1;x9m${>(&sRQi{`=)W|ELz*<ID&$&<FNliR1U)iu~r?J7oYv2ySYFLj3H<
za-4EwQ~E-U%*1;8*d0lSv8i+-Z5S4+DU&WtSM~7lOhV3{LNZaE@>%Z{nK;cEHSxzI
zsr<RrwW`49`D&KWik8*z3Yw3rkSCt$kmFH#N;}bmA?<(sJ40dQtN5cK<lwbyLSBp?
zrNKE5CCM`OvT^LACe|<g;zv($MSd)-eaej=J<u_7dMW*V#t`j0AJvdWNpUbNTh($p
zPFX92N7njLs`!Ts+FQ2nCaxT;K@1qF7b!KitG9ZuN8J`5)hqli1Rn9t1mAH_oj~xJ
zk6ZlHr6r7IC>A`<4Rz6Z8InRbWPpDMuP7)CHtM(&gT2jMVmcA6EvW4pd-}Qk^8M1f
ztkSjGqJ~jOXiyi^+0T{nVA#S8`(S%RprmWLq)G}RWd2UQ3q_lH)oTEP+*uVvg!LsN
zp4D^TqM1J4uq;@HeYBs!V3ZI;JIi2~d7$Bqe6W{zYP=5{*J{>)N}3r95vhNZ5{>PP
zuI(%1IF+xoU4$(JU!{WLTR0_#*HMh#ml-ls=i#wjLt?A2l@z+gU?w4~g&W3HA^s`4
z4F}UB7IHlO-A+nWfeRdy@gjRdiHelQl{X=Mi%^&FKd0*<@l5nI+3Vc#h{rC$Kmoe-
zvpZ3bMn*|-qZj5396y5q;CzC9kn7s`b9To2s502|i^nLOu-LIIk#*-sAS;X19xv|T
z{0*l<mzTJoQHm%rc_RMTDU4`o$~AjGS{dK2*VJbMH)3n?P8HnHM6wYK8aZRgDhavl
zrX`EfaACR>s}liCZmq|f?Z;8zxgGkM?Er0=gxe!}5B)Cfts%zx+KRG~>%xK$^Qwgd
zxLTF&cv9bZ<-o^?!iTp33(@-}y0vl_)2I~G)HF}92jl3v=#<)O=E7Rmdy2oglf|Hk
z9aQ(|WOUn)IJ?Ev)&Fx6bF6)kaLn)DLr)thofgM!&@~!rT^O7bt@e5Or7|3fA$%9Z
zdcgKVh^j?@4Sr`l^A=M|1^Pj?Yjm{L@U2TTmvgF?=HwY>`I&p}76Dqk08?8TUT-A<
znh@%m4Kr8Z2eAG<tXlorP&?kMV^BTn*+#9*Yfiex{qF@FT|#8W)0YWCORkBmCC-Ik
z^eSz{=6)z*$TCm5z=;U_-0;B56=R+#GL~V%ITVasflG|5gTG9Q$`V(*Qj;nahG|{=
z%Sed5Kgw3shgp}{bmx7ZmIZ<fqx^G?FS;*lM!JFJzW4bOa@X_pFW@g^2RalqVKS65
zU6x4IiF1EsN*aS5*NqbiR3ZjW0$l2Gc-vC%)OkAR>|xkt@vbVWT<HX@v_V?@M1J4!
z_Z33i2?Ue|;2$dA`Pjd`aF=7lFl`EsYSQmb+x~SOoJ8FVBX)a2wrif_xsJS%(3)4b
zLPXy4*1{YgSEX%{o99#^vgjAS5wCM?H($$F03NKeC0(}=lS7lqFD8$iMp?re%Ziee
zVnK5-Eg(DRO<G4i*KtO&P%v@hpLkrWsw1~5cCbxvHWvB9+Lz@^v5ohqW+yd9O-EPS
zI4*))E^drPw$szdKaRIF@|R$KM~pdqem0rnj<fVBFZNL3k2Z7ViSaHv^gtz}=}L?L
zwwvaO)R+l~AU-^au6fp~*20IMw+L>WWo?7J*w`48u>8$6i$5I*42V{tPKX%OlI`)B
z()!3ib)k>8#b55^e=w>k^6=B1ZbJ{JnSv5WmCH4hd6f6{<7;|@XRLCWiD<Qu;}Znh
zy7mfnDD!DuG$BuZVy3*-=i0BVVlCKu^!Gm{T{5NAoW~@RM6Zh3&L$?3^r%XcJ}YqT
z1fp4)A$MM4d(SNXxc;*>m99yGhA{}Wd_r0D9El$(X5~ZD@6Yeq%JJ_WnN-%QoT_^&
zls{`|J%9EnfAEk~Gyx};)G-ocS*iS3?;B@s#A40HNR)|D9E{SnkxmA^FH@JMsvova
zX8&ayT=lAu;Am_z@<WIZ?WzR{`g?_Z$zMHkFD37Tx(FNVM8b))63Jm1Fw1J>Z2<%$
z6{BjEfS{w}t2VXwOJOAH%tlL96JNh1#=Wf*>*+C=wXQ*vNIs(oaU1*ElJg~AWBoz3
zYsEM6%XU&tu#eu|7g)PJi(C|&tXR}Xn&hnaxd#gx>+j>0Lv??-YpZnq{-}DCX85D*
z_dAPe6Sn6vuwUIAo->ylZ}KI6nQVFL;mT@J6n=}jh-Pjuf%pEv++Pdkdj*Zwp7DBP
zrhTtVw6{gZP)&6$66l-|HJ4w8%;eg~^EPs4nrqIew2C|SrN%S*vk{KJr_g;`Y{qKK
za@>)WYnnH1$Y7g~H208STe0$q*DizuUr(^Zw@=B+o)Nq~aS$e4yGfT@>daH^DKIsz
z)cwow6@MRvpi+T4eltCU8Nd3<n^l!7^Xy#}WkDiDhDG4ZxENM2jbH5jn}f*jP5iW|
z<-a9mWl0%U5yz-$X@2=Vcr`k6$?6e56?__pGJZ*81e@AQ*XeBe&1(+uq@>QQI$>-+
zFgW?Ai;?m(M6`3~3t0MUNoj|7dB1(@$eb{5jiAiUXxRcsX%N7syb8a-?5DbXQY&dt
z%QtBuucYrm*1Dmz5?dn{A0>WU5x4YjETQ|M(8cAmSNvkb`2Ku7q`f}>zYyO<$8g@x
zUzs*Gk#M%Gr7AVQ9ITO)uwuolOW^<Re|oe3f2I>S4&Y0P4a>%SIVE2<Ogg$wEOaxo
zg|1^}LSgb{ksb(<jt4fI5bee(U$*%Yp@oP?hDS4B5~Uu_^w78ukJEg2twP3bf5rXy
ze*N(J>Gjj|O|=mQu{^v#Iu?g_u(5R=+V-wf&8^chyn4kgY80C{bYAldhLKCo<yd_^
z;XaDiq3(`tFig^go9<t%YU1e1Z@+6gz1$g~fyKrmwM0`u8px}6z<19EUQX!k*96WU
zF)+?5Y;DQov}ftfubdR>o`h9f)w_=Um8!5STwbwF{_U3Tizp{AyHC84h8bs|GATle
zy3F*$Rqlvr6MURu^<;ctoJ-C-Z#8oO_$tvow?$;EY1DOi-(^S-2TIzlGbRuwb&;-U
z7yZw0dus6-cg1D}!{`%wA1C;jA;~pKJ=T-XVnJJKHRncn?@<z=s8fO!zPuOS!NH!V
z@$<fKy<mjii=i`@!MN3{3}(SFi<Ud`qcpfnBr`MTEu0N@k#XVsf<E`rYqkro*n?ED
z>V?ZaEO-)De!(_{cb?PQZM`vF>be}<$+yB>x!<CyF2h>n%iMG>Ka&r0;UkQJwE7Mk
z$`Z}Z4DInTXYHvlPzHtdM7q)!O@~4(CDCwL6=&P!6H8`ZIu8IG){ebnFOOsgJi5#J
zo*23$q%3OfclD3VJx10L$%S6mR#0O{Z-fm;yH{W~(%~1?#@9nDw|X-FV4WFNNE?u=
zNsUpZB9>X39R6ko>B{E@palselu!fi@;2{*99+kw!vd7==a0k1U~mtD?VxZfC+5?F
zS*Nm{8|Z%mvGF+lu?k#)zW_q@WO_h`!~-H-wk-C(55!&CS9;0~tCO{qo!|0Ld`A4R
z!?{Ezel$sA7!Q>QFASC6a)j3Q9elimCP?JOd_Bc7+iW%D38BRl_}=XI4MruG!$~Y!
z)#3G0Ing0FLqmnq)N|04xKy0c__kp$%3zYQ=jbnv1!pZF??**9r5!AA6QQz8HaBac
z-)DvlmLg^;&QlPoDNzVPj2csI+Sp_G<O%{oqF(#;P*U||kdlA2UwtMNH(J-k4fyDH
zA$|{~sQSfwdgl#VXy}(TIerA5M`rq~1_i#(@XfYO&(-$Btp99rn<RS+b)404q`W@!
z`qFaU^99RNr<hnD{I0&>Z>Y~5tTw?OOsB`~G>Igi6#!z<TzW}zubEWM8+;>}vZxRn
z_mZ3%^Wj5E=1-Gbo9g3YL(lz--KY}z4<nU+nSdXfEc9-vv7^lBT8q_tG+ui&R-T|Y
z1XCmt?S=J`v8FL}N>P9;Rev`lWf1j9yQ{VAp+Sf5=7Hy=={c`2yJadWY-BAQd=Lk{
z_R5BjK&)=KT9B)gLV{1>rb2!28ThgAIW#%qO5B*yF`N0*A1srvHd0v*yWIk#^?)Ts
zM5jSs*i*xFz2D7|(i500ZVx1J5^<9kp}e$hGHAC2LutRtBH%ytYf07!h%N|p$O_p@
zM76*Ze2pGv`4>e77PC@W+j}V~VvZ$pJA7_u0J6SUt6#Bso7y)~k6x^LN|Y_7g&3t~
zc23k<JoM`A*ErNBV*eXtcn4^vN0~e~Nw{e-JI}Cg*CHY)V0l_9EqR}$MXll!a0&#@
zGbOWF>Dy<C!L>J8C;xg3Hmb~#>n;^|83;U1180$1Uh7VPe5U0Yj3=xI`|HIDic);1
z%Zx)#tB~cm_w^{1mrymqhd`i6ceMi`P*4dL1i~7qi~&@l{y)Y0VJg~EDlzW{YoEjk
QxT*u><#`b=z=h}h2b9~@i2wiq

literal 0
HcmV?d00001

diff --git a/common/tier4_adapi_rviz_plugin/package.xml b/common/tier4_adapi_rviz_plugin/package.xml
new file mode 100644
index 0000000000000..a45a25898384f
--- /dev/null
+++ b/common/tier4_adapi_rviz_plugin/package.xml
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>tier4_adapi_rviz_plugin</name>
+  <version>0.0.0</version>
+  <description>The autoware adapi rviz plugin package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <maintainer email="hiroki.ota@tier4.jp">Hiroki OTA</maintainer>
+  <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_default_plugins</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>
diff --git a/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml
new file mode 100644
index 0000000000000..ab2c0243282c0
--- /dev/null
+++ b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml
@@ -0,0 +1,7 @@
+<library path="tier4_adapi_rviz_plugin">
+
+  <class type="tier4_adapi_rviz_plugins::RouteTool" base_class_type="rviz_common::Tool">
+    <description>RouteTool</description>
+  </class>
+
+</library>
diff --git a/common/tier4_adapi_rviz_plugin/src/route_tool.cpp b/common/tier4_adapi_rviz_plugin/src/route_tool.cpp
new file mode 100644
index 0000000000000..53871611843e4
--- /dev/null
+++ b/common/tier4_adapi_rviz_plugin/src/route_tool.cpp
@@ -0,0 +1,34 @@
+//  Copyright 2023 The Autoware Contributors
+//
+//  Licensed under the Apache License, Version 2.0 (the "License");
+//  you may not use this file except in compliance with the License.
+//  You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+//  Unless required by applicable law or agreed to in writing, software
+//  distributed under the License is distributed on an "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//  See the License for the specific language governing permissions and
+//  limitations under the License.
+
+#include "route_tool.hpp"
+
+namespace tier4_adapi_rviz_plugins
+{
+
+RouteTool::RouteTool()
+{
+  shortcut_key_ = 'r';
+}
+
+void RouteTool::onInitialize()
+{
+  GoalTool::onInitialize();
+  setName("2D Rough Goal Pose");
+}
+
+}  // namespace tier4_adapi_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::RouteTool, rviz_common::Tool)
diff --git a/common/tier4_adapi_rviz_plugin/src/route_tool.hpp b/common/tier4_adapi_rviz_plugin/src/route_tool.hpp
new file mode 100644
index 0000000000000..6ae88d0b83203
--- /dev/null
+++ b/common/tier4_adapi_rviz_plugin/src/route_tool.hpp
@@ -0,0 +1,34 @@
+//  Copyright 2023 The Autoware Contributors
+//
+//  Licensed under the Apache License, Version 2.0 (the "License");
+//  you may not use this file except in compliance with the License.
+//  You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+//  Unless required by applicable law or agreed to in writing, software
+//  distributed under the License is distributed on an "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//  See the License for the specific language governing permissions and
+//  limitations under the License.
+
+#ifndef ROUTE_TOOL_HPP_
+#define ROUTE_TOOL_HPP_
+
+#include <rviz_default_plugins/tools/goal_pose/goal_tool.hpp>
+
+namespace tier4_adapi_rviz_plugins
+{
+
+class RouteTool : public rviz_default_plugins::tools::GoalTool
+{
+  Q_OBJECT
+
+public:
+  RouteTool();
+  void onInitialize() override;
+};
+
+}  // namespace tier4_adapi_rviz_plugins
+
+#endif  // ROUTE_TOOL_HPP_

From 5abb0093d751a00d1060e6209e1278e9c188e4ce Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com>
Date: Thu, 13 Apr 2023 16:20:07 +0900
Subject: [PATCH 08/15] feat(mission_planner): add reroute interface for api
 (#3298)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../component_interface_specs/planning.hpp    | 12 +++++++++
 planning/mission_planner/README.md            | 24 +++++++++--------
 .../src/mission_planner/mission_planner.cpp   | 26 ++++++++++++++++---
 .../src/mission_planner/mission_planner.hpp   | 12 ++++++++-
 4 files changed, 59 insertions(+), 15 deletions(-)

diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp
index fecee4426cc11..9ee086231ed7c 100644
--- a/common/component_interface_specs/include/component_interface_specs/planning.hpp
+++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp
@@ -39,6 +39,18 @@ struct SetRoute
   static constexpr char name[] = "/planning/mission_planning/set_route";
 };
 
+struct ChangeRoutePoints
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
+  static constexpr char name[] = "/planning/mission_planning/change_route_points";
+};
+
+struct ChangeRoute
+{
+  using Service = autoware_planning_msgs::srv::SetRoute;
+  static constexpr char name[] = "/planning/mission_planning/change_route";
+};
+
 struct ClearRoute
 {
   using Service = autoware_adapi_v1_msgs::srv::ClearRoute;
diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md
index 572d7d55bb968..0b7269b85763b 100644
--- a/planning/mission_planner/README.md
+++ b/planning/mission_planner/README.md
@@ -25,11 +25,13 @@ In current Autoware.universe, only Lanelet2 map format is supported.
 
 ### Services
 
-| Name                                 | Type                                        | Description                       |
-| ------------------------------------ | ------------------------------------------- | --------------------------------- |
-| `/planning/routing/clear_route`      | autoware_adapi_v1_msgs::srv::ClearRoute     | route clear request               |
-| `/planning/routing/set_route_points` | autoware_adapi_v1_msgs::srv::SetRoutePoints | route request with pose waypoints |
-| `/planning/routing/set_route`        | autoware_planning_msgs::srv::SetRoute       | route request with HAD map format |
+| Name                                             | Type                                      | Description                                 |
+| ------------------------------------------------ | ----------------------------------------- | ------------------------------------------- |
+| `/planning/mission_planning/clear_route`         | autoware_adapi_v1_msgs/srv/ClearRoute     | route clear request                         |
+| `/planning/mission_planning/set_route_points`    | autoware_adapi_v1_msgs/srv/SetRoutePoints | route request with pose waypoints           |
+| `/planning/mission_planning/set_route`           | autoware_planning_msgs/srv/SetRoute       | route request with lanelet waypoints        |
+| `/planning/mission_planning/change_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route change request with pose waypoints    |
+| `/planning/mission_planning/change_route`        | autoware_planning_msgs/srv/SetRoute       | route change request with lanelet waypoints |
 
 ### Subscriptions
 
@@ -40,12 +42,12 @@ In current Autoware.universe, only Lanelet2 map format is supported.
 
 ### Publications
 
-| Name                            | Type                                    | Description              |
-| ------------------------------- | --------------------------------------- | ------------------------ |
-| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state              |
-| `/planning/routing/route`       | autoware_planning_msgs/LaneletRoute     | route                    |
-| `debug/route_marker`            | visualization_msgs::msg::MarkerArray    | route marker for debug   |
-| `debug/goal_footprint`          | visualization_msgs::msg::MarkerArray    | goal footprint for debug |
+| Name                            | Type                                  | Description              |
+| ------------------------------- | ------------------------------------- | ------------------------ |
+| `/planning/routing/route_state` | autoware_adapi_v1_msgs/msg/RouteState | route state              |
+| `/planning/routing/route`       | autoware_planning_msgs/LaneletRoute   | route                    |
+| `debug/route_marker`            | visualization_msgs/msg/MarkerArray    | route marker for debug   |
+| `debug/goal_footprint`          | visualization_msgs/msg/MarkerArray    | goal footprint for debug |
 
 ## Route section
 
diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp
index cffbd3d678212..2ab01e0f13d9b 100644
--- a/planning/mission_planner/src/mission_planner/mission_planner.cpp
+++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp
@@ -85,6 +85,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   adaptor.init_srv(srv_clear_route_, this, &MissionPlanner::on_clear_route);
   adaptor.init_srv(srv_set_route_, this, &MissionPlanner::on_set_route);
   adaptor.init_srv(srv_set_route_points_, this, &MissionPlanner::on_set_route_points);
+  adaptor.init_srv(srv_change_route_, this, &MissionPlanner::on_change_route);
+  adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points);
   adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal);
 
   change_state(RouteState::Message::UNSET);
@@ -145,7 +147,7 @@ void MissionPlanner::change_state(RouteState::Message::_state_type state)
   pub_state_->publish(state_);
 }
 
-// NOTE: The route services should be mutually exclusive by callback group.
+// NOTE: The route interface should be mutually exclusive by callback group.
 void MissionPlanner::on_clear_route(
   const ClearRoute::Service::Request::SharedPtr, const ClearRoute::Service::Response::SharedPtr res)
 {
@@ -154,7 +156,7 @@ void MissionPlanner::on_clear_route(
   res->status.success = true;
 }
 
-// NOTE: The route services should be mutually exclusive by callback group.
+// NOTE: The route interface should be mutually exclusive by callback group.
 void MissionPlanner::on_set_route(
   const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res)
 {
@@ -191,7 +193,7 @@ void MissionPlanner::on_set_route(
   res->status.success = true;
 }
 
-// NOTE: The route services should be mutually exclusive by callback group.
+// NOTE: The route interface should be mutually exclusive by callback group.
 void MissionPlanner::on_set_route_points(
   const SetRoutePoints::Service::Request::SharedPtr req,
   const SetRoutePoints::Service::Response::SharedPtr res)
@@ -248,6 +250,24 @@ void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPt
   arrival_checker_.modify_goal(*msg);
 }
 
+void MissionPlanner::on_change_route(
+  const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res)
+{
+  // TODO(Yutaka Shimizu): reroute
+  (void)req;
+  (void)res;
+}
+
+// NOTE: The route interface should be mutually exclusive by callback group.
+void MissionPlanner::on_change_route_points(
+  const SetRoutePoints::Service::Request::SharedPtr req,
+  const SetRoutePoints::Service::Response::SharedPtr res)
+{
+  // TODO(Yutaka Shimizu): reroute
+  (void)req;
+  (void)res;
+}
+
 }  // namespace mission_planner
 
 #include <rclcpp_components/register_node_macro.hpp>
diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp
index 43673eae16406..7e8bcdf11babe 100644
--- a/planning/mission_planner/src/mission_planner/mission_planner.hpp
+++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp
@@ -42,9 +42,11 @@ using PoseStamped = geometry_msgs::msg::PoseStamped;
 using PoseWithUuidStamped = autoware_planning_msgs::msg::PoseWithUuidStamped;
 using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute;
 using MarkerArray = visualization_msgs::msg::MarkerArray;
+using ClearRoute = planning_interface::ClearRoute;
 using SetRoutePoints = planning_interface::SetRoutePoints;
 using SetRoute = planning_interface::SetRoute;
-using ClearRoute = planning_interface::ClearRoute;
+using ChangeRoutePoints = planning_interface::ChangeRoutePoints;
+using ChangeRoute = planning_interface::ChangeRoute;
 using Route = planning_interface::Route;
 using RouteState = planning_interface::RouteState;
 using Odometry = nav_msgs::msg::Odometry;
@@ -79,6 +81,8 @@ class MissionPlanner : public rclcpp::Node
   component_interface_utils::Service<ClearRoute>::SharedPtr srv_clear_route_;
   component_interface_utils::Service<SetRoute>::SharedPtr srv_set_route_;
   component_interface_utils::Service<SetRoutePoints>::SharedPtr srv_set_route_points_;
+  component_interface_utils::Service<ChangeRoute>::SharedPtr srv_change_route_;
+  component_interface_utils::Service<ChangeRoutePoints>::SharedPtr srv_change_route_points_;
   void on_clear_route(
     const ClearRoute::Service::Request::SharedPtr req,
     const ClearRoute::Service::Response::SharedPtr res);
@@ -91,6 +95,12 @@ class MissionPlanner : public rclcpp::Node
 
   component_interface_utils::Subscription<ModifiedGoal>::SharedPtr sub_modified_goal_;
   void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg);
+  void on_change_route(
+    const SetRoute::Service::Request::SharedPtr req,
+    const SetRoute::Service::Response::SharedPtr res);
+  void on_change_route_points(
+    const SetRoutePoints::Service::Request::SharedPtr req,
+    const SetRoutePoints::Service::Response::SharedPtr res);
 };
 
 }  // namespace mission_planner

From 3310854d9f7e12c79404d175b4597a6acd1e60fa Mon Sep 17 00:00:00 2001
From: Tomoya Kimura <tomoya.kimura@tier4.jp>
Date: Thu, 13 Apr 2023 17:01:09 +0900
Subject: [PATCH 09/15] chore(planning_evaluator): add dependency (#3388)

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
---
 launch/tier4_planning_launch/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml
index c0c7ac47145fe..050968fe70329 100644
--- a/launch/tier4_planning_launch/package.xml
+++ b/launch/tier4_planning_launch/package.xml
@@ -66,6 +66,7 @@
   <exec_depend>obstacle_avoidance_planner</exec_depend>
   <exec_depend>obstacle_cruise_planner</exec_depend>
   <exec_depend>obstacle_stop_planner</exec_depend>
+  <exec_depend>planning_evaluator</exec_depend>
   <exec_depend>planning_validator</exec_depend>
   <exec_depend>rtc_auto_mode_manager</exec_depend>
   <exec_depend>scenario_selector</exec_depend>

From 6eccbed60fe0857e4406144ac2fb641e62f9125b Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Thu, 13 Apr 2023 19:16:52 +0900
Subject: [PATCH 10/15] fix(behavior_velocity_planner): revert pr #2775 (#3392)

Revert "chore(behavior_velocity_planner): divide build target for faster recompile (#2775)"

This reverts commit 3c9aae4bf9f38c4d2dbf11778db2e066b5fa27a3.

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../behavior_velocity_planner/CMakeLists.txt  | 65 ++++++-------------
 1 file changed, 20 insertions(+), 45 deletions(-)

diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt
index ba89fe87b4db1..870791444adf3 100644
--- a/planning/behavior_velocity_planner/CMakeLists.txt
+++ b/planning/behavior_velocity_planner/CMakeLists.txt
@@ -11,35 +11,6 @@ find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED COMPONENTS common filters)
 find_package(OpenCV REQUIRED)
 
-# concatenate include_dirs
-set(${PROJECT_NAME}_INCLUDE_DIRS
-  ${${PROJECT_NAME}_FOUND_INCLUDE_DIRS}
-  ${BOOST_INCLUDE_DIRS}
-  ${PCL_INCLUDE_DIRS}
-  ${tf2_geometry_msgs_INCLUDE_DIRS}
-  ${EIGEN3_INCLUDE_DIR}
-  ${CMAKE_CURRENT_SOURCE_DIR}/include
-)
-list(REMOVE_DUPLICATES ${PROJECT_NAME}_INCLUDE_DIRS)
-
-# concatenate link libraries
-set(${PROJECT_NAME}_LINK_LIBRARIES ${${PROJECT_NAME}_FOUND_LIBRARIES} ${PCL_LIBRARIES})
-list(REMOVE_DUPLICATES ${PROJECT_NAME}_LINK_LIBRARIES)
-
-# utilization library
-add_library(behavior_velocity_planner_utilization
-  SHARED
-  src/utilization/path_utilization.cpp
-  src/utilization/util.cpp
-  src/utilization/debug.cpp
-)
-target_include_directories(behavior_velocity_planner_utilization
-  SYSTEM PUBLIC
-  ${${PROJECT_NAME}_INCLUDE_DIRS}
-)
-target_link_libraries(behavior_velocity_planner_utilization ${${PROJECT_NAME}_LINK_LIBRARIES})
-
-# scene modules libraries
 set(scene_modules
   detection_area
   blind_spot
@@ -55,30 +26,36 @@ set(scene_modules
   out_of_lane
 )
 
-set(behavior_velocity_planner_scene_modules behavior_velocity_planner_utilization)
 foreach(scene_module IN LISTS scene_modules)
   file(GLOB_RECURSE scene_module_src "src/scene_module/${scene_module}/*")
-  add_library(behavior_velocity_planner_${scene_module}
-    SHARED ${scene_module_src}
-  )
-  target_include_directories(behavior_velocity_planner_${scene_module}
-    SYSTEM PUBLIC
-    ${${PROJECT_NAME}_INCLUDE_DIRS}
-  )
-  target_link_libraries(behavior_velocity_planner_${scene_module} ${${PROJECT_NAME}_LINK_LIBRARIES} behavior_velocity_planner_utilization)
-  list(APPEND behavior_velocity_planner_scene_modules behavior_velocity_planner_${scene_module})
+  list(APPEND scene_modules_src ${scene_module_src})
 endforeach()
 
-# node library
-add_library(behavior_velocity_planner SHARED
+ament_auto_add_library(behavior_velocity_planner SHARED
   src/node.cpp
   src/planner_manager.cpp
+  src/utilization/path_utilization.cpp
+  src/utilization/util.cpp
+  src/utilization/debug.cpp
+  ${scene_modules_src}
 )
+
 target_include_directories(behavior_velocity_planner
   SYSTEM PUBLIC
-  ${${PROJECT_NAME}_INCLUDE_DIRS}
+    ${BOOST_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+    ${tf2_geometry_msgs_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+ament_target_dependencies(behavior_velocity_planner
+  Boost
+  Eigen3
+  PCL
+  message_filters
 )
-target_link_libraries(behavior_velocity_planner ${${PROJECT_NAME}_LINK_LIBRARIES} ${behavior_velocity_planner_scene_modules})
+
+target_link_libraries(behavior_velocity_planner ${PCL_LIBRARIES})
 
 rclcpp_components_register_node(behavior_velocity_planner
   PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode"
@@ -95,7 +72,6 @@ if(BUILD_TESTING)
   target_link_libraries(utilization-test
     gtest_main
     behavior_velocity_planner
-    ${${PROJECT_NAME}_LINK_LIBRARIES} ${behavior_velocity_planner_scene_modules}
   )
 
   # Gtest for occlusion spot
@@ -110,7 +86,6 @@ if(BUILD_TESTING)
   target_link_libraries(occlusion_spot-test
     gtest_main
     behavior_velocity_planner
-    ${${PROJECT_NAME}_LINK_LIBRARIES} ${behavior_velocity_planner_scene_modules}
   )
 endif()
 

From 5d0d2ce326afb40fa5744af25441d1671f0df965 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Thu, 13 Apr 2023 20:42:50 +0900
Subject: [PATCH 11/15] feat(avoidance): margin can be set independently for
 each class (#3002)

* feat(avoidance): margin can be set independently for each class

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(behavior_path_planner): use lambda function

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../util/avoidance/avoidance_module_data.hpp  | 34 ++++----
 .../util/avoidance/util.hpp                   | 14 ++--
 .../src/behavior_path_planner_node.cpp        | 36 ++++----
 .../avoidance/avoidance_module.cpp            | 84 +++++++++++--------
 .../src/util/avoidance/util.cpp               | 58 +++++++------
 5 files changed, 126 insertions(+), 100 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp
index aa9c1086e3b40..550bb12f62bc7 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp
@@ -29,6 +29,7 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 namespace behavior_path_planner
@@ -45,6 +46,17 @@ using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::TransformStamped;
 
+struct ObjectParameter
+{
+  bool enable{false};
+
+  double envelope_buffer_margin{0.0};
+
+  double safety_buffer_lateral{1.0};
+
+  double safety_buffer_longitudinal{0.0};
+};
+
 struct AvoidanceParameters
 {
   // path resample interval for avoidance planning path.
@@ -123,9 +135,6 @@ struct AvoidanceParameters
   // minimum road shoulder width. maybe 0.5 [m]
   double object_check_min_road_shoulder_width;
 
-  // object's enveloped polygon
-  double object_envelope_buffer;
-
   // vehicles which is moving more than this parameter will not be avoided
   double threshold_time_object_is_moving;
 
@@ -134,16 +143,10 @@ struct AvoidanceParameters
 
   // we want to keep this lateral margin when avoiding
   double lateral_collision_margin;
-  // a buffer in case lateral_collision_margin is set to 0. Will throw error
-  // don't ever set this value to 0
-  double lateral_collision_safety_buffer{0.5};
 
   // if object overhang is less than this value, the ego stops behind the object.
   double lateral_passable_safety_buffer{0.5};
 
-  // margin between object back and end point of avoid shift point
-  double longitudinal_collision_safety_buffer{0.0};
-
   // when complete avoidance motion, there is a distance margin with the object
   // for longitudinal direction
   double longitudinal_collision_margin_min_distance;
@@ -241,17 +244,8 @@ struct AvoidanceParameters
   // matrix col size
   size_t col_size;
 
-  // true by default
-  bool avoid_car{true};      // avoidance is performed for type object car
-  bool avoid_truck{true};    // avoidance is performed for type object truck
-  bool avoid_bus{true};      // avoidance is performed for type object bus
-  bool avoid_trailer{true};  // avoidance is performed for type object trailer
-
-  // false by default
-  bool avoid_unknown{false};     // avoidance is performed for type object unknown
-  bool avoid_bicycle{false};     // avoidance is performed for type object bicycle
-  bool avoid_motorcycle{false};  // avoidance is performed for type object motorbike
-  bool avoid_pedestrian{false};  // avoidance is performed for type object pedestrian
+  // parameters depend on object class
+  std::unordered_map<uint8_t, ObjectParameter> object_parameters;
 
   // drivable area expansion
   double drivable_area_right_bound_offset{};
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp
index fd6b15965c12a..cff9f2b507790 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp
@@ -52,6 +52,9 @@ struct PolygonPoint
 
 bool isOnRight(const ObjectData & obj);
 
+bool isTargetObjectType(
+  const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters);
+
 double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
 
@@ -87,10 +90,10 @@ Polygon2d createEnvelopePolygon(
   const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer);
 
 void generateDrivableArea(
-  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double vehicle_length,
-  const std::shared_ptr<const PlannerData> planner_data, const ObjectDataArray & objects,
-  const bool enable_bound_clipping, const bool disable_path_update,
-  const double original_object_buffer);
+  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
+  const std::shared_ptr<const PlannerData> planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters, const ObjectDataArray & objects,
+  const double vehicle_length, const bool enable_bound_clipping, const bool disable_path_update);
 
 double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v);
 
@@ -101,9 +104,6 @@ lanelet::ConstLanelets getTargetLanelets(
   const std::shared_ptr<const PlannerData> & planner_data, lanelet::ConstLanelets & route_lanelets,
   const double left_offset, const double right_offset);
 
-bool isTargetObjectType(
-  const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters);
-
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   boost::optional<Pose> & p_out);
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index d602e40bda1b8..c1f9a98f7db29 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -477,6 +477,8 @@ AvoidanceByLCParameters BehaviorPathPlannerNode::getAvoidanceByLCParam(
 
 AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
 {
+  using autoware_auto_perception_msgs::msg::ObjectClassification;
+
   AvoidanceParameters p{};
   // general params
   {
@@ -494,7 +496,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
       declare_parameter<double>(ns + "drivable_area_left_bound_offset");
     p.drivable_area_types_to_skip =
       declare_parameter<std::vector<std::string>>(ns + "drivable_area_types_to_skip");
-    p.object_envelope_buffer = declare_parameter<double>(ns + "object_envelope_buffer");
     p.enable_bound_clipping = declare_parameter<bool>(ns + "enable_bound_clipping");
     p.enable_avoidance_over_same_direction =
       declare_parameter<bool>(ns + "enable_avoidance_over_same_direction");
@@ -513,15 +514,26 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
 
   // target object
   {
-    std::string ns = "avoidance.target_object.";
-    p.avoid_car = declare_parameter<bool>(ns + "car");
-    p.avoid_truck = declare_parameter<bool>(ns + "truck");
-    p.avoid_bus = declare_parameter<bool>(ns + "bus");
-    p.avoid_trailer = declare_parameter<bool>(ns + "trailer");
-    p.avoid_unknown = declare_parameter<bool>(ns + "unknown");
-    p.avoid_bicycle = declare_parameter<bool>(ns + "bicycle");
-    p.avoid_motorcycle = declare_parameter<bool>(ns + "motorcycle");
-    p.avoid_pedestrian = declare_parameter<bool>(ns + "pedestrian");
+    const auto get_object_param = [&](std::string && ns) {
+      ObjectParameter param{};
+      param.enable = declare_parameter<bool>("avoidance.target_object." + ns + "enable");
+      param.envelope_buffer_margin =
+        declare_parameter<double>("avoidance.target_object." + ns + "envelope_buffer_margin");
+      param.safety_buffer_lateral =
+        declare_parameter<double>("avoidance.target_object." + ns + "safety_buffer_lateral");
+      param.safety_buffer_longitudinal =
+        declare_parameter<double>("avoidance.target_object." + ns + "safety_buffer_longitudinal");
+      return param;
+    };
+
+    p.object_parameters.emplace(ObjectClassification::MOTORCYCLE, get_object_param("motorcycle."));
+    p.object_parameters.emplace(ObjectClassification::CAR, get_object_param("car."));
+    p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param("truck."));
+    p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param("trailer."));
+    p.object_parameters.emplace(ObjectClassification::BUS, get_object_param("bus."));
+    p.object_parameters.emplace(ObjectClassification::PEDESTRIAN, get_object_param("pedestrian."));
+    p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param("bicycle."));
+    p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param("unknown."));
   }
 
   // target filtering
@@ -564,8 +576,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   {
     std::string ns = "avoidance.avoidance.lateral.";
     p.lateral_collision_margin = declare_parameter<double>(ns + "lateral_collision_margin");
-    p.lateral_collision_safety_buffer =
-      declare_parameter<double>(ns + "lateral_collision_safety_buffer");
     p.lateral_passable_safety_buffer =
       declare_parameter<double>(ns + "lateral_passable_safety_buffer");
     p.road_shoulder_safety_margin = declare_parameter<double>(ns + "road_shoulder_safety_margin");
@@ -579,8 +589,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   {
     std::string ns = "avoidance.avoidance.longitudinal.";
     p.prepare_time = declare_parameter<double>(ns + "prepare_time");
-    p.longitudinal_collision_safety_buffer =
-      declare_parameter<double>(ns + "longitudinal_collision_safety_buffer");
     p.min_prepare_distance = declare_parameter<double>(ns + "min_prepare_distance");
     p.min_avoidance_distance = declare_parameter<double>(ns + "min_avoidance_distance");
     p.min_nominal_avoidance_speed = declare_parameter<double>(ns + "min_nominal_avoidance_speed");
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
index 478f5d1632f3c..f54955c20acae 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -479,16 +479,18 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
   }
 
   const auto o_front = data.stop_target_object.get();
+  const auto t = util::getHighestProbLabel(o_front.object.classification);
+  const auto object_parameter = parameters_->object_parameters.at(t);
   const auto & base_link2front = planner_data_->parameters.base_link2front;
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
-  const auto max_avoid_margin = parameters_->lateral_collision_safety_buffer +
+  const auto max_avoid_margin = object_parameter.safety_buffer_lateral +
                                 parameters_->lateral_collision_margin + 0.5 * vehicle_width;
 
   const auto variable =
     getSharpAvoidanceDistance(getShiftLength(o_front, isOnRight(o_front), max_avoid_margin));
-  const auto constant = getNominalPrepareDistance() +
-                        parameters_->longitudinal_collision_safety_buffer + base_link2front;
+  const auto constant =
+    getNominalPrepareDistance() + object_parameter.safety_buffer_longitudinal + base_link2front;
   const auto total_avoid_distance = variable + constant;
 
   const auto opt_feasible_bound = calcLongitudinalOffsetPose(
@@ -711,7 +713,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
 
   // To be consistent with changes in the ego position, the current shift length is considered.
   const auto current_ego_shift = getCurrentShift();
-  // // implement lane detection here.
+  const auto & base_link2front = planner_data_->parameters.base_link2front;
+  const auto & base_link2rear = planner_data_->parameters.base_link2rear;
 
   AvoidLineArray avoid_lines;
   std::vector<AvoidanceDebugMsg> avoidance_debug_msg_array;
@@ -729,7 +732,6 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
     avoidance_debug_msg.longitudinal_distance = o.longitudinal;
     avoidance_debug_msg.lateral_distance_from_centerline = o.lateral;
     avoidance_debug_msg.to_furthest_linestring_distance = o.to_road_shoulder_distance;
-    // avoidance_debug_msg.max_shift_length = max_allowable_lateral_distance;
 
     if (!o.avoid_margin) {
       avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN);
@@ -762,6 +764,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
     const auto nominal_avoid_distance = getNominalAvoidanceDistance(avoiding_shift);
     const auto nominal_return_distance = getNominalAvoidanceDistance(return_shift);
 
+    // use each object param
+    const auto t = util::getHighestProbLabel(o.object.classification);
+    const auto object_parameter = parameters_->object_parameters.at(t);
+
     /**
      * Is there enough distance from ego to object for avoidance?
      *   - Yes -> use the nominal distance.
@@ -770,8 +776,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
      *     - No -> ignore this object. Expected behavior is that the vehicle will stop in front
      *             of the obstacle, then start avoidance.
      */
-    const bool has_enough_distance = o.longitudinal > (prepare_distance + nominal_avoid_distance);
-    const auto remaining_distance = o.longitudinal - prepare_distance;
+    const auto constant =
+      object_parameter.safety_buffer_longitudinal + base_link2front + prepare_distance;
+    const auto has_enough_distance = o.longitudinal > constant + nominal_avoid_distance;
+    const auto remaining_distance = o.longitudinal - constant;
     if (!has_enough_distance) {
       if (remaining_distance <= 0.0) {
         // TODO(Horibe) Even if there is no enough distance for avoidance shift, the
@@ -818,29 +826,38 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
       has_enough_distance);
 
     AvoidLine al_avoid;
-    al_avoid.end_shift_length = shift_length;
-    al_avoid.start_shift_length = current_ego_shift;
-    al_avoid.end_longitudinal = o.longitudinal;
-    al_avoid.start_longitudinal = o.longitudinal - avoiding_distance;
-    al_avoid.id = getOriginalShiftLineUniqueId();
-    al_avoid.object = o;
-    avoid_lines.push_back(al_avoid);
-
-    // The end_margin also has the purpose of preventing the return path from NOT being
-    // triggered at the end point.
-    const auto end_margin = 1.0;
-    const auto return_remaining_distance =
-      std::max(avoidance_data_.arclength_from_ego.back() - o.longitudinal - end_margin, 0.0);
+    {
+      const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front;
+
+      al_avoid.start_shift_length = current_ego_shift;
+      al_avoid.end_shift_length = shift_length;
+      al_avoid.start_longitudinal = o.longitudinal - offset - avoiding_distance;
+      al_avoid.end_longitudinal = o.longitudinal - offset;
+      al_avoid.id = getOriginalShiftLineUniqueId();
+      al_avoid.object = o;
+
+      avoid_lines.push_back(al_avoid);
+    }
 
     AvoidLine al_return;
-    al_return.end_shift_length = 0.0;
-    al_return.start_shift_length = shift_length;
-    al_return.start_longitudinal = o.longitudinal + o.length;
-    al_return.end_longitudinal =
-      o.longitudinal + o.length + std::min(nominal_return_distance, return_remaining_distance);
-    al_return.id = getOriginalShiftLineUniqueId();
-    al_return.object = o;
-    avoid_lines.push_back(al_return);
+    {
+      const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length;
+      // The end_margin also has the purpose of preventing the return path from NOT being
+      // triggered at the end point.
+      const auto end_margin = 1.0;
+      const auto return_remaining_distance = std::max(
+        avoidance_data_.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0);
+
+      al_return.start_shift_length = shift_length;
+      al_return.end_shift_length = 0.0;
+      al_return.start_longitudinal = o.longitudinal + offset;
+      al_return.end_longitudinal =
+        o.longitudinal + offset + std::min(nominal_return_distance, return_remaining_distance);
+      al_return.id = getOriginalShiftLineUniqueId();
+      al_return.object = o;
+
+      avoid_lines.push_back(al_return);
+    }
 
     DEBUG_PRINT(
       "object is set: avoid_shift = %f, return_shift = %f, dist = (avoidStart: %3.3f, avoidEnd: "
@@ -2496,9 +2513,8 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
   {
     const auto & p = planner_data_->parameters;
     generateDrivableArea(
-      path, output.drivable_lanes, p.vehicle_length, planner_data_, avoidance_data_.target_objects,
-      parameters_->enable_bound_clipping, parameters_->disable_path_update,
-      parameters_->object_envelope_buffer);
+      path, output.drivable_lanes, planner_data_, parameters_, avoidance_data_.target_objects,
+      p.vehicle_length, parameters_->enable_bound_clipping, parameters_->disable_path_update);
   }
 }
 
@@ -3481,13 +3497,15 @@ void AvoidanceModule::insertWaitPoint(
   // D5: base_link2front
 
   const auto o_front = data.stop_target_object.get();
+  const auto t = util::getHighestProbLabel(o_front.object.classification);
+  const auto object_parameter = parameters_->object_parameters.at(t);
 
   const auto avoid_margin =
-    p->lateral_collision_safety_buffer + p->lateral_collision_margin + 0.5 * vehicle_width;
+    object_parameter.safety_buffer_lateral + p->lateral_collision_margin + 0.5 * vehicle_width;
   const auto variable =
     getMinimumAvoidanceDistance(getShiftLength(o_front, isOnRight(o_front), avoid_margin));
   const auto constant =
-    p->min_prepare_distance + p->longitudinal_collision_safety_buffer + base_link2front;
+    p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + base_link2front;
   const auto start_longitudinal =
     o_front.longitudinal -
     std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance);
diff --git a/planning/behavior_path_planner/src/util/avoidance/util.cpp b/planning/behavior_path_planner/src/util/avoidance/util.cpp
index e76080b10a983..8d4142e430024 100644
--- a/planning/behavior_path_planner/src/util/avoidance/util.cpp
+++ b/planning/behavior_path_planner/src/util/avoidance/util.cpp
@@ -378,6 +378,18 @@ bool isOnRight(const ObjectData & obj)
   return obj.lateral < 0.0;
 }
 
+bool isTargetObjectType(
+  const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  const auto t = util::getHighestProbLabel(object.classification);
+
+  if (parameters->object_parameters.count(t) == 0) {
+    return false;
+  }
+
+  return parameters->object_parameters.at(t).enable;
+}
+
 double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin)
 {
@@ -563,10 +575,10 @@ Polygon2d createEnvelopePolygon(
 }
 
 void generateDrivableArea(
-  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double vehicle_length,
-  const std::shared_ptr<const PlannerData> planner_data, const ObjectDataArray & objects,
-  const bool enable_bound_clipping, const bool disable_path_update,
-  const double original_object_buffer)
+  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
+  const std::shared_ptr<const PlannerData> planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters, const ObjectDataArray & objects,
+  const double vehicle_length, const bool enable_bound_clipping, const bool disable_path_update)
 {
   util::generateDrivableArea(path, lanes, vehicle_length, planner_data);
   if (objects.empty() || !enable_bound_clipping) {
@@ -589,9 +601,13 @@ void generateDrivableArea(
       continue;
     }
 
+    const auto t = util::getHighestProbLabel(object.object.classification);
+    const auto object_parameter = parameters->object_parameters.at(t);
+
     // generate obstacle polygon
     const auto & obj_pose = object.object.kinematics.initial_pose_with_covariance.pose;
-    const double diff_poly_buffer = object.avoid_margin.get() - original_object_buffer -
+    const double diff_poly_buffer = object.avoid_margin.get() -
+                                    object_parameter.envelope_buffer_margin -
                                     planner_data->parameters.vehicle_width / 2.0;
     const auto obj_poly =
       tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer);
@@ -707,23 +723,6 @@ lanelet::ConstLanelets getTargetLanelets(
   return target_lanelets;
 }
 
-bool isTargetObjectType(
-  const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
-{
-  using autoware_auto_perception_msgs::msg::ObjectClassification;
-  const auto t = util::getHighestProbLabel(object.classification);
-  const auto is_object_type =
-    ((t == ObjectClassification::CAR && parameters->avoid_car) ||
-     (t == ObjectClassification::TRUCK && parameters->avoid_truck) ||
-     (t == ObjectClassification::BUS && parameters->avoid_bus) ||
-     (t == ObjectClassification::TRAILER && parameters->avoid_trailer) ||
-     (t == ObjectClassification::UNKNOWN && parameters->avoid_unknown) ||
-     (t == ObjectClassification::BICYCLE && parameters->avoid_bicycle) ||
-     (t == ObjectClassification::MOTORCYCLE && parameters->avoid_motorcycle) ||
-     (t == ObjectClassification::PEDESTRIAN && parameters->avoid_pedestrian));
-  return is_object_type;
-}
-
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   boost::optional<Pose> & p_out)
@@ -761,6 +760,10 @@ void fillObjectEnvelopePolygon(
 {
   using boost::geometry::within;
 
+  const auto t = util::getHighestProbLabel(object_data.object.classification);
+  const auto object_parameter = parameters->object_parameters.at(t);
+  const auto & envelope_buffer_margin = object_parameter.envelope_buffer_margin;
+
   const auto id = object_data.object.object_id;
   const auto same_id_obj = std::find_if(
     registered_objects.begin(), registered_objects.end(),
@@ -768,7 +771,7 @@ void fillObjectEnvelopePolygon(
 
   if (same_id_obj == registered_objects.end()) {
     object_data.envelope_poly =
-      createEnvelopePolygon(object_data, closest_pose, parameters->object_envelope_buffer);
+      createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);
     return;
   }
 
@@ -776,7 +779,7 @@ void fillObjectEnvelopePolygon(
 
   if (!within(object_polygon, same_id_obj->envelope_poly)) {
     object_data.envelope_poly =
-      createEnvelopePolygon(object_data, closest_pose, parameters->object_envelope_buffer);
+      createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);
     return;
   }
 
@@ -942,6 +945,9 @@ void filterTargetObjects(
     const auto object_closest_index = findNearestIndex(path_points, object_pose.position);
     const auto object_closest_pose = path_points.at(object_closest_index).point.pose;
 
+    const auto t = util::getHighestProbLabel(o.object.classification);
+    const auto object_parameter = parameters->object_parameters.at(t);
+
     if (!isTargetObjectType(o.object, parameters)) {
       data.other_objects.push_back(o);
       continue;
@@ -1007,10 +1013,10 @@ void filterTargetObjects(
 
     // calculate avoid_margin dynamically
     // NOTE: This calculation must be after calculating to_road_shoulder_distance.
-    const double max_avoid_margin = parameters->lateral_collision_safety_buffer +
+    const double max_avoid_margin = object_parameter.safety_buffer_lateral +
                                     parameters->lateral_collision_margin + 0.5 * vehicle_width;
     const double min_safety_lateral_distance =
-      parameters->lateral_collision_safety_buffer + 0.5 * vehicle_width;
+      object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;
     const auto max_allowable_lateral_distance =
       o.to_road_shoulder_distance - parameters->road_shoulder_safety_margin - 0.5 * vehicle_width;
 

From 3d4502564d6b858a745f69b5defccfe8a3a5fcf5 Mon Sep 17 00:00:00 2001
From: Takamasa Horibe <horibe.takamasa@gmail.com>
Date: Thu, 13 Apr 2023 23:12:09 +0900
Subject: [PATCH 12/15] chore(planning_test_utils): add maintainer (#3394)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
---
 planning/planning_test_utils/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml
index 68ed495cd4372..8a851486125eb 100644
--- a/planning/planning_test_utils/package.xml
+++ b/planning/planning_test_utils/package.xml
@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>ROS2 node for testing interface of the nodes in planning module</description>
   <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
+  <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</author>

From 44c134b0929612f143088b4e7cd0575d4c714987 Mon Sep 17 00:00:00 2001
From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>
Date: Thu, 13 Apr 2023 23:31:34 +0900
Subject: [PATCH 13/15] refactor(behavior_path_planner): change util to utils
 (#3395)

Signed-off-by: yutaka <purewater0901@gmail.com>
---
 planning/behavior_path_planner/CMakeLists.txt | 42 +++++++++----------
 ...ior_path_planner_path_generation_design.md |  2 +-
 .../behavior_path_planner_node.hpp            | 14 +++----
 .../behavior_path_planner/data_manager.hpp    |  2 +-
 .../marker_util/avoidance/debug.hpp           |  2 +-
 .../marker_util/debug_utilities.hpp           |  2 +-
 .../marker_util/lane_change/debug.hpp         |  2 +-
 .../behavior_path_planner/planner_manager.hpp |  2 +-
 .../avoidance/avoidance_module.hpp            |  2 +-
 .../scene_module/avoidance/manager.hpp        |  2 +-
 .../scene_module/avoidance_by_lc/manager.hpp  |  2 +-
 .../scene_module/avoidance_by_lc/module.hpp   |  8 ++--
 .../external_request_lane_change_module.hpp   |  4 +-
 .../lane_change/lane_change_module.hpp        |  6 +--
 .../lane_following/lane_following_module.hpp  |  2 +-
 .../scene_module/pull_out/pull_out_module.hpp | 12 +++---
 .../pull_over/pull_over_module.hpp            | 14 +++----
 .../scene_module/scene_module_interface.hpp   |  2 +-
 .../side_shift/side_shift_module.hpp          |  4 +-
 .../avoidance/avoidance_module_data.hpp       |  8 ++--
 .../{util => utils}/avoidance/util.hpp        |  8 ++--
 .../avoidance_by_lc/module_data.hpp           | 10 ++---
 .../create_vehicle_footprint.hpp              |  6 +--
 .../drivable_area_expansion.hpp               | 10 ++---
 .../drivable_area_expansion/expansion.hpp     | 10 ++---
 .../drivable_area_expansion/footprints.hpp    | 10 ++---
 .../drivable_area_expansion/map_utils.hpp     |  8 ++--
 .../drivable_area_expansion/parameters.hpp    |  8 ++--
 .../path_projection.hpp                       |  8 ++--
 .../drivable_area_expansion/types.hpp         |  6 +--
 .../geometric_parallel_parking.hpp            |  6 +--
 .../lane_change/lane_change_module_data.hpp   |  6 +--
 .../lane_change/lane_change_path.hpp          | 10 ++---
 .../{util => utils}/lane_change/util.hpp      | 12 +++---
 .../lane_following/module_data.hpp            |  6 +--
 ...ccupancy_grid_based_collision_detector.hpp |  6 +--
 .../path_shifter/path_shifter.hpp             |  6 +--
 .../{util => utils}/path_utils.hpp            |  8 ++--
 .../pull_out/geometric_pull_out.hpp           | 12 +++---
 .../pull_out/pull_out_parameters.hpp          |  6 +--
 .../pull_out/pull_out_path.hpp                |  8 ++--
 .../pull_out/pull_out_planner_base.hpp        | 12 +++---
 .../pull_out/shift_pull_out.hpp               | 10 ++---
 .../{util => utils}/pull_out/util.hpp         |  6 +--
 .../pull_over/freespace_pull_over.hpp         |  8 ++--
 .../pull_over/geometric_pull_over.hpp         | 12 +++---
 .../pull_over/goal_searcher.hpp               | 10 ++---
 .../pull_over/goal_searcher_base.hpp          |  8 ++--
 .../pull_over/pull_over_parameters.hpp        |  6 +--
 .../pull_over/pull_over_planner_base.hpp      | 10 ++---
 .../pull_over/shift_pull_over.hpp             | 10 ++---
 .../{util => utils}/pull_over/util.hpp        | 10 ++---
 .../{util => utils}/safety_check.hpp          |  8 ++--
 .../side_shift/side_shift_parameters.hpp      |  6 +--
 .../{util => utils}/side_shift/util.hpp       |  6 +--
 .../{util => utils}/utils.hpp                 | 10 ++---
 .../src/behavior_path_planner_node.cpp        |  4 +-
 .../src/behavior_tree_manager.cpp             |  2 +-
 .../src/marker_util/avoidance/debug.cpp       |  4 +-
 .../src/marker_util/debug_utilities.cpp       |  4 +-
 .../src/marker_util/lane_change/debug.cpp     |  2 +-
 .../src/planner_manager.cpp                   |  4 +-
 .../avoidance/avoidance_module.cpp            |  6 +--
 .../scene_module/avoidance_by_lc/module.cpp   |  8 ++--
 .../external_request_lane_change_module.cpp   |  6 +--
 .../lane_change/lane_change_module.cpp        |  6 +--
 .../lane_following/lane_following_module.cpp  |  4 +-
 .../scene_module/pull_out/pull_out_module.cpp |  8 ++--
 .../pull_over/pull_over_module.cpp            | 10 ++---
 .../side_shift/side_shift_module.cpp          |  6 +--
 .../src/turn_signal_decider.cpp               |  2 +-
 .../src/{util => utils}/avoidance/util.cpp    |  8 ++--
 .../drivable_area_expansion.cpp               | 12 +++---
 .../drivable_area_expansion/expansion.cpp     |  6 +--
 .../drivable_area_expansion/footprints.cpp    |  4 +-
 .../drivable_area_expansion/map_utils.cpp     |  4 +-
 .../geometric_parallel_parking.cpp            |  6 +--
 .../src/{util => utils}/lane_change/util.cpp  | 14 +++----
 ...ccupancy_grid_based_collision_detector.cpp |  2 +-
 .../path_shifter/path_shifter.cpp             |  6 +--
 .../src/{util => utils}/path_utils.cpp        |  4 +-
 .../pull_out/geometric_pull_out.cpp           |  6 +--
 .../pull_out/shift_pull_out.cpp               |  8 ++--
 .../src/{util => utils}/pull_out/util.cpp     | 10 ++---
 .../pull_over/freespace_pull_over.cpp         |  6 +--
 .../pull_over/geometric_pull_over.cpp         |  6 +--
 .../pull_over/goal_searcher.cpp               |  6 +--
 .../pull_over/shift_pull_over.cpp             |  6 +--
 .../src/{util => utils}/pull_over/util.cpp    |  6 +--
 .../src/{util => utils}/safety_check.cpp      |  2 +-
 .../src/{util => utils}/side_shift/util.cpp   |  4 +-
 .../src/{util => utils}/utils.cpp             |  4 +-
 planning/behavior_path_planner/test/input.hpp |  2 +-
 .../test/test_avoidance_utils.cpp             |  4 +-
 .../test/test_drivable_area_expansion.cpp     |  6 +--
 .../behavior_path_planner/test/test_utils.cpp |  2 +-
 .../static_centerline_optimizer/src/utils.cpp |  2 +-
 97 files changed, 329 insertions(+), 329 deletions(-)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/avoidance/avoidance_module_data.hpp (98%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/avoidance/util.hpp (94%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/avoidance_by_lc/module_data.hpp (76%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/create_vehicle_footprint.hpp (89%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/drivable_area_expansion/drivable_area_expansion.hpp (83%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/drivable_area_expansion/expansion.hpp (90%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/drivable_area_expansion/footprints.hpp (87%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/drivable_area_expansion/map_utils.hpp (81%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/drivable_area_expansion/parameters.hpp (94%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/drivable_area_expansion/path_projection.hpp (96%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/drivable_area_expansion/types.hpp (88%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/geometric_parallel_parking/geometric_parallel_parking.hpp (94%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/lane_change/lane_change_module_data.hpp (92%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/lane_change/lane_change_path.hpp (81%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/lane_change/util.hpp (95%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/lane_following/module_data.hpp (82%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp (92%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/path_shifter/path_shifter.hpp (96%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/path_utils.hpp (93%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_out/geometric_pull_out.hpp (72%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_out/pull_out_parameters.hpp (89%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_out/pull_out_path.hpp (76%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_out/pull_out_planner_base.hpp (81%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_out/shift_pull_out.hpp (85%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_out/util.hpp (91%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_over/freespace_pull_over.hpp (84%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_over/geometric_pull_over.hpp (82%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_over/goal_searcher.hpp (82%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_over/goal_searcher_base.hpp (87%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_over/pull_over_parameters.hpp (94%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_over/pull_over_planner_base.hpp (89%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_over/shift_pull_over.hpp (84%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/pull_over/util.hpp (89%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/safety_check.hpp (95%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/side_shift/side_shift_parameters.hpp (85%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/side_shift/util.hpp (88%)
 rename planning/behavior_path_planner/include/behavior_path_planner/{util => utils}/utils.hpp (98%)
 rename planning/behavior_path_planner/src/{util => utils}/avoidance/util.cpp (99%)
 rename planning/behavior_path_planner/src/{util => utils}/drivable_area_expansion/drivable_area_expansion.cpp (93%)
 rename planning/behavior_path_planner/src/{util => utils}/drivable_area_expansion/expansion.cpp (97%)
 rename planning/behavior_path_planner/src/{util => utils}/drivable_area_expansion/footprints.cpp (95%)
 rename planning/behavior_path_planner/src/{util => utils}/drivable_area_expansion/map_utils.cpp (91%)
 rename planning/behavior_path_planner/src/{util => utils}/geometric_parallel_parking/geometric_parallel_parking.cpp (99%)
 rename planning/behavior_path_planner/src/{util => utils}/lane_change/util.cpp (99%)
 rename planning/behavior_path_planner/src/{util => utils}/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp (98%)
 rename planning/behavior_path_planner/src/{util => utils}/path_shifter/path_shifter.cpp (99%)
 rename planning/behavior_path_planner/src/{util => utils}/path_utils.cpp (99%)
 rename planning/behavior_path_planner/src/{util => utils}/pull_out/geometric_pull_out.cpp (94%)
 rename planning/behavior_path_planner/src/{util => utils}/pull_out/shift_pull_out.cpp (98%)
 rename planning/behavior_path_planner/src/{util => utils}/pull_out/util.cpp (93%)
 rename planning/behavior_path_planner/src/{util => utils}/pull_over/freespace_pull_over.cpp (96%)
 rename planning/behavior_path_planner/src/{util => utils}/pull_over/geometric_pull_over.cpp (93%)
 rename planning/behavior_path_planner/src/{util => utils}/pull_over/goal_searcher.cpp (98%)
 rename planning/behavior_path_planner/src/{util => utils}/pull_over/shift_pull_over.cpp (98%)
 rename planning/behavior_path_planner/src/{util => utils}/pull_over/util.cpp (96%)
 rename planning/behavior_path_planner/src/{util => utils}/safety_check.cpp (99%)
 rename planning/behavior_path_planner/src/{util => utils}/side_shift/util.cpp (96%)
 rename planning/behavior_path_planner/src/{util => utils}/utils.cpp (99%)

diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
index 4e7b7f9a93957..41735185787a0 100644
--- a/planning/behavior_path_planner/CMakeLists.txt
+++ b/planning/behavior_path_planner/CMakeLists.txt
@@ -21,27 +21,27 @@ set(common_src
   src/scene_module/lane_change/lane_change_module.cpp
   src/scene_module/lane_change/external_request_lane_change_module.cpp
   src/turn_signal_decider.cpp
-  src/util/utils.cpp
-  src/util/path_utils.cpp
-  src/util/safety_check.cpp
-  src/util/avoidance/util.cpp
-  src/util/lane_change/util.cpp
-  src/util/side_shift/util.cpp
-  src/util/pull_over/util.cpp
-  src/util/pull_over/shift_pull_over.cpp
-  src/util/pull_over/geometric_pull_over.cpp
-  src/util/pull_over/freespace_pull_over.cpp
-  src/util/pull_over/goal_searcher.cpp
-  src/util/pull_out/util.cpp
-  src/util/pull_out/shift_pull_out.cpp
-  src/util/pull_out/geometric_pull_out.cpp
-  src/util/path_shifter/path_shifter.cpp
-  src/util/drivable_area_expansion/drivable_area_expansion.cpp
-  src/util/drivable_area_expansion/map_utils.cpp
-  src/util/drivable_area_expansion/footprints.cpp
-  src/util/drivable_area_expansion/expansion.cpp
-  src/util/geometric_parallel_parking/geometric_parallel_parking.cpp
-  src/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
+  src/utils/utils.cpp
+  src/utils/path_utils.cpp
+  src/utils/safety_check.cpp
+  src/utils/avoidance/util.cpp
+  src/utils/lane_change/util.cpp
+  src/utils/side_shift/util.cpp
+  src/utils/pull_over/util.cpp
+  src/utils/pull_over/shift_pull_over.cpp
+  src/utils/pull_over/geometric_pull_over.cpp
+  src/utils/pull_over/freespace_pull_over.cpp
+  src/utils/pull_over/goal_searcher.cpp
+  src/utils/pull_out/util.cpp
+  src/utils/pull_out/shift_pull_out.cpp
+  src/utils/pull_out/geometric_pull_out.cpp
+  src/utils/path_shifter/path_shifter.cpp
+  src/utils/drivable_area_expansion/drivable_area_expansion.cpp
+  src/utils/drivable_area_expansion/map_utils.cpp
+  src/utils/drivable_area_expansion/footprints.cpp
+  src/utils/drivable_area_expansion/expansion.cpp
+  src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp
+  src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
   src/marker_util/debug_utilities.cpp
   src/marker_util/avoidance/debug.cpp
   src/marker_util/lane_change/debug.cpp
diff --git a/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md b/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md
index 9a42a44fc4900..2a439c1d28182 100644
--- a/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md
+++ b/planning/behavior_path_planner/behavior_path_planner_path_generation_design.md
@@ -1,6 +1,6 @@
 # Path Generation design
 
-This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp).
+This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp).
 
 ## Overview
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
index 968f998bd47e3..562a71fb00f6b 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -39,13 +39,13 @@
 
 #include "behavior_path_planner/steering_factor_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
-#include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
-#include "behavior_path_planner/util/avoidance_by_lc/module_data.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
-#include "behavior_path_planner/util/lane_following/module_data.hpp"
-#include "behavior_path_planner/util/pull_out/pull_out_parameters.hpp"
-#include "behavior_path_planner/util/pull_over/pull_over_parameters.hpp"
-#include "behavior_path_planner/util/side_shift/side_shift_parameters.hpp"
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/lane_following/module_data.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_parameters.hpp"
+#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp"
+#include "behavior_path_planner/utils/side_shift/side_shift_parameters.hpp"
 
 #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp"
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
index c11662d5125d8..2111760083358 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
@@ -16,7 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__DATA_MANAGER_HPP_
 
 #include "behavior_path_planner/parameters.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/parameters.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp
index 157098685aff3..51de1d2d6318d 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp
@@ -16,7 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__MARKER_UTIL__AVOIDANCE__DEBUG_HPP_
 
 #include "behavior_path_planner/marker_util/debug_utilities.hpp"
-#include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
 
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp
index d9203e63b9875..aab2d5353c34a 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp
@@ -14,7 +14,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_
 #define BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_
 
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/lane_change/debug.hpp
index 769de28470de6..ce91f1434f3f0 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/lane_change/debug.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/lane_change/debug.hpp
@@ -16,7 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__MARKER_UTIL__LANE_CHANGE__DEBUG_HPP_
 
 #include "behavior_path_planner/marker_util/debug_utilities.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_path.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
 
 #include <string>
 #include <unordered_map>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
index 803464312d6db..04fb694813026 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
@@ -17,7 +17,7 @@
 
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
-#include "behavior_path_planner/util/lane_following/module_data.hpp"
+#include "behavior_path_planner/utils/lane_following/module_data.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
index d3d7687d94803..b2c475e32f188 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -17,7 +17,7 @@
 
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
-#include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp
index c04ce131467b5..517057f8d6ac8 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp
@@ -17,7 +17,7 @@
 
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
 #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
-#include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp
index 62c58fb9f8151..4727275a6d4ec 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp
@@ -17,7 +17,7 @@
 
 #include "behavior_path_planner/scene_module/avoidance_by_lc/module.hpp"
 #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
-#include "behavior_path_planner/util/avoidance_by_lc/module_data.hpp"
+#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp
index 9776305ab79c9..a8acd932742d8 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp
@@ -18,10 +18,10 @@
 #include "behavior_path_planner/marker_util/lane_change/debug.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
-#include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
-#include "behavior_path_planner/util/avoidance_by_lc/module_data.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_path.hpp"
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp
index 363881aabdb45..cfc15de05de5c 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp
@@ -18,8 +18,8 @@
 #include "behavior_path_planner/marker_util/lane_change/debug.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_path.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
index 543f141a54c9d..932e2cd03e5b1 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
@@ -18,9 +18,9 @@
 #include "behavior_path_planner/marker_util/lane_change/debug.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_path.hpp"
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp
index 8490926d9327e..50dc859939d00 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp
@@ -16,7 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_FOLLOWING__LANE_FOLLOWING_MODULE_HPP_
 
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
-#include "behavior_path_planner/util/lane_following/module_data.hpp"
+#include "behavior_path_planner/utils/lane_following/module_data.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
index 4e7ef7450ce2d..78cc37b0a9d68 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
@@ -16,12 +16,12 @@
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_
 
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
-#include "behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp"
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
-#include "behavior_path_planner/util/pull_out/geometric_pull_out.hpp"
-#include "behavior_path_planner/util/pull_out/pull_out_parameters.hpp"
-#include "behavior_path_planner/util/pull_out/pull_out_path.hpp"
-#include "behavior_path_planner/util/pull_out/shift_pull_out.hpp"
+#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/pull_out/geometric_pull_out.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_parameters.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_path.hpp"
+#include "behavior_path_planner/utils/pull_out/shift_pull_out.hpp"
 
 #include <lane_departure_checker/lane_departure_checker.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
index bed4751199af6..799b38166e5a4 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
@@ -16,13 +16,13 @@
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_
 
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
-#include "behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp"
-#include "behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
-#include "behavior_path_planner/util/pull_over/freespace_pull_over.hpp"
-#include "behavior_path_planner/util/pull_over/geometric_pull_over.hpp"
-#include "behavior_path_planner/util/pull_over/goal_searcher.hpp"
-#include "behavior_path_planner/util/pull_over/pull_over_parameters.hpp"
-#include "behavior_path_planner/util/pull_over/shift_pull_over.hpp"
+#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
+#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+#include "behavior_path_planner/utils/pull_over/freespace_pull_over.hpp"
+#include "behavior_path_planner/utils/pull_over/geometric_pull_over.hpp"
+#include "behavior_path_planner/utils/pull_over/goal_searcher.hpp"
+#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp"
+#include "behavior_path_planner/utils/pull_over/shift_pull_over.hpp"
 
 #include <freespace_planning_algorithms/astar_search.hpp>
 #include <freespace_planning_algorithms/rrtstar.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
index 2612201ed7b15..063e5d215ea23 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
@@ -18,7 +18,7 @@
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/module_status.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <behavior_path_planner/steering_factor_interface.hpp>
 #include <behavior_path_planner/turn_signal_decider.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp
index ec7e7db692aab..20dfbbff7310a 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp
@@ -16,8 +16,8 @@
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_
 
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
-#include "behavior_path_planner/util/side_shift/side_shift_parameters.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/side_shift/side_shift_parameters.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
similarity index 98%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
index 550bb12f62bc7..aa2eef71384fc 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
 
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
@@ -526,4 +526,4 @@ struct DebugData
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/util.hpp
similarity index 94%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/util.hpp
index cff9f2b507790..f61f4ee243b38 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/util.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE__UTIL_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE__UTIL_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTIL_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTIL_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
-#include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
 
 #include <memory>
 #include <string>
@@ -130,4 +130,4 @@ void filterTargetObjects(
   const std::shared_ptr<AvoidanceParameters> & parameters);
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE__UTIL_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTIL_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance_by_lc/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp
similarity index 76%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/avoidance_by_lc/module_data.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp
index 4608dc5ff35c4..e556c56eac8d7 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance_by_lc/module_data.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp
@@ -11,11 +11,11 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE_BY_LC__MODULE_DATA_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE_BY_LC__MODULE_DATA_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_
 
-#include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
 
 #include <memory>
 
@@ -42,4 +42,4 @@ struct AvoidanceByLCParameters
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE_BY_LC__MODULE_DATA_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/create_vehicle_footprint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/create_vehicle_footprint.hpp
similarity index 89%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/create_vehicle_footprint.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/create_vehicle_footprint.hpp
index 06f6ac44a1071..a4f71305c3fbf 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/create_vehicle_footprint.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/create_vehicle_footprint.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_
 
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
@@ -44,4 +44,4 @@ inline tier4_autoware_utils::LinearRing2d createVehicleFootprint(
   return footprint;
 }
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp
similarity index 83%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/drivable_area_expansion.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp
index bbec1b7e841e5..eadfb72a42afa 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/drivable_area_expansion.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_
 
-#include "behavior_path_planner/util/drivable_area_expansion/parameters.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/types.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 
 #include <route_handler/route_handler.hpp>
 
@@ -49,4 +49,4 @@ polygon_t createExpandedDrivableAreaPolygon(
 void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area);
 }  // namespace drivable_area_expansion
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp
similarity index 90%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/expansion.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp
index cc0f748e76e3f..33bccf90ffbe8 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/expansion.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_
 
-#include "behavior_path_planner/util/drivable_area_expansion/parameters.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/types.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 
 #include <route_handler/route_handler.hpp>
 
@@ -82,4 +82,4 @@ multipolygon_t createExpansionLaneletPolygons(
   const DrivableAreaExpansionParameters & params);
 }  // namespace drivable_area_expansion
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp
similarity index 87%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/footprints.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp
index bc1ede877dc71..25f061affaa48 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/footprints.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_
 
-#include "behavior_path_planner/util/drivable_area_expansion/parameters.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/types.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
 
@@ -66,4 +66,4 @@ multipolygon_t createObjectFootprints(
 multipolygon_t createPathFootprints(
   const PathWithLaneId & path, const DrivableAreaExpansionParameters & params);
 }  // namespace drivable_area_expansion
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp
similarity index 81%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/map_utils.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp
index e60b49d60e5fa..33c4cc8a0eff3 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/map_utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_
 
-#include "behavior_path_planner/util/drivable_area_expansion/types.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 
 #include <lanelet2_core/LaneletMap.h>
 
@@ -38,4 +38,4 @@ multilinestring_t extractUncrossableLines(
 bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector<std::string> & types);
 }  // namespace drivable_area_expansion
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp
similarity index 94%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/parameters.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp
index 0a85ad9feae13..b8d8a235580e4 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_
 
-#include "behavior_path_planner/util/drivable_area_expansion/types.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 
 #include <rclcpp/node.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
@@ -116,4 +116,4 @@ struct DrivableAreaExpansionParameters
 };
 
 }  // namespace drivable_area_expansion
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp
similarity index 96%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/path_projection.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp
index 77069d43c2f72..3e2b177f59167 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/path_projection.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_
 
-#include "behavior_path_planner/util/drivable_area_expansion/types.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 
 #include <interpolation/linear_interpolation.hpp>
 
@@ -191,4 +191,4 @@ inline linestring_t sub_linestring(
 }
 }  // namespace drivable_area_expansion
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp
similarity index 88%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/types.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp
index f269c42c0e8e8..e56ef4961589d 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/drivable_area_expansion/types.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__TYPES_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__TYPES_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_
 
 #include "tier4_autoware_utils/geometry/boost_geometry.hpp"
 
@@ -50,4 +50,4 @@ struct Projection
   double arc_length;
 };
 }  // namespace drivable_area_expansion
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__DRIVABLE_AREA_EXPANSION__TYPES_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
similarity index 94%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
index 90102cb69e053..b5cc03897e2fd 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/parameters.hpp"
@@ -129,4 +129,4 @@ class GeometricParallelParking
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
similarity index 92%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
index ee4b073c8f7e7..1481e0bd3789d 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
@@ -11,8 +11,8 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
 
 #include "lanelet2_core/geometry/Lanelet.h"
 
@@ -94,4 +94,4 @@ struct LaneChangeTargetObjectIndices
 enum class LaneChangeModuleType { NORMAL = 0, EXTERNAL_REQUEST };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp
similarity index 81%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_path.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp
index 500550bd48a8a..4ebcc35641ba3 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_path.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
 
 #include "behavior_path_planner/turn_signal_decider.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
@@ -56,4 +56,4 @@ struct LaneChangeStatus
 };
 
 }  // namespace behavior_path_planner
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/util.hpp
similarity index 95%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/util.hpp
index fa604a259022a..c8020ec801ef1 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/util.hpp
@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__UTIL_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__UTIL_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTIL_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTIL_HPP_
 
 #include "behavior_path_planner/marker_util/lane_change/debug.hpp"
 #include "behavior_path_planner/parameters.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_path.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <route_handler/route_handler.hpp>
 
@@ -182,4 +182,4 @@ lanelet::ConstLanelets getLaneChangeLanes(
   const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length,
   const double prepare_duration, const Direction direction, const LaneChangeModuleType type);
 }  // namespace behavior_path_planner::util::lane_change
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__UTIL_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTIL_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp
similarity index 82%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp
index ed28007d7b913..d4eab4835dddb 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__LANE_FOLLOWING__MODULE_DATA_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__LANE_FOLLOWING__MODULE_DATA_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_
 
 #include <string>
 #include <vector>
@@ -36,4 +36,4 @@ struct LaneFollowingParameters
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__LANE_FOLLOWING__MODULE_DATA_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp
similarity index 92%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp
index 8364c0bd7cc09..91d99fcd4a0d6 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
 
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 
@@ -149,4 +149,4 @@ class OccupancyGridBasedCollisionDetector
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp
similarity index 96%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp
index a6c8ae67cecf0..b5b1e5af5c328 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PATH_SHIFTER__PATH_SHIFTER_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PATH_SHIFTER__PATH_SHIFTER_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_
 
 #include "behavior_path_planner/parameters.hpp"
 
@@ -225,4 +225,4 @@ class PathShifter
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PATH_SHIFTER__PATH_SHIFTER_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp
similarity index 93%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/path_utils.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp
index 2c488ccb52d14..ec20f594bedc8 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/path_utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PATH_UTILS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PATH_UTILS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_
 
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
 #include <behavior_path_planner/parameters.hpp>
 #include <freespace_planning_algorithms/abstract_algorithm.hpp>
@@ -94,4 +94,4 @@ std::vector<Pose> interpolatePose(
 
 }  // namespace behavior_path_planner::util
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PATH_UTILS_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp
similarity index 72%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/geometric_pull_out.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp
index ae5d15fce969b..788b37448c4a8 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/geometric_pull_out.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_
 
-#include "behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp"
-#include "behavior_path_planner/util/pull_out/pull_out_path.hpp"
-#include "behavior_path_planner/util/pull_out/pull_out_planner_base.hpp"
+#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_path.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
@@ -38,4 +38,4 @@ class GeometricPullOut : public PullOutPlannerBase
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp
similarity index 89%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/pull_out_parameters.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp
index 1d9784b529bec..49a72d4b79524 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/pull_out_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp
@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__PULL_OUT_PARAMETERS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__PULL_OUT_PARAMETERS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PARAMETERS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PARAMETERS_HPP_
 
 #include <string>
 #include <vector>
@@ -59,4 +59,4 @@ struct PullOutParameters
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__PULL_OUT_PARAMETERS_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PARAMETERS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_path.hpp
similarity index 76%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/pull_out_path.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_path.hpp
index adf7cee3630b6..5d6c5c340ef66 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/pull_out_path.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_path.hpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__PULL_OUT_PATH_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__PULL_OUT_PATH_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PATH_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PATH_HPP_
 
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
@@ -31,4 +31,4 @@ struct PullOutPath
   Pose end_pose;
 };
 }  // namespace behavior_path_planner
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__PULL_OUT_PATH_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PATH_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp
similarity index 81%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/pull_out_planner_base.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp
index 0a9304fe33e7f..955e856061310 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/pull_out_planner_base.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp
@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/parameters.hpp"
-#include "behavior_path_planner/util/create_vehicle_footprint.hpp"
-#include "behavior_path_planner/util/pull_out/pull_out_parameters.hpp"
-#include "behavior_path_planner/util/pull_out/pull_out_path.hpp"
+#include "behavior_path_planner/utils/create_vehicle_footprint.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_parameters.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_path.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -68,4 +68,4 @@ class PullOutPlannerBase
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/shift_pull_out.hpp
similarity index 85%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/shift_pull_out.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/shift_pull_out.hpp
index 4126d30bb72dc..ac5aa89ea675b 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/shift_pull_out.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/shift_pull_out.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__SHIFT_PULL_OUT_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__SHIFT_PULL_OUT_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__SHIFT_PULL_OUT_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__SHIFT_PULL_OUT_HPP_
 
-#include "behavior_path_planner/util/pull_out/pull_out_path.hpp"
-#include "behavior_path_planner/util/pull_out/pull_out_planner_base.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_path.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp"
 
 #include <lane_departure_checker/lane_departure_checker.hpp>
 
@@ -56,4 +56,4 @@ class ShiftPullOut : public PullOutPlannerBase
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__SHIFT_PULL_OUT_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__SHIFT_PULL_OUT_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/util.hpp
similarity index 91%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/util.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/util.hpp
index 676946a999697..314a4c9426cab 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_out/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/util.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__UTIL_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__UTIL_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__UTIL_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__UTIL_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
 
@@ -48,4 +48,4 @@ Pose getBackedPose(
   const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance);
 }  // namespace behavior_path_planner::pull_out_utils
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OUT__UTIL_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__UTIL_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/freespace_pull_over.hpp
similarity index 84%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/freespace_pull_over.hpp
index 6f5acfd30ef2b..d8f671163722e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/freespace_pull_over.hpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__FREESPACE_PULL_OVER_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__FREESPACE_PULL_OVER_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FREESPACE_PULL_OVER_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FREESPACE_PULL_OVER_HPP_
 
-#include "behavior_path_planner/util/pull_over/pull_over_planner_base.hpp"
+#include "behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp"
 
 #include <freespace_planning_algorithms/abstract_algorithm.hpp>
 #include <freespace_planning_algorithms/astar_search.hpp>
@@ -50,4 +50,4 @@ class FreespacePullOver : public PullOverPlannerBase
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__FREESPACE_PULL_OVER_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FREESPACE_PULL_OVER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/geometric_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/geometric_pull_over.hpp
similarity index 82%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/geometric_pull_over.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/geometric_pull_over.hpp
index c54d7d52de2b3..8a47a2a696ebd 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/geometric_pull_over.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/geometric_pull_over.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_
 
-#include "behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp"
-#include "behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
-#include "behavior_path_planner/util/pull_over/pull_over_planner_base.hpp"
+#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
+#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+#include "behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp"
 
 #include <lane_departure_checker/lane_departure_checker.hpp>
 
@@ -69,4 +69,4 @@ class GeometricPullOver : public PullOverPlannerBase
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher.hpp
similarity index 82%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/goal_searcher.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher.hpp
index 0f50fd4f134fe..36abe47a3ebe6 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/goal_searcher.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__GOAL_SEARCHER_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__GOAL_SEARCHER_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_HPP_
 
-#include "behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
-#include "behavior_path_planner/util/pull_over/goal_searcher_base.hpp"
+#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+#include "behavior_path_planner/utils/pull_over/goal_searcher_base.hpp"
 
 #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp"
 
@@ -52,4 +52,4 @@ class GoalSearcher : public GoalSearcherBase
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__GOAL_SEARCHER_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher_base.hpp
similarity index 87%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/goal_searcher_base.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher_base.hpp
index ab57cd7272e73..8bdbca45f4bb5 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/goal_searcher_base.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher_base.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__GOAL_SEARCHER_BASE_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__GOAL_SEARCHER_BASE_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_BASE_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_BASE_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/parameters.hpp"
-#include "behavior_path_planner/util/pull_over/pull_over_parameters.hpp"
+#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp"
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
@@ -73,4 +73,4 @@ class GoalSearcherBase
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__GOAL_SEARCHER_BASE_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_BASE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp
similarity index 94%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp
index 15a2c17149d0c..279c0b143861e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp
@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__PULL_OVER_PARAMETERS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__PULL_OVER_PARAMETERS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PARAMETERS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PARAMETERS_HPP_
 
 #include <freespace_planning_algorithms/abstract_algorithm.hpp>
 #include <freespace_planning_algorithms/astar_search.hpp>
@@ -111,4 +111,4 @@ struct PullOverParameters
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__PULL_OVER_PARAMETERS_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PARAMETERS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp
similarity index 89%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_planner_base.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp
index e15a58b8eda37..dbf50363676f1 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_planner_base.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/parameters.hpp"
-#include "behavior_path_planner/util/create_vehicle_footprint.hpp"
-#include "behavior_path_planner/util/pull_over/pull_over_parameters.hpp"
+#include "behavior_path_planner/utils/create_vehicle_footprint.hpp"
+#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -111,4 +111,4 @@ class PullOverPlannerBase
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/shift_pull_over.hpp
similarity index 84%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/shift_pull_over.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/shift_pull_over.hpp
index 227fc6442f20c..d42eb7f656bf9 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/shift_pull_over.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/shift_pull_over.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__SHIFT_PULL_OVER_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__SHIFT_PULL_OVER_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__SHIFT_PULL_OVER_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__SHIFT_PULL_OVER_HPP_
 
-#include "behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
-#include "behavior_path_planner/util/pull_over/pull_over_planner_base.hpp"
+#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+#include "behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp"
 
 #include <lane_departure_checker/lane_departure_checker.hpp>
 
@@ -61,4 +61,4 @@ class ShiftPullOver : public PullOverPlannerBase
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__SHIFT_PULL_OVER_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__SHIFT_PULL_OVER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/util.hpp
similarity index 89%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/util.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/util.hpp
index 9a6d528ddfe4c..21cfa8f8e0cfb 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/util.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__UTIL_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__UTIL_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__UTIL_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__UTIL_HPP_
 
-#include "behavior_path_planner/util/pull_over/goal_searcher_base.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/pull_over/goal_searcher_base.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lane_departure_checker/lane_departure_checker.hpp>
 
@@ -64,4 +64,4 @@ MarkerArray createGoalCandidatesMarkerArray(
 }  // namespace pull_over_utils
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__UTIL_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__UTIL_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp
similarity index 95%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/safety_check.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp
index 02e11f656bd80..07d056da9a96b 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/safety_check.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__SAFETY_CHECK_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__SAFETY_CHECK_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SAFETY_CHECK_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__SAFETY_CHECK_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/marker_util/debug_utilities.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
 
 #include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 
@@ -128,4 +128,4 @@ bool isSafeInFreeSpaceCollisionCheck(
 
 }  // namespace behavior_path_planner::util::safety_check
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__SAFETY_CHECK_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__SAFETY_CHECK_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/side_shift_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp
similarity index 85%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/side_shift_parameters.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp
index 5a9edd1279f85..ee139b0774803 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/side_shift_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_
 
 #include <tier4_planning_msgs/msg/lateral_offset.hpp>
 
@@ -44,4 +44,4 @@ struct SideShiftParameters
 };
 
 }  // namespace behavior_path_planner
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp
similarity index 88%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/util.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp
index 4f725f235be30..65d79a2a4977b 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/side_shift/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__SIDE_SHIFT__UTIL_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__SIDE_SHIFT__UTIL_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/point.hpp>
@@ -44,4 +44,4 @@ Point transformToGrid(
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__SIDE_SHIFT__UTIL_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
similarity index 98%
rename from planning/behavior_path_planner/include/behavior_path_planner/util/utils.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
index 55f4f1292c17c..1d20143abfa39 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/util/utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTIL__UTILS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTIL__UTILS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/marker_util/debug_utilities.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
-#include "behavior_path_planner/util/pull_out/pull_out_path.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/pull_out/pull_out_path.hpp"
 #include "motion_utils/motion_utils.hpp"
 #include "perception_utils/predicted_path_utils.hpp"
 
@@ -350,4 +350,4 @@ lanelet::ConstLanelets getLaneletsFromPath(
 std::string convertToSnakeCase(const std::string & input_str);
 }  // namespace behavior_path_planner::util
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTIL__UTILS_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index c1f9a98f7db29..11331cc4958eb 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -15,8 +15,8 @@
 #include "behavior_path_planner/behavior_path_planner_node.hpp"
 
 #include "behavior_path_planner/marker_util/debug_utilities.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/map_utils.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
 
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp
index c3f17563d0877..a4214f816a86a 100644
--- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp
+++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp
@@ -17,7 +17,7 @@
 #include "behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <algorithm>
 #include <memory>
diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp
index 0e9fdeaec428e..7b5309c1d622a 100644
--- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp
+++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp
@@ -14,8 +14,8 @@
 
 #include "behavior_path_planner/marker_util/avoidance/debug.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <tf2/utils.h>
 
diff --git a/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp b/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp
index 8ec72388ff05a..181a71556f547 100644
--- a/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp
+++ b/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp
@@ -14,8 +14,8 @@
 
 #include "behavior_path_planner/marker_util/debug_utilities.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
diff --git a/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp
index 8a8eeb526eb15..ed43324ef6a40 100644
--- a/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp
+++ b/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
 #include <behavior_path_planner/marker_util/lane_change/debug.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
index 63352cfb9fee7..add019d6bcbc2 100644
--- a/planning/behavior_path_planner/src/planner_manager.cpp
+++ b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -14,8 +14,8 @@
 
 #include "behavior_path_planner/planner_manager.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
index f54955c20acae..75e1a81febdaa 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -16,9 +16,9 @@
 
 #include "behavior_path_planner/marker_util/avoidance/debug.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
-#include "behavior_path_planner/util/avoidance/util.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/avoidance/util.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp
index e715f9d589403..da86b77fefbf1 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp
@@ -17,10 +17,10 @@
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
-#include "behavior_path_planner/util/avoidance/util.hpp"
-#include "behavior_path_planner/util/lane_change/util.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/avoidance/util.hpp"
+#include "behavior_path_planner/utils/lane_change/util.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp
index 365b014c0e728..a1b6bf5a7f5e5 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp
@@ -16,9 +16,9 @@
 
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
-#include "behavior_path_planner/util/lane_change/util.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/lane_change/util.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
index 6ebd7edaccf38..5228739ad0ab3 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
@@ -16,9 +16,9 @@
 
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
-#include "behavior_path_planner/util/lane_change/util.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/lane_change/util.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp
index b7134a3a3f8c5..cf80d853d0915 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp
@@ -14,8 +14,8 @@
 
 #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 
diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
index 03bbb1628588c..ae217800ff404 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
@@ -14,10 +14,10 @@
 
 #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
 
-#include "behavior_path_planner/util/create_vehicle_footprint.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/pull_out/util.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/create_vehicle_footprint.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/pull_out/util.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
index 97e6069f7d1d5..286f4d15eb685 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
@@ -14,11 +14,11 @@
 
 #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp"
 
-#include "behavior_path_planner/util/create_vehicle_footprint.hpp"
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/pull_over/util.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/create_vehicle_footprint.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/pull_over/util.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp
index 0dc99349a85e7..c90aea06a65f1 100644
--- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp
@@ -14,9 +14,9 @@
 
 #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/side_shift/util.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/side_shift/util.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 
diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp
index 8eec32bdd3f9b..14fbc3673a53a 100644
--- a/planning/behavior_path_planner/src/turn_signal_decider.cpp
+++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp
@@ -14,7 +14,7 @@
 
 #include "behavior_path_planner/turn_signal_decider.hpp"
 
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/util/avoidance/util.cpp b/planning/behavior_path_planner/src/utils/avoidance/util.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/util/avoidance/util.cpp
rename to planning/behavior_path_planner/src/utils/avoidance/util.cpp
index 8d4142e430024..94f7688a1f13d 100644
--- a/planning/behavior_path_planner/src/util/avoidance/util.cpp
+++ b/planning/behavior_path_planner/src/utils/avoidance/util.cpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/avoidance/util.hpp"
+#include "behavior_path_planner/utils/avoidance/util.hpp"
 
-#include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <autoware_auto_tf2/tf2_autoware_auto_msgs.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/util/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
similarity index 93%
rename from planning/behavior_path_planner/src/util/drivable_area_expansion/drivable_area_expansion.cpp
rename to planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
index acaf845da7feb..a27010e4f8d15 100644
--- a/planning/behavior_path_planner/src/util/drivable_area_expansion/drivable_area_expansion.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/drivable_area_expansion/drivable_area_expansion.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp"
 
-#include "behavior_path_planner/util/drivable_area_expansion/expansion.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/footprints.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/map_utils.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/parameters.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/types.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 
 #include <boost/geometry.hpp>
 
diff --git a/planning/behavior_path_planner/src/util/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp
similarity index 97%
rename from planning/behavior_path_planner/src/util/drivable_area_expansion/expansion.cpp
rename to planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp
index 659c66f4ca13e..4bdbdedf44c77 100644
--- a/planning/behavior_path_planner/src/util/drivable_area_expansion/expansion.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/drivable_area_expansion/expansion.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp"
 
-#include "behavior_path_planner/util/drivable_area_expansion/map_utils.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/path_projection.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp"
 
 namespace drivable_area_expansion
 {
diff --git a/planning/behavior_path_planner/src/util/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp
similarity index 95%
rename from planning/behavior_path_planner/src/util/drivable_area_expansion/footprints.cpp
rename to planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp
index f6b9a187c02b3..77c8b7faa27eb 100644
--- a/planning/behavior_path_planner/src/util/drivable_area_expansion/footprints.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp
@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/drivable_area_expansion/footprints.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp"
 
-#include "behavior_path_planner/util/drivable_area_expansion/parameters.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
 
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
diff --git a/planning/behavior_path_planner/src/util/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp
similarity index 91%
rename from planning/behavior_path_planner/src/util/drivable_area_expansion/map_utils.cpp
rename to planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp
index 17cfa9018a298..39a69fbd74914 100644
--- a/planning/behavior_path_planner/src/util/drivable_area_expansion/map_utils.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp
@@ -13,9 +13,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/drivable_area_expansion/map_utils.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp"
 
-#include "behavior_path_planner/util/drivable_area_expansion/types.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 #include "lanelet2_core/primitives/LineString.h"
 
 #include <boost/geometry.hpp>
diff --git a/planning/behavior_path_planner/src/util/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/util/geometric_parallel_parking/geometric_parallel_parking.cpp
rename to planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp
index 76c9ec1bc6a84..1e5cb6ba97fcd 100644
--- a/planning/behavior_path_planner/src/util/geometric_parallel_parking/geometric_parallel_parking.cpp
+++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp"
+#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 
 #include <interpolation/spline_interpolation.hpp>
diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/utils/lane_change/util.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/util/lane_change/util.cpp
rename to planning/behavior_path_planner/src/utils/lane_change/util.cpp
index b38dc17a56371..43c13dfca2491 100644
--- a/planning/behavior_path_planner/src/util/lane_change/util.cpp
+++ b/planning/behavior_path_planner/src/utils/lane_change/util.cpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/lane_change/util.hpp"
+#include "behavior_path_planner/utils/lane_change/util.hpp"
 
 #include "behavior_path_planner/parameters.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
-#include "behavior_path_planner/util/lane_change/lane_change_path.hpp"
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/safety_check.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/safety_check.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
diff --git a/planning/behavior_path_planner/src/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
similarity index 98%
rename from planning/behavior_path_planner/src/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
rename to planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
index 527e738ef41f1..d244d08abf99b 100644
--- a/planning/behavior_path_planner/src/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
+++ b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
 
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
diff --git a/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp
rename to planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
index b77e1689f4e44..113382a510c05 100644
--- a/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp
+++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <interpolation/spline_interpolation.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/util/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/util/path_utils.cpp
rename to planning/behavior_path_planner/src/utils/path_utils.cpp
index 2b959d682b409..fef2518e61bd5 100644
--- a/planning/behavior_path_planner/src/util/path_utils.cpp
+++ b/planning/behavior_path_planner/src/utils/path_utils.cpp
@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/path_utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
 
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <interpolation/spline_interpolation.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/util/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp
similarity index 94%
rename from planning/behavior_path_planner/src/util/pull_out/geometric_pull_out.cpp
rename to planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp
index ae7e27504c8d9..c324ec6912802 100644
--- a/planning/behavior_path_planner/src/util/pull_out/geometric_pull_out.cpp
+++ b/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/pull_out/geometric_pull_out.hpp"
+#include "behavior_path_planner/utils/pull_out/geometric_pull_out.hpp"
 
-#include "behavior_path_planner/util/pull_out/util.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/pull_out/util.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 
diff --git a/planning/behavior_path_planner/src/util/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp
similarity index 98%
rename from planning/behavior_path_planner/src/util/pull_out/shift_pull_out.cpp
rename to planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp
index c72afdebe4284..d0adc661e1883 100644
--- a/planning/behavior_path_planner/src/util/pull_out/shift_pull_out.cpp
+++ b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/pull_out/shift_pull_out.hpp"
+#include "behavior_path_planner/utils/pull_out/shift_pull_out.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/pull_out/util.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/pull_out/util.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 
diff --git a/planning/behavior_path_planner/src/util/pull_out/util.cpp b/planning/behavior_path_planner/src/utils/pull_out/util.cpp
similarity index 93%
rename from planning/behavior_path_planner/src/util/pull_out/util.cpp
rename to planning/behavior_path_planner/src/utils/pull_out/util.cpp
index d052254d7767d..1502d3831e681 100644
--- a/planning/behavior_path_planner/src/util/pull_out/util.cpp
+++ b/planning/behavior_path_planner/src/utils/pull_out/util.cpp
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/pull_out/util.hpp"
+#include "behavior_path_planner/utils/pull_out/util.hpp"
 
-#include "behavior_path_planner/util/create_vehicle_footprint.hpp"
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/create_vehicle_footprint.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
diff --git a/planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp
similarity index 96%
rename from planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp
rename to planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp
index 2692a09016405..26bf8471835c1 100644
--- a/planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp
+++ b/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/pull_over/freespace_pull_over.hpp"
+#include "behavior_path_planner/utils/pull_over/freespace_pull_over.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/pull_over/util.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/pull_over/util.hpp"
 
 #include <memory>
 #include <vector>
diff --git a/planning/behavior_path_planner/src/util/pull_over/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/pull_over/geometric_pull_over.cpp
similarity index 93%
rename from planning/behavior_path_planner/src/util/pull_over/geometric_pull_over.cpp
rename to planning/behavior_path_planner/src/utils/pull_over/geometric_pull_over.cpp
index 1d8c01db27ac7..cb594cb1a40a4 100644
--- a/planning/behavior_path_planner/src/util/pull_over/geometric_pull_over.cpp
+++ b/planning/behavior_path_planner/src/utils/pull_over/geometric_pull_over.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/pull_over/geometric_pull_over.hpp"
+#include "behavior_path_planner/utils/pull_over/geometric_pull_over.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/pull_over/util.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/pull_over/util.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/util/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/pull_over/goal_searcher.cpp
similarity index 98%
rename from planning/behavior_path_planner/src/util/pull_over/goal_searcher.cpp
rename to planning/behavior_path_planner/src/utils/pull_over/goal_searcher.cpp
index 1a0cb2a25e950..3914c0d39245a 100644
--- a/planning/behavior_path_planner/src/util/pull_over/goal_searcher.cpp
+++ b/planning/behavior_path_planner/src/utils/pull_over/goal_searcher.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/pull_over/goal_searcher.hpp"
+#include "behavior_path_planner/utils/pull_over/goal_searcher.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/pull_over/util.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/pull_over/util.hpp"
 #include "lanelet2_extension/utility/utilities.hpp"
 
 #include <boost/optional.hpp>
diff --git a/planning/behavior_path_planner/src/util/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp
similarity index 98%
rename from planning/behavior_path_planner/src/util/pull_over/shift_pull_over.cpp
rename to planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp
index 8c095c3918fab..1ce4d74b37ffc 100644
--- a/planning/behavior_path_planner/src/util/pull_over/shift_pull_over.cpp
+++ b/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/pull_over/shift_pull_over.hpp"
+#include "behavior_path_planner/utils/pull_over/shift_pull_over.hpp"
 
-#include "behavior_path_planner/util/path_utils.hpp"
-#include "behavior_path_planner/util/pull_over/util.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/pull_over/util.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/util/pull_over/util.cpp b/planning/behavior_path_planner/src/utils/pull_over/util.cpp
similarity index 96%
rename from planning/behavior_path_planner/src/util/pull_over/util.cpp
rename to planning/behavior_path_planner/src/utils/pull_over/util.cpp
index c7a5cc2531287..413b1934bd4d3 100644
--- a/planning/behavior_path_planner/src/util/pull_over/util.cpp
+++ b/planning/behavior_path_planner/src/utils/pull_over/util.cpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/pull_over/util.hpp"
+#include "behavior_path_planner/utils/pull_over/util.hpp"
 
-#include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
-#include "behavior_path_planner/util/path_utils.hpp"
+#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/util/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/util/safety_check.cpp
rename to planning/behavior_path_planner/src/utils/safety_check.cpp
index d4a4def6f859f..eaa854223f9a8 100644
--- a/planning/behavior_path_planner/src/util/safety_check.cpp
+++ b/planning/behavior_path_planner/src/utils/safety_check.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/safety_check.hpp"
+#include "behavior_path_planner/utils/safety_check.hpp"
 
 #include "perception_utils/predicted_path_utils.hpp"
 
diff --git a/planning/behavior_path_planner/src/util/side_shift/util.cpp b/planning/behavior_path_planner/src/utils/side_shift/util.cpp
similarity index 96%
rename from planning/behavior_path_planner/src/util/side_shift/util.cpp
rename to planning/behavior_path_planner/src/utils/side_shift/util.cpp
index 28e6027a50888..f8127abb2d9f0 100644
--- a/planning/behavior_path_planner/src/util/side_shift/util.cpp
+++ b/planning/behavior_path_planner/src/utils/side_shift/util.cpp
@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/side_shift/util.hpp"
+#include "behavior_path_planner/utils/side_shift/util.hpp"
 
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/src/util/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/util/utils.cpp
rename to planning/behavior_path_planner/src/utils/utils.cpp
index 737ace5db5163..775d2b0acb3f2 100644
--- a/planning/behavior_path_planner/src/util/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
-#include "behavior_path_planner/util/drivable_area_expansion/drivable_area_expansion.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp"
 #include "motion_utils/trajectory/path_with_lane_id.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
diff --git a/planning/behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/test/input.hpp
index cd423dd9b3bb9..de167bc8b4bcb 100644
--- a/planning/behavior_path_planner/test/input.hpp
+++ b/planning/behavior_path_planner/test/input.hpp
@@ -16,7 +16,7 @@
 #define INPUT_HPP_
 
 #endif  // INPUT_HPP_
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include "autoware_auto_planning_msgs/msg/path_point.hpp"
 #include "geometry_msgs/msg/twist.hpp"
diff --git a/planning/behavior_path_planner/test/test_avoidance_utils.cpp b/planning/behavior_path_planner/test/test_avoidance_utils.cpp
index c11cfcba3c600..1d8bb5ae72bdb 100644
--- a/planning/behavior_path_planner/test/test_avoidance_utils.cpp
+++ b/planning/behavior_path_planner/test/test_avoidance_utils.cpp
@@ -11,8 +11,8 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
-#include "behavior_path_planner/util/avoidance/util.hpp"
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/utils/avoidance/util.hpp"
 
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp
index 504d5fdf03f85..d1e9873e92c4c 100644
--- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp
+++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp
@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/util/drivable_area_expansion/drivable_area_expansion.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/path_projection.hpp"
-#include "behavior_path_planner/util/drivable_area_expansion/types.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 #include "lanelet2_extension/utility/message_conversion.hpp"
 
 #include <gtest/gtest.h>
diff --git a/planning/behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/test/test_utils.cpp
index 323151686d01d..8a762a3a49957 100644
--- a/planning/behavior_path_planner/test/test_utils.cpp
+++ b/planning/behavior_path_planner/test/test_utils.cpp
@@ -11,7 +11,7 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 #include "input.hpp"
 #include "lanelet2_core/Attribute.h"
 #include "lanelet2_core/geometry/Point.h"
diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp
index 8c75186ae5919..04d74570d347d 100644
--- a/planning/static_centerline_optimizer/src/utils.cpp
+++ b/planning/static_centerline_optimizer/src/utils.cpp
@@ -15,7 +15,7 @@
 #include "static_centerline_optimizer/utils.hpp"
 
 #include "behavior_path_planner/data_manager.hpp"
-#include "behavior_path_planner/util/utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
 namespace static_centerline_optimizer

From 99b04da5ba151ba1842c995db9ce07380ef2b223 Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Fri, 14 Apr 2023 01:07:09 +0900
Subject: [PATCH 14/15] refactor: add param files in planning module (#3363)

* add planning_interface_test_manager class

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add counter function

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for motion_velocity_smoother

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add param in default_motion_velocity_smoother,param.yaml

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* successfully launch target node

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* successfully build the test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test for test_motion_velocity_smoother

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add abnormal trajectory test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete unnecessary part

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* style(pre-commit): autofix

* run pre-commit

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add declaration

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Refactor callback functions for standardization

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refacotoring

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactored

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Refactor functions into a single template function

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* relete unnecessary definition

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Revert "delete unnecessary definition"

This reverts commit 6cd13f82868f34140fa1538d43f99d99b9648df3.

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete unnecessary definition

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete unnecessary definition

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete module dependent part

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete unnecessary arg

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* apply motion_velocity_smoother change

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for obstacle stop planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* run pre-commit

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test fot obstacle_cruise_planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test fot planning_vaildator

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for freespace_planner_node

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for obstacle_stop_planner with slow down

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add part of interface test for freespace

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* change package name

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* apply change of package name

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix build error

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp commit for debug

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add planning interface test manager for scenario selector

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update parameter

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp for behavior_path_planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add param file

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test free space planner module

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update map

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test for behavior path planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* pass freespace test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* pass freespace test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update map

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update map detection area

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add no stopping area

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* for print debug

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update param

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add param file

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor(behavior_path_planner): remove unnecessary functions (#3271)

* refactor(behavior_path_planner): remove unnecessary functions

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

---------

Signed-off-by: yutaka <purewater0901@gmail.com>

* Revert "temp"

This reverts commit b82805e29744f7689885f15d0ce89e760c73a7b9.

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* suppress build error (-Werror=pedantic)

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for behavior_velocity_planner

* build success

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add param

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* pass test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* pass test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* build success

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update param

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete param file

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Resolve differences outside of parameter files

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete duplicate parameter

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Signed-off-by: yutaka <purewater0901@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>
---
 .../config/avoidance/avoidance.param.yaml     | 13 ++++---
 .../config/behavior_path_planner.param.yaml   | 19 ++++++---
 .../config/lane_change/lane_change.param.yaml |  4 +-
 .../config/pull_over/pull_over.param.yaml     | 27 ++++++-------
 .../behavior_velocity_planner.param.yaml      |  8 ++--
 .../config/crosswalk.param.yaml               |  2 +
 .../config/intersection.param.yaml            |  4 ++
 .../config/out_of_lane.param.yaml             | 39 +++++++++++++++++++
 .../config/stop_line.param.yaml               |  4 +-
 .../config/virtual_traffic_light.param.yaml   |  2 +-
 10 files changed, 91 insertions(+), 31 deletions(-)
 create mode 100644 planning/behavior_velocity_planner/config/out_of_lane.param.yaml

diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
index c739d0eb73774..0e260f7723496 100644
--- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
+++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -16,6 +16,7 @@
       enable_force_avoidance_for_stopped_vehicle: false
       enable_safety_check: false
       enable_yield_maneuver: false
+      disable_path_update: false
 
       # for debug
       publish_debug_marker: false
@@ -52,11 +53,11 @@
 
       # For safety check
       safety_check:
-        safety_check_backward_distance: 50.0            # [m]
-        safety_check_time_horizon: 10.0                 # [s]
-        safety_check_idling_time: 1.5                   # [s]
-        safety_check_accel_for_rss: 2.5                 # [m/ss]
-        safety_check_hysteresis_factor: 2.0             # [-]
+        safety_check_backward_distance: 100.0            # [m]
+        safety_check_time_horizon: 10.0                  # [s]
+        safety_check_idling_time: 1.5                    # [s]
+        safety_check_accel_for_rss: 2.5                  # [m/ss]
+        safety_check_hysteresis_factor: 2.0              # [-]
 
       # For avoidance maneuver
       avoidance:
@@ -65,7 +66,7 @@
           lateral_collision_margin: 1.0                 # [m]
           lateral_collision_safety_buffer: 0.7          # [m]
           lateral_passable_safety_buffer: 0.0           # [m]
-          road_shoulder_safety_margin: 0.0              # [m]
+          road_shoulder_safety_margin: 0.3              # [m]
           avoidance_execution_lateral_threshold: 0.499
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml
index 1df3df2bbbcbf..7132f4057f381 100644
--- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml
+++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml
@@ -1,9 +1,13 @@
 /**:
   ros__parameters:
-    planning_hz: 10.0
+    verbose: false
+
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
 
+    planning_hz: 10.0
     backward_path_length: 5.0
-    forward_path_length: 200.0
+    forward_path_length: 300.0
     backward_length_buffer_for_end_of_pull_over: 5.0
     backward_length_buffer_for_end_of_pull_out: 5.0
     minimum_pull_over_length: 16.0
@@ -18,7 +22,10 @@
     turn_signal_shift_length_threshold: 0.3
     turn_signal_on_swerving: true
 
-    path_interval: 2.0
+    enable_akima_spline_first: false
+    enable_cog_on_centerline: false
+    input_path_interval: 2.0
+    output_path_interval: 2.0
 
     visualize_maximum_drivable_area: true
 
@@ -28,8 +35,8 @@
     expected_front_deceleration: -1.0
     expected_rear_deceleration: -1.0
 
-    expected_front_deceleration_for_abort: -2.0
-    expected_rear_deceleration_for_abort: -2.5
+    expected_front_deceleration_for_abort: -1.0
+    expected_rear_deceleration_for_abort: -2.0
 
     rear_vehicle_reaction_time: 2.0
-    rear_vehicle_safety_time_margin: 2.0
+    rear_vehicle_safety_time_margin: 1.0
diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
index 58858ccbdbdcd..87d6caa848b41 100644
--- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
+++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
@@ -1,6 +1,8 @@
 /**:
   ros__parameters:
     lane_change:
+      lane_change_prepare_duration: 4.0         # [s]
+      lane_changing_safety_check_duration: 8.0  # [s]
       prepare_duration: 4.0         # [s]
 
       minimum_prepare_length: 2.0 # [m]
@@ -16,7 +18,7 @@
       minimum_lane_changing_velocity: 2.78        # [m/s]
       prediction_time_resolution: 0.5           # [s]
       maximum_deceleration: 1.0                 # [m/s2]
-      lane_change_sampling_num: 10
+      lane_change_sampling_num: 3
 
       # target object
       target_object:
diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml
index 8f7843f7a87c5..854cfd3cb2680 100644
--- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml
+++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml
@@ -1,6 +1,7 @@
 /**:
   ros__parameters:
     pull_over:
+      request_length: 100.0
       minimum_request_length: 200.0
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
@@ -18,7 +19,7 @@
       backward_goal_search_length: 20.0
       goal_search_interval: 2.0
       longitudinal_margin: 3.0
-      max_lateral_offset: 1.0
+      max_lateral_offset: 0.5
       lateral_offset_interval: 0.25
       ignore_distance_from_lane_start: 15.0
       # occupancy grid map
@@ -36,18 +37,7 @@
       maximum_lateral_jerk: 2.0
       minimum_lateral_jerk: 0.5
       deceleration_interval: 15.0
-      after_pull_over_straight_distance: 5.0
-      # parallel parking path
-      enable_arc_forward_parking: true
-      enable_arc_backward_parking: true
-      after_forward_parking_straight_distance: 2.0
-      after_backward_parking_straight_distance: 2.0
-      forward_parking_velocity: 1.38
-      backward_parking_velocity: -1.38
-      forward_parking_lane_departure_margin: 0.0
-      backward_parking_lane_departure_margin: 0.0
-      arc_path_interval: 1.0
-      pull_over_max_steer_angle: 0.35  # 20deg
+      after_pull_over_straight_distance: 1.0
       # freespace parking
       enable_freespace_parking: true
       freespace_parking:
@@ -79,6 +69,17 @@
           max_planning_time: 150.0
           neighbor_radius: 8.0
           margin: 1.0
+      # parallel parking path
+      enable_arc_forward_parking: true
+      enable_arc_backward_parking: true
+      after_forward_parking_straight_distance: 2.0
+      after_backward_parking_straight_distance: 2.0
+      forward_parking_velocity: 1.38
+      backward_parking_velocity: -1.38
+      forward_parking_lane_departure_margin: 0.0
+      backward_parking_lane_departure_margin: 0.0
+      arc_path_interval: 1.0
+      pull_over_max_steer_angle: 0.35  # 20deg
       # hazard on when parked
       hazard_on_threshold_distance: 1.0
       hazard_on_threshold_velocity: 0.5
diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml
index 09ee0b00a0d4c..6f1341cb0264d 100644
--- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml
+++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml
@@ -6,13 +6,15 @@
     launch_intersection: true
     launch_blind_spot: true
     launch_detection_area: true
-    launch_virtual_traffic_light: true
-    launch_occlusion_spot: true
+    launch_virtual_traffic_light: false # disabled by default to not confuse newcomers
+    launch_occlusion_spot: false
     launch_no_stopping_area: true
     launch_run_out: false
-    launch_speed_bump: true
+    launch_speed_bump: false
+    launch_out_of_lane: true
     forward_path_length: 1000.0
     backward_path_length: 5.0
+    stop_line_extend_length: 5.0
     max_accel: -2.8
     max_jerk: -5.0
     system_delay: 0.5
diff --git a/planning/behavior_velocity_planner/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/config/crosswalk.param.yaml
index 2bbc5d31fcaff..444912b60e222 100644
--- a/planning/behavior_velocity_planner/config/crosswalk.param.yaml
+++ b/planning/behavior_velocity_planner/config/crosswalk.param.yaml
@@ -29,6 +29,7 @@
       ego_yield_query_stop_duration: 0.1 # [s] the amount of time which ego should be stopping to query whether it yields or not
 
       # param for input data
+      external_input_timeout: 1.0
       tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal
 
       # param for target area & object
@@ -42,3 +43,4 @@
     walkway:
       stop_duration_sec: 1.0 # [s] stop time at stop position
       stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists
+      external_input_timeout: 1.0
diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml
index 7d4cce642b2fb..9f6d39189f465 100644
--- a/planning/behavior_velocity_planner/config/intersection.param.yaml
+++ b/planning/behavior_velocity_planner/config/intersection.param.yaml
@@ -22,6 +22,10 @@
       assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
       enable_front_car_decel_prediction: false # By default this feature is disabled
       stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collision detection
+      external_input_timeout: 1.0
 
     merge_from_private_road:
       stop_duration_sec: 1.0
+    merge_from_private:
+      merge_from_private_area:
+        stop_duration_sec: 1.0
diff --git a/planning/behavior_velocity_planner/config/out_of_lane.param.yaml b/planning/behavior_velocity_planner/config/out_of_lane.param.yaml
new file mode 100644
index 0000000000000..c501599b4ae1c
--- /dev/null
+++ b/planning/behavior_velocity_planner/config/out_of_lane.param.yaml
@@ -0,0 +1,39 @@
+/**:
+  ros__parameters:
+    out_of_lane:  # module to stop or slowdown before overlapping another lane with other objects
+      mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc"
+      skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane
+
+      threshold:
+        time_threshold: 5.0  # [s] consider objects that will reach an overlap within this time
+      intervals:  # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego
+        ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer
+        objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer
+      ttc:
+        threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap
+
+      objects:
+        minimum_velocity: 0.5  # [m/s] objects lower than this velocity will be ignored
+        use_predicted_paths: true  # if true, use the predicted paths to estimate future positions.
+                                   # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in.
+
+      overlap:
+        minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered
+        extra_length: 0.0  # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times)
+
+      action:  # action to insert in the path if an object causes a conflict at an overlap
+        skip_if_over_max_decel: true  # if true, do not take an action that would cause more deceleration than the maximum allowed
+        strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego
+                     # if false, ego stops just before entering a lane but may then be overlapping another lane.
+        distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
+        slowdown:
+          distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap
+          velocity: 2.0  # [m/s] slowdown velocity
+        stop:
+          distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap
+
+      ego:
+        extra_front_offset: 0.0 # [m] extra front distance
+        extra_rear_offset: 0.0 # [m] extra rear distance
+        extra_right_offset: 0.0 # [m] extra right distance
+        extra_left_offset: 0.0 # [m] extra left distance
diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml
index be7b0e7e70bb4..12967d753e25b 100644
--- a/planning/behavior_velocity_planner/config/stop_line.param.yaml
+++ b/planning/behavior_velocity_planner/config/stop_line.param.yaml
@@ -2,8 +2,10 @@
   ros__parameters:
     stop_line:
       stop_margin: 0.0
+      stop_check_dist: 2.0
       stop_duration_sec: 1.0
-      hold_stop_margin_distance: 2.0
       use_initialization_stop_line_state: true
+      hold_stop_margin_distance: 2.0
+
       debug:
         show_stopline_collision_check: false            # [-] whether to show stopline collision
diff --git a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml
index 8879408d40386..ca5fc6fc1e3d1 100644
--- a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml
+++ b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml
@@ -4,6 +4,6 @@
       max_delay_sec: 3.0
       near_line_distance: 1.0
       dead_line_margin: 1.0
-      hold_stop_margin_distance: 0.0
       max_yaw_deviation_deg: 90.0
       check_timeout_after_stop_line: true
+      hold_stop_margin_distance: 0.0

From 806a2ac94af51214d78e1e976661a9ff26a70a08 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Fri, 14 Apr 2023 09:08:11 +0900
Subject: [PATCH 15/15] refactor(planner_manager): cleanup code (#3382)

* refactor(planner_manager): cleanup

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(planner_manager): cleanup code

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../behavior_path_planner/planner_manager.hpp | 129 ++++-
 .../src/planner_manager.cpp                   | 441 ++++++++++--------
 2 files changed, 360 insertions(+), 210 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
index 04fb694813026..727c9d10be1dc 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
@@ -59,8 +59,16 @@ class PlannerManager
     rclcpp::Node & node, const std::shared_ptr<LaneFollowingParameters> & parameters,
     const bool verbose);
 
+  /**
+   * @brief run all candidate and approved modules.
+   * @param planner data.
+   */
   BehaviorModuleOutput run(const std::shared_ptr<PlannerData> & data);
 
+  /**
+   * @brief register managers.
+   * @param manager pointer.
+   */
   void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr)
   {
     RCLCPP_INFO(logger_, "register %s module", manager_ptr->getModuleName().c_str());
@@ -68,6 +76,10 @@ class PlannerManager
     processing_time_.emplace(manager_ptr->getModuleName(), 0.0);
   }
 
+  /**
+   * @brief update module parameters. used for dynamic reconfigure.
+   * @param parameters.
+   */
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters)
   {
     std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [&parameters](const auto & m) {
@@ -75,6 +87,9 @@ class PlannerManager
     });
   }
 
+  /**
+   * @brief reset all member variables.
+   */
   void reset()
   {
     approved_module_ptrs_.clear();
@@ -84,14 +99,25 @@ class PlannerManager
     resetProcessingTime();
   }
 
+  /**
+   * @brief publish all registered modules' debug markers.
+   */
   void publishDebugMarker() const
   {
     std::for_each(
       manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishDebugMarker(); });
   }
 
+  /**
+   * @brief get manager pointers.
+   * @return manager pointers.
+   */
   std::vector<SceneModuleManagerPtr> getSceneModuleManagers() const { return manager_ptrs_; }
 
+  /**
+   * @brief get all scene modules status (is execution request, is ready and status).
+   * @return statuses
+   */
   std::vector<std::shared_ptr<SceneModuleStatus>> getSceneModuleStatus() const
   {
     std::vector<std::shared_ptr<SceneModuleStatus>> ret;
@@ -117,15 +143,31 @@ class PlannerManager
     return ret;
   }
 
+  /**
+   * @brief reset root lanelet. if there are approved modules, don't reset root lanelet.
+   * @param planner data.
+   * @details this function is called only when it is in disengage and drive by manual.
+   */
   void resetRootLanelet(const std::shared_ptr<PlannerData> & data);
 
+  /**
+   * @brief show planner manager internal condition.
+   */
   void print() const;
 
 private:
+  /**
+   * @brief run the module and publish RTC status.
+   * @param module.
+   * @param planner data.
+   * @return planning result.
+   */
   BehaviorModuleOutput run(
     const SceneModulePtr & module_ptr, const std::shared_ptr<PlannerData> & planner_data,
     const BehaviorModuleOutput & previous_module_output) const
   {
+    stop_watch_.tic(module_ptr->name());
+
     module_ptr->setData(planner_data);
     module_ptr->setPreviousModuleOutput(previous_module_output);
 
@@ -137,34 +179,46 @@ class PlannerManager
 
     module_ptr->publishRTCStatus();
 
+    processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true);
+
     return result;
   }
 
+  /**
+   * @brief stop and unregister the module from manager.
+   * @param module.
+   */
   void deleteExpiredModules(SceneModulePtr & module_ptr) const
   {
-    const auto itr = std::find_if(
-      manager_ptrs_.begin(), manager_ptrs_.end(),
-      [&module_ptr](const auto & m) { return m->getModuleName() == module_ptr->name(); });
-    if (itr == manager_ptrs_.end()) {
-      return;
-    }
-
-    (*itr)->deleteModules(module_ptr);
+    const auto manager = getManager(module_ptr);
+    manager->deleteModules(module_ptr);
   }
 
-  void sortByPriority(std::vector<SceneModulePtr> & request_modules) const
+  /**
+   * @brief swap the modules order based on it's priority.
+   * @param modules.
+   * @details for now, the priority is decided in config file and doesn't change runtime.
+   */
+  void sortByPriority(std::vector<SceneModulePtr> & modules) const
   {
     // TODO(someone) enhance this priority decision method.
-    std::sort(request_modules.begin(), request_modules.end(), [this](auto a, auto b) {
+    std::sort(modules.begin(), modules.end(), [this](auto a, auto b) {
       return getManager(a)->getPriority() < getManager(b)->getPriority();
     });
   }
 
+  /**
+   * @brief push back to approved_module_ptrs_.
+   * @param approved module pointer.
+   */
   void addApprovedModule(const SceneModulePtr & module_ptr)
   {
     approved_module_ptrs_.push_back(module_ptr);
   }
 
+  /**
+   * @brief reset processing time.
+   */
   void resetProcessingTime()
   {
     for (auto & t : processing_time_) {
@@ -172,14 +226,22 @@ class PlannerManager
     }
   }
 
+  /**
+   * @brief stop and remove all modules in candidate_module_ptrs_.
+   */
   void clearCandidateModules()
   {
-    for (auto & m : candidate_module_ptrs_) {
+    std::for_each(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) {
       deleteExpiredModules(m);
-    }
+    });
     candidate_module_ptrs_.clear();
   }
 
+  /**
+   * @brief get current root lanelet. the lanelet is used for reference path generation.
+   * @param planner data.
+   * @return root lanelet.
+   */
   lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr<PlannerData> & data) const
   {
     lanelet::ConstLanelet ret{};
@@ -188,6 +250,11 @@ class PlannerManager
     return ret;
   }
 
+  /**
+   * @brief get manager pointer from module name.
+   * @param module pointer.
+   * @return manager pointer.
+   */
   SceneModuleManagerPtr getManager(const SceneModulePtr & module_ptr) const
   {
     const auto itr = std::find_if(
@@ -201,20 +268,54 @@ class PlannerManager
     return *itr;
   }
 
+  /**
+   * @brief run all modules in approved_module_ptrs_ and get a planning result as
+   * approved_modules_output.
+   * @param planner data.
+   * @return valid planning result.
+   * @details in this function, expired modules (ModuleStatus::FAILURE or ModuleStatus::SUCCESS) are
+   * removed from approved_module_ptrs_.
+   */
   BehaviorModuleOutput runApprovedModules(const std::shared_ptr<PlannerData> & data);
 
+  /**
+   * @brief get reference path from root_lanelet_ centerline.
+   * @param planner data.
+   * @return reference path.
+   */
   BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & data) const;
 
+  /**
+   * @brief select a module that should be execute at first.
+   * @param modules that make execution request.
+   * @return the highest priority module.
+   */
   SceneModulePtr selectHighestPriorityModule(std::vector<SceneModulePtr> & request_modules) const;
 
+  /**
+   * @brief update candidate_module_ptrs_ based on latest request modules.
+   * @param the highest priority module in latest request modules.
+   */
   void updateCandidateModules(
-    const std::vector<SceneModulePtr> & candidate_modules,
+    const std::vector<SceneModulePtr> & request_modules,
     const SceneModulePtr & highest_priority_module);
 
+  /**
+   * @brief get all modules that make execution request.
+   * @param decided (=approved) path.
+   * @return request modules.
+   */
   std::vector<SceneModulePtr> getRequestModules(
     const BehaviorModuleOutput & previous_module_output) const;
 
-  std::pair<SceneModulePtr, BehaviorModuleOutput> runCandidateModules(
+  /**
+   * @brief checks whether a path of trajectory has forward driving direction
+   * @param modules that make execution request.
+   * @param planner data.
+   * @param decided (=approved) path.
+   * @return the highest priority module in request modules, and it's planning result.
+   */
+  std::pair<SceneModulePtr, BehaviorModuleOutput> runRequestModules(
     const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
     const BehaviorModuleOutput & previous_module_output);
 
diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
index add019d6bcbc2..675e90ec0f53f 100644
--- a/planning/behavior_path_planner/src/planner_manager.cpp
+++ b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -73,7 +73,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
      * STEP4: if there is module that should be launched, execute the module
      */
     const auto [highest_priority_module, candidate_modules_output] =
-      runCandidateModules(request_modules, data, approved_modules_output);
+      runRequestModules(request_modules, data, approved_modules_output);
     if (!highest_priority_module) {
       processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
       return approved_modules_output;
@@ -107,120 +107,117 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
 {
   std::vector<SceneModulePtr> request_modules{};
 
-  const auto block_simultaneous_execution = [this]() {
-    for (const auto & module_ptr : approved_module_ptrs_) {
-      const auto itr = std::find_if(
-        manager_ptrs_.begin(), manager_ptrs_.end(),
-        [&module_ptr](const auto & m) { return m->getModuleName() == module_ptr->name(); });
-      if (itr == manager_ptrs_.end()) {
-        return true;
-      }
-      if (!(*itr)->isSimultaneousExecutableAsApprovedModule()) {
-        return true;
-      }
+  /**
+   * check whether it is possible to push back more modules to approved modules.
+   */
+  {
+    const auto find_block_module = [this](const auto & m) {
+      return !getManager(m)->isSimultaneousExecutableAsApprovedModule();
+    };
+
+    const auto itr =
+      std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module);
+
+    if (itr != approved_module_ptrs_.end()) {
+      return {};
     }
-    return false;
-  }();
+  }
+
+  const auto toc = [this](const auto & name) {
+    processing_time_.at(name) += stop_watch_.toc(name, true);
+  };
 
-  // pickup execution requested modules
   for (const auto & manager_ptr : manager_ptrs_) {
     stop_watch_.tic(manager_ptr->getModuleName());
 
-    const auto toc = [this, &manager_ptr]() {
+    /**
+     * don't launch candidate module if approved modules already exist.
+     */
+    if (!approved_module_ptrs_.empty()) {
+      if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) {
+        toc(manager_ptr->getModuleName());
+        continue;
+      }
+    }
+
+    /**
+     * launch new candidate module.
+     */
+    {
       const auto name = manager_ptr->getModuleName();
-      processing_time_.at(name) += stop_watch_.toc(name, true);
-    };
+      const auto find_same_name_module = [&name](const auto & m) { return m->name() == name; };
+      const auto itr = std::find_if(
+        candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), find_same_name_module);
 
-    // already exist the modules that don't support simultaneous execution. -> DO NOTHING.
-    if (block_simultaneous_execution) {
-      toc();
-      continue;
-    }
+      if (itr == candidate_module_ptrs_.end()) {
+        if (manager_ptr->canLaunchNewModule()) {
+          const auto new_module_ptr = manager_ptr->getNewModule();
 
-    // the module doesn't support simultaneous execution. -> DO NOTHING.
-    if (
-      !approved_module_ptrs_.empty() && !manager_ptr->isSimultaneousExecutableAsApprovedModule()) {
-      toc();
-      continue;
-    }
+          if (manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) {
+            request_modules.emplace_back(new_module_ptr);
+          }
+        }
 
-    // check there is a launched module which is waiting approval as candidate.
-    if (candidate_module_ptrs_.empty()) {
-      // launched module num reach limit. -> CAN'T LAUNCH NEW MODULE. DO NOTHING.
-      if (!manager_ptr->canLaunchNewModule()) {
-        toc();
+        toc(manager_ptr->getModuleName());
         continue;
       }
-
-      // the module requests it to be launch. -> CAN LAUNCH THE MODULE. PUSH BACK AS REQUEST
-      // MODULES.
-      const auto new_module_ptr = manager_ptr->getNewModule();
-      if (manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) {
-        request_modules.emplace_back(new_module_ptr);
-      }
-
-      toc();
-      continue;
     }
 
-    for (const auto & module_ptr : candidate_module_ptrs_) {
-      const auto & name = module_ptr->name();
+    /**
+     * module already exist in candidate modules. check whether other modules can be launch as
+     * candidate. if locked, break this loop.
+     */
+    {
+      const auto name = manager_ptr->getModuleName();
+      const auto find_block_module = [&name](const auto & m) {
+        return m->name() == name && m->isLockedNewModuleLaunch();
+      };
       const auto itr = std::find_if(
-        manager_ptrs_.begin(), manager_ptrs_.end(),
-        [&name](const auto & m) { return m->getModuleName() == name; });
+        candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), find_block_module);
 
-      if (itr == manager_ptrs_.end()) {
-        continue;
-      }
-
-      // when the approved module throw another approval request, block all. -> CLEAR REQUEST
-      // MODULES AND PUSH BACK.
-      if (module_ptr->isLockedNewModuleLaunch()) {
+      if (itr != candidate_module_ptrs_.end()) {
         request_modules.clear();
-        request_modules.emplace_back(module_ptr);
-        toc();
-        return request_modules;
+        request_modules.emplace_back(*itr);
+        toc(manager_ptr->getModuleName());
+        break;
       }
     }
 
+    /**
+     * module already exist. keep using it as candidate.
+     */
     {
-      const auto & name = manager_ptr->getModuleName();
+      const auto name = manager_ptr->getModuleName();
+      const auto find_launched_module = [&name](const auto & m) { return m->name() == name; };
       const auto itr = std::find_if(
-        candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(),
-        [&name](const auto & m) { return m->name() == name; });
+        candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), find_launched_module);
 
-      // same name module already launched as candidate.
       if (itr != candidate_module_ptrs_.end()) {
-        // the module launched as candidate and is running now. the module hasn't thrown any
-        // approval yet. -> PUSH BACK AS REQUEST MODULES.
-        if ((*itr)->getCurrentStatus() == ModuleStatus::RUNNING) {
-          request_modules.emplace_back(*itr);
-          // } else {
-          //   // TODO(Satoshi OTA) this line is no longer needed? think later.
-          //   manager_ptr->deleteModules(*itr);
-        }
-
-        toc();
+        request_modules.emplace_back(*itr);
+        toc(manager_ptr->getModuleName());
         continue;
       }
     }
 
-    // different name module already launched as candidate.
-    // launched module num reach limit. -> CAN'T LAUNCH NEW MODULE. DO NOTHING.
-    if (!manager_ptr->canLaunchNewModule()) {
-      toc();
-      continue;
-    }
+    /**
+     * same name module doesn't exist in candidate modules. launch new module.
+     */
+    {
+      if (!manager_ptr->canLaunchNewModule()) {
+        toc(manager_ptr->getModuleName());
+        continue;
+      }
 
-    // the module requests it to be launch. -> CAN LAUNCH THE MODULE. PUSH BACK AS REQUEST MODULES.
-    const auto new_module_ptr = manager_ptr->getNewModule();
-    if (!manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) {
-      toc();
-      continue;
+      const auto new_module_ptr = manager_ptr->getNewModule();
+      if (!manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) {
+        toc(manager_ptr->getModuleName());
+        continue;
+      }
+
+      request_modules.emplace_back(new_module_ptr);
     }
 
-    toc();
-    request_modules.emplace_back(new_module_ptr);
+    toc(manager_ptr->getModuleName());
   }
 
   return request_modules;
@@ -238,97 +235,107 @@ SceneModulePtr PlannerManager::selectHighestPriorityModule(
   return request_modules.front();
 }
 
-std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runCandidateModules(
+std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModules(
   const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
   const BehaviorModuleOutput & previous_module_output)
 {
+  // modules that are filtered by simultaneous executable condition.
   std::vector<SceneModulePtr> executable_modules;
+
+  // modules that are not approved after running yet.
   std::vector<SceneModulePtr> waiting_approved_modules;
+
+  // modules that are already approved after running.
   std::vector<SceneModulePtr> already_approved_modules;
+
+  // all request modules planning results.
   std::unordered_map<std::string, BehaviorModuleOutput> results;
 
+  /**
+   * sort by priority. sorted_request_modules.front() is the highest priority module.
+   */
   auto sorted_request_modules = request_modules;
-
   sortByPriority(sorted_request_modules);
 
-  const auto is_simultaneous_executable = [this](const auto & module_ptrs) {
-    for (const auto & module_ptr : module_ptrs) {
-      if (!getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule()) {
-        return false;
-      }
-    }
-
-    return true;
-  };
-
+  /**
+   * remove non-executable modules.
+   */
   for (const auto & module_ptr : sorted_request_modules) {
-    const auto simultaneous_executable =
-      getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule();
-
-    if (!simultaneous_executable) {
+    if (!getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule()) {
       if (executable_modules.empty()) {
         executable_modules.push_back(module_ptr);
         break;
       }
     }
 
-    if (is_simultaneous_executable(executable_modules)) {
+    const auto itr =
+      std::find_if(executable_modules.begin(), executable_modules.end(), [this](const auto & m) {
+        return !getManager(m)->isSimultaneousExecutableAsCandidateModule();
+      });
+
+    if (itr == executable_modules.end()) {
       executable_modules.push_back(module_ptr);
     }
   }
 
+  /**
+   * run executable modules.
+   */
   for (const auto & module_ptr : executable_modules) {
-    const auto name = module_ptr->name();
-
-    stop_watch_.tic(name);
-
     const auto & manager_ptr = getManager(module_ptr);
 
     if (!manager_ptr->exist(module_ptr)) {
       manager_ptr->registerNewModule(module_ptr, previous_module_output);
     }
 
-    const auto result = run(module_ptr, data, previous_module_output);
-    results.emplace(name, result);
-
-    processing_time_.at(name) += stop_watch_.toc(name, true);
+    results.emplace(module_ptr->name(), run(module_ptr, data, previous_module_output));
   }
 
-  const auto remove_expired_modules = [this](auto & m) {
-    if (m->getCurrentStatus() == ModuleStatus::FAILURE) {
-      deleteExpiredModules(m);
-      return true;
-    }
+  /**
+   * remove expired modules.
+   */
+  {
+    const auto remove_expired_modules = [this](auto & m) {
+      if (m->getCurrentStatus() == ModuleStatus::FAILURE) {
+        deleteExpiredModules(m);
+        return true;
+      }
 
-    if (m->getCurrentStatus() == ModuleStatus::SUCCESS) {
-      deleteExpiredModules(m);
-      return true;
-    }
+      if (m->getCurrentStatus() == ModuleStatus::SUCCESS) {
+        deleteExpiredModules(m);
+        return true;
+      }
 
-    return false;
-  };
+      return false;
+    };
 
-  executable_modules.erase(
-    std::remove_if(executable_modules.begin(), executable_modules.end(), remove_expired_modules),
-    executable_modules.end());
+    executable_modules.erase(
+      std::remove_if(executable_modules.begin(), executable_modules.end(), remove_expired_modules),
+      executable_modules.end());
+  }
 
+  /**
+   * return null data if valid candidate module doesn't exist.
+   */
   if (executable_modules.empty()) {
-    std::for_each(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) {
-      deleteExpiredModules(m);
-    });
-    candidate_module_ptrs_.clear();
+    clearCandidateModules();
     return std::make_pair(nullptr, BehaviorModuleOutput{});
   }
 
-  for (const auto & module_ptr : executable_modules) {
-    if (module_ptr->isWaitingApproval()) {
-      waiting_approved_modules.push_back(module_ptr);
+  /**
+   * separate by approve condition.
+   */
+  std::for_each(executable_modules.begin(), executable_modules.end(), [&](const auto & m) {
+    if (m->isWaitingApproval()) {
+      waiting_approved_modules.push_back(m);
     } else {
-      already_approved_modules.push_back(module_ptr);
+      already_approved_modules.push_back(m);
     }
-  }
+  });
 
-  // select one module to run as candidate module.
+  /**
+   * choice highest priority module.
+   */
   const auto module_ptr = [&]() {
     if (!already_approved_modules.empty()) {
       return selectHighestPriorityModule(already_approved_modules);
@@ -341,6 +348,9 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runCandidateModu
     return SceneModulePtr();
   }();
 
+  /**
+   * register candidate modules.
+   */
   updateCandidateModules(executable_modules, module_ptr);
 
   return std::make_pair(module_ptr, results.at(module_ptr->name()));
@@ -348,66 +358,96 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runCandidateModu
 
 BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<PlannerData> & data)
 {
-  BehaviorModuleOutput output = getReferencePath(data);  // generate root reference path.
+  std::unordered_map<std::string, BehaviorModuleOutput> results;
 
-  bool remove_after_module = false;
+  BehaviorModuleOutput output = getReferencePath(data);
+  results.emplace("root", output);
 
-  for (auto itr = approved_module_ptrs_.begin(); itr != approved_module_ptrs_.end();) {
-    const auto & name = (*itr)->name();
+  /**
+   * execute all approved modules.
+   */
+  std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
+    output = run(m, data, output);
+    results.emplace(m->name(), output);
+  });
 
-    stop_watch_.tic(name);
+  /**
+   * pop waiting approve module. push it back candidate modules. if waiting approve module is found
+   * in iteration std::find_if, not only the module but also the next one are removed from
+   * approved_module_ptrs_ since the next module's input (= waiting approve module's output) maybe
+   * change significantly. And, only the waiting approve module is pushed back to
+   * candidate_module_ptrs_.
+   */
+  {
+    const auto waiting_approval_modules = [](const auto & m) { return m->isWaitingApproval(); };
 
-    // if one of the approved modules changes to waiting approval, remove all behind modules.
-    if (remove_after_module) {
-      deleteExpiredModules(*itr);
-      itr = approved_module_ptrs_.erase(itr);
-      processing_time_.at(name) += stop_watch_.toc(name, true);
-      continue;
+    const auto itr = std::find_if(
+      approved_module_ptrs_.begin(), approved_module_ptrs_.end(), waiting_approval_modules);
+
+    if (itr != approved_module_ptrs_.end()) {
+      clearCandidateModules();
+      candidate_module_ptrs_.push_back(*itr);
     }
 
-    const auto result = run(*itr, data, output);  // execute approved module planning.
+    approved_module_ptrs_.erase(
+      std::remove_if(
+        approved_module_ptrs_.begin(), approved_module_ptrs_.end(), waiting_approval_modules),
+      approved_module_ptrs_.end());
+  }
 
-    // check the module is necessary or not.
-    const auto current_status = (*itr)->getCurrentStatus();
-    if (current_status != ModuleStatus::RUNNING) {
-      if (itr == approved_module_ptrs_.begin()) {
-        // update root lanelet when the lane change is done.
-        if (current_status == ModuleStatus::SUCCESS) {
-          if (name.find("lane_change") != std::string::npos) {
-            root_lanelet_ = updateRootLanelet(data);
-          }
-          output = result;
-        }
+  /**
+   * remove failure modules. these modules' outputs are discarded as invalid plan.
+   */
+  {
+    const auto itr = std::find_if(
+      approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+      [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::FAILURE; });
 
-        if (current_status == ModuleStatus::FAILURE) {
-          remove_after_module = true;
-        }
+    std::for_each(itr, approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); });
 
-        clearCandidateModules();
-        deleteExpiredModules(*itr);  // unregister the expired module from manager.
-        itr = approved_module_ptrs_.erase(itr);
-        processing_time_.at(name) += stop_watch_.toc(name, true);
-        continue;
-      }
+    if (itr != approved_module_ptrs_.end()) {
+      clearCandidateModules();
     }
 
-    // if one of the approved modules is waiting approval, the module is popped as candidate
-    // module again.
-    if ((*itr)->isWaitingApproval()) {
-      clearCandidateModules();
-      candidate_module_ptrs_.push_back(*itr);
-      itr = approved_module_ptrs_.erase(itr);
-      remove_after_module = true;
-      processing_time_.at(name) += stop_watch_.toc(name, true);
-      continue;
+    approved_module_ptrs_.erase(itr, approved_module_ptrs_.end());
+  }
+
+  if (approved_module_ptrs_.empty()) {
+    return results.at("root");
+  }
+
+  /**
+   * use the last module's output as approved modules planning result.
+   */
+  const auto output_module_name = approved_module_ptrs_.back()->name();
+  const auto approved_modules_output = [&output_module_name, &results]() {
+    if (results.count(output_module_name) == 0) {
+      return results.at("root");
     }
+    return results.at(output_module_name);
+  }();
 
-    processing_time_.at(name) += stop_watch_.toc(name, true);
-    output = result;
-    itr++;
+  /**
+   * 1.remove success modules. these modules' outputs are used as valid plan.
+   * 2.update root lanelet if lane change module succeeded path planning, and finished correctly.
+   */
+  {
+    const auto itr = std::find_if(
+      approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+      [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::SUCCESS; });
+
+    if (itr == approved_module_ptrs_.begin()) {
+      if ((*itr)->name().find("lane_change") != std::string::npos) {
+        root_lanelet_ = updateRootLanelet(data);
+      }
+
+      clearCandidateModules();
+      deleteExpiredModules(*itr);
+      approved_module_ptrs_.erase(itr);
+    }
   }
 
-  return output;
+  return approved_modules_output;
 }
 
 BehaviorModuleOutput PlannerManager::getReferencePath(
@@ -475,7 +515,7 @@ BehaviorModuleOutput PlannerManager::getReferencePath(
 }
 
 void PlannerManager::updateCandidateModules(
-  const std::vector<SceneModulePtr> & candidate_modules,
+  const std::vector<SceneModulePtr> & request_modules,
   const SceneModulePtr & highest_priority_module)
 {
   const auto exist = [](const auto & module_ptr, const auto & module_ptrs) {
@@ -486,21 +526,29 @@ void PlannerManager::updateCandidateModules(
     return itr != module_ptrs.end();
   };
 
-  const auto candidate_to_remove = [&](auto & itr) {
-    if (!exist(itr, candidate_modules)) {
-      deleteExpiredModules(itr);
-      return true;
-    }
-    return itr->name() == highest_priority_module->name() &&
-           !highest_priority_module->isWaitingApproval();
-  };
+  /**
+   * unregister expired modules
+   */
+  {
+    const auto candidate_to_remove = [&](auto & itr) {
+      if (!exist(itr, request_modules)) {
+        deleteExpiredModules(itr);
+        return true;
+      }
+      return itr->name() == highest_priority_module->name() &&
+             !highest_priority_module->isWaitingApproval();
+    };
 
-  candidate_module_ptrs_.erase(
-    std::remove_if(
-      candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), candidate_to_remove),
-    candidate_module_ptrs_.end());
+    candidate_module_ptrs_.erase(
+      std::remove_if(
+        candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), candidate_to_remove),
+      candidate_module_ptrs_.end());
+  }
 
-  for (const auto & m : candidate_modules) {
+  /**
+   * register running candidate modules
+   */
+  for (const auto & m : request_modules) {
     if (
       m->name() == highest_priority_module->name() &&
       !highest_priority_module->isWaitingApproval()) {
@@ -512,9 +560,10 @@ void PlannerManager::updateCandidateModules(
     }
   }
 
-  std::sort(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto a, auto b) {
-    return getManager(a)->getPriority() < getManager(b)->getPriority();
-  });
+  /**
+   * sort by priority. sorted_request_modules.front() is the highest priority module.
+   */
+  sortByPriority(candidate_module_ptrs_);
 }
 
 void PlannerManager::resetRootLanelet(const std::shared_ptr<PlannerData> & data)