Merge pull request #561 from tier4/sync-upstream

chore: sync upstream

common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp

@@ -66,8 +66,8 @@ std::string getModuleName(const uint8_t module_type)
     case Module::GOAL_PLANNER: {
       return "goal_planner";
     }
-    case Module::PULL_OUT: {
-      return "pull_out";
+    case Module::START_PLANNER: {
+      return "start_planner";
     }
     case Module::TRAFFIC_LIGHT: {
       return "traffic_light";
@@ -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::GOAL_PLANNER || module_type == Module::PULL_OUT) {
+    module_type == Module::GOAL_PLANNER || module_type == Module::START_PLANNER) {
     return true;
   }
   return false;

common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp

@@ -232,8 +232,8 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray::
         case SteeringFactor::STATION:
           label->setText("STATION");
           break;
-        case SteeringFactor::PULL_OUT:
-          label->setText("PULL_OUT");
+        case SteeringFactor::START_PLANNER:
+          label->setText("START_PLANNER");
           break;
         case SteeringFactor::GOAL_PLANNER:
           label->setText("GOAL_PLANNER");

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -24,22 +24,13 @@
   <arg name="dynamic_avoidance_param_path"/>
   <arg name="lane_change_param_path"/>
   <arg name="goal_planner_param_path"/>
-  <arg name="pull_out_param_path"/>
+  <arg name="start_planner_param_path"/>
   <arg name="drivable_area_expansion_param_path"/>
   <arg name="behavior_path_planner_param_path"/>
   <!-- behavior velocity planner -->
   <arg name="behavior_velocity_smoother_type_param_path"/>
-  <arg name="blind_spot_param_path"/>
-  <arg name="crosswalk_param_path"/>
-  <arg name="detection_area_param_path"/>
-  <arg name="intersection_param_path"/>
-  <arg name="stop_line_param_path"/>
-  <arg name="traffic_light_param_path"/>
-  <arg name="virtual_traffic_light_param_path"/>
-  <arg name="occlusion_spot_param_path"/>
-  <arg name="no_stopping_area_param_path"/>
-  <arg name="run_out_param_path"/>
-  <arg name="behavior_velocity_planner_param_path"/>
+  <arg name="behavior_velocity_planner_common_param_path"/>
+  <arg name="behavior_velocity_planner_module_param_paths"/>
   <!-- parking -->
   <arg name="freespace_planner_param_path"/>
   <!-- motion -->

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

@@ -55,8 +55,8 @@ def launch_setup(context, *args, **kwargs):
         lane_change_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("start_planner_param_path").perform(context), "r") as f:
+        start_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f:
         drivable_area_expansion_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(LaunchConfiguration("scene_module_manager_param_path").perform(context), "r") as f:
@@ -96,7 +96,7 @@ def launch_setup(context, *args, **kwargs):
             dynamic_avoidance_param,
             lane_change_param,
             goal_planner_param,
-            pull_out_param,
+            start_planner_param,
             drivable_area_expansion_param,
             scene_module_manager_param,
             behavior_path_planner_param,
@@ -131,34 +131,22 @@ def launch_setup(context, *args, **kwargs):
         behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
     # behavior velocity planner
-    with open(LaunchConfiguration("blind_spot_param_path").perform(context), "r") as f:
-        blind_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("crosswalk_param_path").perform(context), "r") as f:
-        crosswalk_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("detection_area_param_path").perform(context), "r") as f:
-        detection_area_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("intersection_param_path").perform(context), "r") as f:
-        intersection_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("stop_line_param_path").perform(context), "r") as f:
-        stop_line_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("traffic_light_param_path").perform(context), "r") as f:
-        traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("virtual_traffic_light_param_path").perform(context), "r") as f:
-        virtual_traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("occlusion_spot_param_path").perform(context), "r") as f:
-        occlusion_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("no_stopping_area_param_path").perform(context), "r") as f:
-        no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("run_out_param_path").perform(context), "r") as f:
-        run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("speed_bump_param_path").perform(context), "r") as f:
-        speed_bump_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(LaunchConfiguration("out_of_lane_param_path").perform(context), "r") as f:
-        out_of_lane_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    with open(
-        LaunchConfiguration("behavior_velocity_planner_param_path").perform(context), "r"
-    ) as f:
-        behavior_velocity_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    behavior_velocity_planner_common_param_path = LaunchConfiguration(
+        "behavior_velocity_planner_common_param_path"
+    ).perform(context)
+    behavior_velocity_planner_module_param_paths = LaunchConfiguration(
+        "behavior_velocity_planner_module_param_paths"
+    ).perform(context)
+
+    behavior_velocity_planner_params_paths = [
+        behavior_velocity_planner_common_param_path,
+        *yaml.safe_load(behavior_velocity_planner_module_param_paths),
+    ]
+
+    behavior_velocity_planner_params = {}
+    for path in behavior_velocity_planner_params_paths:
+        with open(path) as f:
+            behavior_velocity_planner_params.update(yaml.safe_load(f)["/**"]["ros__parameters"])
 
     behavior_velocity_planner_component = ComposableNode(
         package="behavior_velocity_planner",
@@ -210,20 +198,8 @@ def launch_setup(context, *args, **kwargs):
         ],
         parameters=[
             nearest_search_param,
-            behavior_velocity_planner_param,
-            blind_spot_param,
-            crosswalk_param,
-            detection_area_param,
-            intersection_param,
-            stop_line_param,
-            traffic_light_param,
-            virtual_traffic_light_param,
-            occlusion_spot_param,
-            no_stopping_area_param,
+            behavior_velocity_planner_params,
             vehicle_param,
-            run_out_param,
-            speed_bump_param,
-            out_of_lane_param,
             common_param,
             motion_velocity_smoother_param,
             behavior_velocity_smoother_type_param,
@@ -244,17 +220,10 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # This condition is true if run_out module is enabled and its detection method is Points
-    launch_run_out_with_points_method = PythonExpression(
-        [
-            LaunchConfiguration(
-                "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"]
-            ),
-            " and ",
-            "'",
-            run_out_param["run_out"]["detection_method"],
-            "' == 'Points'",
-        ]
-    )
+    run_out_module = "behavior_velocity_planner::RunOutModulePlugin"
+    run_out_method = behavior_velocity_planner_params.get("run_out", {}).get("detection_method")
+    launch_run_out = run_out_module in behavior_velocity_planner_params["launch_modules"]
+    launch_run_out_with_points_method = launch_run_out and run_out_method == "Points"
 
     # load compare map for run_out module
     load_compare_map = IncludeLaunchDescription(
@@ -270,7 +239,7 @@ def launch_setup(context, *args, **kwargs):
             "use_multithread": "true",
         }.items(),
         # launch compare map only when run_out module is enabled and detection method is Points
