chore: sync upstream

common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp

@@ -63,8 +63,8 @@ std::string getModuleName(const uint8_t module_type)
     case Module::AVOIDANCE_RIGHT: {
       return "avoidance_right";
     }
-    case Module::PULL_OVER: {
-      return "pull_over";
+    case Module::GOAL_PLANNER: {
+      return "goal_planner";
     }
     case Module::PULL_OUT: {
       return "pull_out";
@@ -105,7 +105,7 @@ bool isPathChangeModule(const uint8_t module_type)
     module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT ||
     module_type == Module::AVOIDANCE_BY_LC_LEFT || module_type == Module::AVOIDANCE_BY_LC_RIGHT ||
     module_type == Module::AVOIDANCE_LEFT || module_type == Module::AVOIDANCE_RIGHT ||
-    module_type == Module::PULL_OVER || module_type == Module::PULL_OUT) {
+    module_type == Module::GOAL_PLANNER || module_type == Module::PULL_OUT) {
     return true;
   }
   return false;

common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp

@@ -234,8 +234,8 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray::
         case SteeringFactor::PULL_OUT:
           label->setText("PULL_OUT");
           break;
-        case SteeringFactor::PULL_OVER:
-          label->setText("PULL_OVER");
+        case SteeringFactor::GOAL_PLANNER:
+          label->setText("GOAL_PLANNER");
           break;
         case SteeringFactor::EMERGENCY_OPERATION:
           label->setText("EMERGENCY_OPERATION");

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -22,7 +22,7 @@
   <arg name="side_shift_param_path"/>
   <arg name="avoidance_param_path"/>
   <arg name="lane_change_param_path"/>
-  <arg name="pull_over_param_path"/>
+  <arg name="goal_planner_param_path"/>
   <arg name="pull_out_param_path"/>
   <arg name="drivable_area_expansion_param_path"/>
   <arg name="behavior_path_planner_param_path"/>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -51,8 +51,8 @@ def launch_setup(context, *args, **kwargs):
         avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f:
         lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("pull_over_param_path").perform(context), "r") as f:
-        pull_over_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f:
+        goal_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(LaunchConfiguration("pull_out_param_path").perform(context), "r") as f:
         pull_out_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f:
@@ -91,7 +91,7 @@ def launch_setup(context, *args, **kwargs):
             avoidance_param,
             avoidance_by_lc_param,
             lane_change_param,
-            pull_over_param,
+            goal_planner_param,
             pull_out_param,
             drivable_area_expansion_param,
             scene_module_manager_param,

launch/tier4_planning_launch/package.xml

@@ -7,7 +7,7 @@
   <!-- behavior path planner -->
   <!-- lane change module -->
   <maintainer email="zulfaqar.azmi@tier4.jp">Zulfaqar Azmi</maintainer>
-  <!-- pull over module -->
+  <!-- goal planner module -->
   <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
   <!-- pull out module -->
   <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>

planning/behavior_path_planner/CMakeLists.txt

