feat(behavior_velocity): add run out module (autowarefoundation#339)

* feat(behavior_velocity): add parameter yaml for behavior_velocity_planner Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * feat(behavior_velocity): add run out module Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
TomohitoAndo · Jun 1, 2022 · d702502 · d702502
1 parent 5adf891
commit d702502
Show file tree

Hide file tree

Showing 10 changed files with 274 additions and 25 deletions.
diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
@@ -78,7 +78,10 @@
 
   <!-- Planning -->
   <group>
-    <include file="$(find-pkg-share planning_launch)/launch/planning.launch.xml"/>
+    <include file="$(find-pkg-share planning_launch)/launch/planning.launch.xml">
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+    </include>
   </group>
 
   <!-- Control -->

diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml
@@ -94,7 +94,10 @@
 
   <!-- Planning -->
   <group>
-    <include file="$(find-pkg-share planning_launch)/launch/planning.launch.xml" if="$(var planning)"/>
+    <include file="$(find-pkg-share planning_launch)/launch/planning.launch.xml" if="$(var planning)">
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+    </include>
   </group>
 
   <!-- Control -->

diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
@@ -1117,6 +1117,18 @@ Visualization Manager:
                             Reliability Policy: Reliable
                             Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light
                           Value: true
+                        - Class: rviz_default_plugins/MarkerArray
+                          Enabled: true
+                          Name: VirtualWall (RunOut)
+                          Namespaces:
+                            {}
+                          Topic:
+                            Depth: 5
+                            Durability Policy: Volatile
+                            History Policy: Keep Last
+                            Reliability Policy: Reliable
+                            Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out
+                          Value: true
                       Enabled: true
                       Name: VirtualWall
                     - Class: rviz_common/Group
@@ -1241,6 +1253,18 @@ Visualization Manager:
                             Reliability Policy: Reliable
                             Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area
                           Value: false
+                        - Class: rviz_default_plugins/MarkerArray
+                          Enabled: false
+                          Name: RunOut
+                          Namespaces:
+                            {}
+                          Topic:
+                            Depth: 5
+                            Durability Policy: Volatile
+                            History Policy: Keep Last
+                            Reliability Policy: Reliable
+                            Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out
+                          Value: false
                       Enabled: false
                       Name: DebugMarker
                   Enabled: true

diff --git a/..._driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/..._driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml
@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    launch_stop_line: true
+    launch_crosswalk: true
+    launch_traffic_light: true
+    launch_intersection: true
+    launch_blind_spot: true
+    launch_detection_area: true
+    launch_virtual_traffic_light: true
+    launch_occlusion_spot: true
+    launch_no_stopping_area: true
+    launch_run_out: false
+    forward_path_length: 1000.0
+    backward_path_length: 5.0
+    max_accel: -2.8
+    max_jerk: -5.0
+    system_delay: 0.5
+    delay_response_time: 1.3
diff --git a/...ario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/...ario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
@@ -0,0 +1,45 @@
+/**:
+  ros__parameters:
+    run_out:
+      detection_method: "Object"  # [-] candidate: Object, ObjectWithoutPath, Points
+      use_partition_lanelet: true # [-] whether to use partition lanelet map data
+      specify_decel_jerk: false   # [-] whether to specify jerk when ego decelerates
+      stop_margin: 2.5            # [m] the vehicle decelerates to be able to stop with this margin
+      passing_margin: 1.1         # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin
+      deceleration_jerk: -0.3     # [m/s^3] ego decelerates with this jerk when stopping for obstacles
+      obstacle_velocity_kph: 5.0  # [km/h] assumption for obstacle velocity
+      detection_distance: 45.0    # [m] ahead distance from ego to detect the obstacles
+      detection_span: 1.0         # [m] calculate collision with this span to reduce calculation time
+      min_vel_ego_kmph: 3.6       # [km/h] min velocity to calculate time to collision
+
+      # rectangle detection area for Points method
+      # this will be replaced with more appropriate detection area
+      detection_area_size:
+        dist_ahead: 50.0 # [m] ahead distance from ego position
+        dist_behind: 5.0 # [m] behind distance from ego position
+        dist_right: 7.0  # [m] right distance from ego position
+        dist_left: 7.0   # [m] left distance from ego position
+
+      # parameter to create abstracted dynamic obstacles
+      dynamic_obstacle:
+        min_vel_kmph: 0.0         # [km/h] minimum velocity for dynamic obstacles
+        max_vel_kmph: 5.0         # [km/h] maximum velocity for dynamic obstacles
+        diameter: 0.1             # [m] diameter of obstacles. used for creating dynamic obstacles from points
+        height: 2.0               # [m] height of obstacles. used for creating dynamic obstacles from points
+        max_prediction_time: 10.0 # [sec] create predicted path until this time
+        time_step: 0.5            # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
+
+      # approach if ego has stopped in the front of the obstacle for a certain amount of time
+      approaching:
+        enable: false
+        margin: 0.0           # [m] distance on how close ego approaches the obstacle
+        limit_vel_kmph: 3.0   # [km/h] limit velocity for approaching after stopping
+        stop_thresh: 0.01     # [m/s] threshold to decide if ego is stopping
+        stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state
+        dist_thresh: 0.5      # [m] end the approaching state if distance to the obstacle is longer than stop_margin + dist_thresh
+
+      # parameter to avoid sudden stopping
+      slow_down_limit:
+        enable: true
+        max_jerk: -0.7  # [m/s^3] minimum jerk deceleration for safe brake.
+        max_acc : -2.0  # [m/s^2] minimum accel deceleration for safe brake.
diff --git a/planning_launch/launch/planning.launch.xml b/planning_launch/launch/planning.launch.xml
@@ -1,5 +1,10 @@
 <launch>
   <!-- planning module -->
+
+  <!-- pointcloud container -->
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+
   <group>
     <push-ros-namespace namespace="planning"/>
     <!-- mission planning module -->
@@ -13,6 +18,8 @@
     <group>
       <push-ros-namespace namespace="scenario_planning"/>
       <include file="$(find-pkg-share planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
+        <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+        <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
       </include>
     </group>
 

diff --git a/planning_launch/launch/scenario_planning/lane_driving.launch.xml b/planning_launch/launch/scenario_planning/lane_driving.launch.xml
@@ -2,6 +2,10 @@
   <!-- common param -->
   <arg name="common_param_path"/>
 
+  <!-- pointcloud container -->
+  <arg name="use_pointcloud_container"/>
+  <arg name="pointcloud_container_name"/>
+
   <!-- lane_driving scenario -->
   <group>
     <push-ros-namespace namespace="lane_driving"/>
@@ -10,6 +14,8 @@
       <push-ros-namespace namespace="behavior_planning"/>
       <include file="$(find-pkg-share planning_launch)/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py">
         <arg name="use_multithread" value="true" />
+        <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+        <arg name="container_name" value="$(var pointcloud_container_name)"/>
       </include>
     </group>
 

diff --git a/...aunch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/...aunch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -18,10 +18,13 @@
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
 from launch.actions import SetLaunchConfiguration
 from launch.conditions import IfCondition
 from launch.conditions import UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PythonExpression
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
 from launch_ros.substitutions import FindPackageShare
@@ -190,27 +193,27 @@ def generate_launch_description():
     with open(common_param_path, "r") as f:
         common_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
-    base_param_path = os.path.join(
+    motion_velocity_smoother_param_path = os.path.join(
         get_package_share_directory("planning_launch"),
         "config",
         "scenario_planning",
         "common",
         "motion_velocity_smoother",
         "motion_velocity_smoother.param.yaml",
     )
-    with open(base_param_path, "r") as f:
-        base_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    with open(motion_velocity_smoother_param_path, "r") as f:
+        motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
-    smoother_param_path = os.path.join(
+    smoother_type_param_path = os.path.join(
         get_package_share_directory("planning_launch"),
         "config",
         "scenario_planning",
         "common",
         "motion_velocity_smoother",
         "Analytical.param.yaml",
     )
-    with open(smoother_param_path, "r") as f:
-        smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    with open(smoother_type_param_path, "r") as f:
+        smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
     # behavior velocity planner
     blind_spot_param_path = os.path.join(
@@ -321,6 +324,30 @@ def generate_launch_description():
     with open(no_stopping_area_param_path, "r") as f:
         no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
+    run_out_param_path = os.path.join(
+        get_package_share_directory("planning_launch"),
+        "config",
+        "scenario_planning",
+        "lane_driving",
+        "behavior_planning",
+        "behavior_velocity_planner",
+        "run_out.param.yaml",
+    )
+    with open(run_out_param_path, "r") as f:
+        run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+    behavior_velocity_planner_param_path = os.path.join(
+        get_package_share_directory("planning_launch"),
+        "config",
+        "scenario_planning",
+        "lane_driving",
+        "behavior_planning",
+        "behavior_velocity_planner",
+        "behavior_velocity_planner.param.yaml",
+    )
+    with open(behavior_velocity_planner_param_path, "r") as f:
+        behavior_velocity_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
     behavior_velocity_planner_component = ComposableNode(
         package="behavior_velocity_planner",
         plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode",
@@ -335,6 +362,10 @@ def generate_launch_description():
                 "~/input/no_ground_pointcloud",
                 "/perception/obstacle_segmentation/pointcloud",
             ),
+            (
+                "~/input/compare_map_filtered_pointcloud",
+                "compare_map_filtered/pointcloud",
+            ),
             (
                 "~/input/traffic_signals",
                 "/perception/traffic_light_recognition/traffic_signals",
@@ -361,24 +392,10 @@ def generate_launch_description():
             ("~/output/traffic_signal", "debug/traffic_signal"),
         ],
         parameters=[
-            {
-                "launch_stop_line": True,
-                "launch_crosswalk": True,
-                "launch_traffic_light": True,
-                "launch_intersection": True,
-                "launch_blind_spot": True,
-                "launch_detection_area": True,
-                "launch_virtual_traffic_light": True,
-                "launch_occlusion_spot": True,
-                "launch_no_stopping_area": True,
-                "forward_path_length": 1000.0,
-                "backward_path_length": 5.0,
-                "max_accel": -2.8,
-                "delay_response_time": 1.3,
-            },
+            behavior_velocity_planner_param,
             common_param,
-            base_param,
-            smoother_param,
+            motion_velocity_smoother_param,
+            smoother_type_param,
             blind_spot_param,
             crosswalk_param,
             detection_area_param,
@@ -388,6 +405,7 @@ def generate_launch_description():
             virtual_traffic_light_param,
             occlusion_spot_param,
             no_stopping_area_param,
+            run_out_param,
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
@@ -415,6 +433,35 @@ def generate_launch_description():
         condition=IfCondition(LaunchConfiguration("use_multithread")),
     )
 
+    # load compare map for run out module
+    load_compare_map = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                FindPackageShare("planning_launch"),
+                "/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py",
+            ]
+        ),
+        launch_arguments={
+            "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
+            "container_name": LaunchConfiguration("container_name"),
+            "use_multithread": "true",
+        }.items(),
+        # launch compare map only when run_out module is enabled and detection method is Points
+        condition=IfCondition(
+            PythonExpression(
+                [
+                    LaunchConfiguration(
+                        "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"]
+                    ),
+                    " and ",
+                    "'",
+                    run_out_param["run_out"]["detection_method"],
+                    "' == 'Points'",
+                ]
+            )
+        ),
+    )
+
     return launch.LaunchDescription(
         [
             DeclareLaunchArgument(
@@ -426,6 +473,7 @@ def generate_launch_description():
             set_container_executable,
             set_container_mt_executable,
             container,
+            load_compare_map,
             ExecuteProcess(
                 cmd=[
                     "ros2",