-        condition=IfCondition(launch_run_out_with_points_method),
+        condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))),
     )
 
     load_vector_map_inside_area_filter = IncludeLaunchDescription(
@@ -287,7 +256,7 @@ def launch_setup(context, *args, **kwargs):
             "polygon_type": "no_obstacle_segmentation_area_for_run_out",
         }.items(),
         # launch vector map filter only when run_out module is enabled and detection method is Points
-        condition=IfCondition(launch_run_out_with_points_method),
+        condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))),
     )
 
     group = GroupAction(

launch/tier4_planning_launch/package.xml

@@ -9,7 +9,7 @@
   <maintainer email="zulfaqar.azmi@tier4.jp">Zulfaqar Azmi</maintainer>
   <!-- goal planner module -->
   <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
-  <!-- pull out module -->
+  <!-- start planner module -->
   <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
   <!-- side shift module -->
   <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>

perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz

@@ -1225,15 +1225,15 @@ Visualization Manager:
                           Value: false
                         - Class: rviz_default_plugins/MarkerArray
                           Enabled: false
-                          Name: PullOut
+                          Name: StartPlanner
                           Namespaces:
                             {}
                           Topic:
                             Depth: 5
                             Durability Policy: Volatile
                             History Policy: Keep Last
                             Reliability Policy: Reliable
-                            Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullout
+                            Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner
                           Value: false
                         - Class: rviz_default_plugins/MarkerArray
                           Enabled: false

perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz

@@ -1226,15 +1226,15 @@ Visualization Manager:
                           Value: false
                         - Class: rviz_default_plugins/MarkerArray
                           Enabled: false
-                          Name: PullOut
+                          Name: StartPlanner
                           Namespaces:
                             {}
                           Topic:
                             Depth: 5
                             Durability Policy: Volatile
                             History Policy: Keep Last
                             Reliability Policy: Reliable
-                            Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullout
+                            Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner
                           Value: false
                         - Class: rviz_default_plugins/MarkerArray
                           Enabled: false

planning/behavior_path_planner/CMakeLists.txt

@@ -15,7 +15,7 @@ set(common_src
   src/scene_module/scene_module_visitor.cpp
   src/scene_module/avoidance/avoidance_module.cpp
   src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
-  src/scene_module/pull_out/pull_out_module.cpp
+  src/scene_module/start_planner/start_planner_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
@@ -35,9 +35,9 @@ set(common_src
   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
+  src/utils/start_planner/util.cpp
+  src/utils/start_planner/shift_pull_out.cpp
+  src/utils/start_planner/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
@@ -67,7 +67,7 @@ else()
     src/planner_manager.cpp
     src/scene_module/avoidance/manager.cpp
     src/scene_module/dynamic_avoidance/manager.cpp
-    src/scene_module/pull_out/manager.cpp
+    src/scene_module/start_planner/manager.cpp
     src/scene_module/goal_planner/manager.cpp
     src/scene_module/side_shift/manager.cpp
     src/scene_module/lane_change/manager.cpp

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                                                        |
-| 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)   |
+| 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_start_planner_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)
 

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -17,6 +17,7 @@
       enable_force_avoidance_for_stopped_vehicle: false
       enable_safety_check: true
       enable_yield_maneuver: true
+      enable_yield_maneuver_during_shifting: false
       disable_path_update: false
       use_hatched_road_markings: false
 

planning/behavior_path_planner/config/behavior_path_planner_tree.xml

@@ -8,8 +8,8 @@
                 <Action ID="GoalPlanner_Plan" output="{output}"/>
             </ReactiveSequence>
             <ReactiveSequence>
-                <Condition ID="PullOut_Request"/>
-                <Action ID="PullOut_Plan" output="{output}"/>
+                <Condition ID="StartPlanner_Request"/>
+                <Action ID="StartPlanner_Plan" output="{output}"/>
             </ReactiveSequence>
             <ReactiveSequence>
                 <Condition ID="SideShift_Request"/>
@@ -57,10 +57,10 @@
             <output_port name="output" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
         </Action>
         <Condition ID="LaneFollowing_Request"/>
-        <Action ID="PullOut_Plan">
+        <Action ID="StartPlanner_Plan">
             <output_port name="output"/>
         </Action>
-        <Condition ID="PullOut_Request"/>
+        <Condition ID="StartPlanner_Request"/>
         <Action ID="GoalPlanner_Plan">
             <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
         </Action>