@@ -16,7 +16,7 @@ set(common_src
   src/scene_module/avoidance/avoidance_module.cpp
   src/scene_module/avoidance_by_lc/module.cpp
   src/scene_module/pull_out/pull_out_module.cpp
-  src/scene_module/pull_over/pull_over_module.cpp
+  src/scene_module/goal_planner/goal_planner_module.cpp
   src/scene_module/side_shift/side_shift_module.cpp
   src/scene_module/lane_change/interface.cpp
   src/scene_module/lane_change/lane_change_module.cpp
@@ -30,12 +30,12 @@ set(common_src
   src/utils/avoidance/util.cpp
   src/utils/lane_change/utils.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_over/default_fixed_goal_planner.cpp
+  src/utils/goal_planner/util.cpp
+  src/utils/goal_planner/shift_pull_over.cpp
+  src/utils/goal_planner/geometric_pull_over.cpp
+  src/utils/goal_planner/freespace_pull_over.cpp
+  src/utils/goal_planner/goal_searcher.cpp
+  src/utils/goal_planner/default_fixed_goal_planner.cpp
   src/utils/pull_out/util.cpp
   src/utils/pull_out/shift_pull_out.cpp
   src/utils/pull_out/geometric_pull_out.cpp
@@ -56,7 +56,6 @@ if(COMPILE_WITH_OLD_ARCHITECTURE)
     src/behavior_tree_manager.cpp
     src/scene_module/scene_module_bt_node_interface.cpp
     src/scene_module/lane_following/lane_following_module.cpp
-    src/scene_module/pull_over/pull_over_module.cpp
     ${common_src}
   )
 

planning/behavior_path_planner/README.md

@@ -16,16 +16,16 @@ The `behavior_path_planner` module is responsible to generate
 
 Behavior path planner has following scene modules.
 
-| Name                 | Description                                                                                                                                                                | Details                                                    |
-| :------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------------------------- |
-| Lane Following       | this module generates reference path from lanelet centerline.                                                                                                              | LINK                                                       |
-| Avoidance            | this module generates avoidance path when there is objects that should be avoid.                                                                                           | [LINK](./docs/behavior_path_planner_avoidance_design.md)   |
-| Avoidance By LC      | this module generates lane change path when there is objects that should be avoid.                                                                                         | LINK                                                       |
-| Lane Change          | this module is performed when it is necessary and a collision check with other vehicles is cleared.                                                                        | [LINK](./docs/behavior_path_planner_lane_change_design.md) |
-| External Lane Change | WIP                                                                                                                                                                        | LINK                                                       |
-| Pull Over            | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal.                                         | [LINK](./docs/behavior_path_planner_pull_over_design.md)   |
-| Pull Out             | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_pull_out_design.md)    |
-| Side Shift           | (for remote control) shift the path to left or right according to an external instruction.                                                                                 | [LINK](./docs/behavior_path_planner_side_shift_design.md)  |
+| Name                 | Description                                                                                                                                                                | Details                                                     |
+| :------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------- |
+| Lane Following       | this module generates reference path from lanelet centerline.                                                                                                              | LINK                                                        |
+| Avoidance            | this module generates avoidance path when there is objects that should be avoid.                                                                                           | [LINK](./docs/behavior_path_planner_avoidance_design.md)    |
+| Avoidance By LC      | this module generates lane change path when there is objects that should be avoid.                                                                                         | LINK                                                        |
+| Lane Change          | this module is performed when it is necessary and a collision check with other vehicles is cleared.                                                                        | [LINK](./docs/behavior_path_planner_lane_change_design.md)  |
+| External Lane Change | WIP                                                                                                                                                                        | LINK                                                        |
+| Goal Planner         | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal.                                         | [LINK](./docs/behavior_path_planner_goal_planner_design.md) |
+| Pull Out             | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_pull_out_design.md)     |
+| Side Shift           | (for remote control) shift the path to left or right according to an external instruction.                                                                                 | [LINK](./docs/behavior_path_planner_side_shift_design.md)   |
 
 ![behavior_modules](./image/behavior_modules.png)
 
@@ -91,13 +91,13 @@ The current behavior tree structure is shown below. Each modules (LaneChange, Av
 
 ### input
 
-| Name                           | Type                                                   | Description                                                                        |
-| :----------------------------- | :----------------------------------------------------- | :--------------------------------------------------------------------------------- |
-| ~/input/route                  | `autoware_auto_mapping_msgs::msg::LaneletRoute`        | current route from start to goal.                                                  |
-| ~/input/vector_map             | `autoware_auto_mapping_msgs::msg::HADMapBin`           | map information.                                                                   |
-| ~/input/objects                | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module.                                            |
-| ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid`                         | occupancy grid map from perception module. This is used for only Pull Over module. |
-| ~/input/kinematic_state        | `nav_msgs::msg::Odometry`                              | for ego velocity.                                                                  |
+| Name                           | Type                                                   | Description                                                                           |
+| :----------------------------- | :----------------------------------------------------- | :------------------------------------------------------------------------------------ |
+| ~/input/route                  | `autoware_auto_mapping_msgs::msg::LaneletRoute`        | current route from start to goal.                                                     |
+| ~/input/vector_map             | `autoware_auto_mapping_msgs::msg::HADMapBin`           | map information.                                                                      |
+| ~/input/objects                | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module.                                               |
+| ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid`                         | occupancy grid map from perception module. This is used for only Goal Planner module. |
+| ~/input/kinematic_state        | `nav_msgs::msg::Odometry`                              | for ego velocity.                                                                     |
 
 ## General features of behavior path planner
 

planning/behavior_path_planner/config/pull_over/pull_over.param.yaml → planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    pull_over:
+    goal_planner:
       minimum_request_length: 100.0
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01

planning/behavior_path_planner/config/scene_module_manager.param.yaml

@@ -45,7 +45,7 @@
       priority: 2
       max_module_size: 1
 
-    pull_over:
+    goal_planner:
       enable_module: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false

planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md

@@ -57,7 +57,7 @@ This section gives details of the generation of the drivable area (`left_bound`
 
 ### Drivable Lanes Generation
 
-Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Pull Over`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes.
+Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Goal Planner`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes.
 
 ```cpp
 struct DrivalbleLanes

planning/behavior_path_planner/docs/behavior_path_planner_pull_over_design.md → planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md

@@ -1,14 +1,14 @@
-# Pull Over design
+# Goal Planner design
 
 ## Purpose / Role
 
-Search for a space where there are no objects and pull over there.
+Search for a space where there are no objects and goal planner there.
 
 ## Design
 
 ```plantuml
 @startuml
-package pull_over{
+package goal_planner{
     abstract class PullOverPlannerBase {
     }
     abstract class GoalSeacherBase {
@@ -29,7 +29,7 @@ package pull_over{
     class GoalSeacher {
     }
 
-    class PullOverModule {
+    class GoalPlannerModule {
     }
 
 
@@ -54,7 +54,7 @@ package freespace_planning_algorithms
     class RRTStar{}
 }
 
-' pull over
+' goal planner
 ShiftPullOver --|> PullOverPlannerBase
 GeometricPullOver --|> PullOverPlannerBase
 FreeSpacePullOver --|> PullOverPlannerBase
@@ -65,16 +65,16 @@ GeometricParallelParking --o GeometricPullOver
 AstarSearch --o FreeSpacePullOver
 RRTStar --o FreeSpacePullOver
 
-PullOverPlannerBase --o PullOverModule
-GoalSeacherBase --o PullOverModule
+PullOverPlannerBase --o GoalPlannerModule
+GoalSeacherBase --o GoalPlannerModule
 
 PullOverPath --o PullOverPlannerBase
 GoalCandidates --o GoalSeacherBase
 
 @enduml
 ```
 
-## General parameters for pull_over
+## General parameters for goal_planner
 
 | Name                       | Unit   | Type   | Description                                                                                                                             | Default value |
 | :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |

planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md

@@ -53,7 +53,7 @@ When turning on the blinker, it decides whether or not to turn on the specified
 
 ![activation_distance](../image/turn_signal_decider/activation_distance.drawio.svg)
 
-For left turn, right turn, avoidance, lane change, pull over and pull out, we define these two sections, which are elaborated in the following part.
+For left turn, right turn, avoidance, lane change, goal planner and pull out, we define these two sections, which are elaborated in the following part.
 
 #### 1. Left and Right turn
 
@@ -141,7 +141,7 @@ Second section
 
 ![section_pull_out](../image/turn_signal_decider/pull_out.drawio.svg)
 
-#### 5. Pull over
+#### 5. Goal Planner
 
 - desired start point  
   `v * turn_signal_search_time` meters before the start point of the pull over path.

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -21,29 +21,29 @@
 #ifdef USE_OLD_ARCHITECTURE
 #include "behavior_path_planner/behavior_tree_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
+#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
 #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
 #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
-#include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp"
 #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
 #else
 #include "behavior_path_planner/planner_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp"
+#include "behavior_path_planner/scene_module/goal_planner/manager.hpp"
 #include "behavior_path_planner/scene_module/lane_change/manager.hpp"
 #include "behavior_path_planner/scene_module/pull_out/manager.hpp"
-#include "behavior_path_planner/scene_module/pull_over/manager.hpp"
 #include "behavior_path_planner/scene_module/side_shift/manager.hpp"
 #endif
 
 #include "behavior_path_planner/steering_factor_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.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/goal_planner/goal_planner_parameters.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"
@@ -152,7 +152,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   std::shared_ptr<SideShiftParameters> side_shift_param_ptr_;
   std::shared_ptr<LaneChangeParameters> lane_change_param_ptr_;
   std::shared_ptr<PullOutParameters> pull_out_param_ptr_;
-  std::shared_ptr<PullOverParameters> pull_over_param_ptr_;
+  std::shared_ptr<GoalPlannerParameters> goal_planner_param_ptr_;
 
   BehaviorPathPlannerParameters getCommonParam();
 
@@ -163,7 +163,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   AvoidanceParameters getAvoidanceParam();
   LaneChangeParameters getLaneChangeParam();
   SideShiftParameters getSideShiftParam();
-  PullOverParameters getPullOverParam();
+  GoalPlannerParameters getGoalPlannerParam();
   PullOutParameters getPullOutParam();
   AvoidanceByLCParameters getAvoidanceByLCParam(
     const std::shared_ptr<AvoidanceParameters> & avoidance_param,

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -36,7 +36,7 @@ struct BehaviorPathPlannerParameters
   ModuleConfigParameters config_avoidance;
   ModuleConfigParameters config_avoidance_by_lc;
   ModuleConfigParameters config_pull_out;
-  ModuleConfigParameters config_pull_over;
+  ModuleConfigParameters config_goal_planner;
   ModuleConfigParameters config_side_shift;
   ModuleConfigParameters config_lane_change_left;
   ModuleConfigParameters config_lane_change_right;

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -105,6 +105,15 @@ class PlannerManager
       manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishDebugMarker(); });
   }
 
+  /**
+   * @brief publish all registered modules' virtual wall.
+   */
+  void publishVirtualWall() const
+  {
+    std::for_each(
+      manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishVirtualWall(); });
+  }
+
   /**
    * @brief get manager pointers.
    * @return manager pointers.