feat(behavior_velocity): add run out module (#752)

* fix(behavior_velocity): calculate detection area from the nearest point from ego (#730) * fix(behavior_velocity): calculate lateral distance from the beginning of the path Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add argument of min_velocity Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * use veloicty from the nearest point from ego Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * pass struct by reference Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix to interpolate point in util Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix(longitudinal_controller_node, vehicle_cmd_gate): update stopped condition and behavior (#700) * fix(longitudinal_controller_node): parameterize stopped state entry condition Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(longitudinal_controller_node): simply set stopped velocity in STOPPED STATE Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(vehicle_cmd_gate): check time duration since the vehicle stopped Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * docs(autoware_testing): fix link (#741) * docs(autoware_testing): fix link * fix typo Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix: trajectory visualizer (#745) Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix(tier4_autoware_utils): modify build error in rolling (#720) * fix(tier4_autoware_utils): modify build error in rolling Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(lanelet2_extension): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(ekf_localizer): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(freespace_planning_algorithms): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(interpolation): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(freespace_planner): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(lane_departure_checker): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(map_based_prediction): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(ground_segmentation): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(motion_velocity_smoother): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(multi_object_tracker): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(trajectory_follower): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(control_performance_analysis): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(detected_object_validation): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(goal_distance_calculator): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(ndt_scan_matcher): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(route_handler): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(behavior_path_planner): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(mission_planner): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(obstacle_avoidance_planner): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(obstacle_stop_planner): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(obstacle_collision_checker): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(shape_estimation): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(behavior_velocity_planner): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(path_distance_calculator): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(detection_by_tracker): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(surround_obstacle_checker): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(probabilistic_occupancy_grid_map): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(tier4_debug_tools): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(tier4_vehicle_rviz_plugin): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(pure_pursuit): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(trajectory_follower_nodes): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(occupancy_grid_map_outlier_filter): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(traffic_light_map_based_detector): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(planning_error_monitor): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(planning_evaluator): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix(lidar_centerpoint): add target compile definition for geometry2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix(behavior_velocity): handle the case when finding index failed (#746) Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * feat: add scene module of dynamic obstacle stop Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix warnings Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add temporary debug value Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add feature to go after stopping Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix parameter namespace Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * use planner util Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix calculation when multiple obstacles are detected in one step polygon Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unnecessary debug Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add option to apply limit jerk Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * Modify parameter name Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * Add param file Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unnecessary comments Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add feature to exclude obstacles outside of partition Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * modify search distance for partitions Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * apply voxel grid filter to input points Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * set smoother param by passing node instance Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add parameter for velocity limit (temporary) Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add dynamic reconfigure Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add debug value Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * avoid acceleration when stopping for obstacles Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix lateral distance to publish distance from vehicle side Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * modify the location to publish longitudinal distance Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix calculation of stop index Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * use current velocity for stop dicision Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add dynamic parameter for slow down limit Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add debug value to display passing dist Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * modify stopping velocity Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * update param Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * use smoother in planner data Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * use path with lane id instead of trajectory Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unnecessary data check Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * extend path to consider obstacles after the end of the path Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * rename public member variables Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unused paramter Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * create detection area using util Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix visualization of stop distance marker Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * make option for detection method easier to understand Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove parameter about whether to enable this feature Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * calculate and publish debug data in function Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * use compare map filtered points Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add comment Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix visualization of detection area when calculation of stop distance failed Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add option whether to specify the jerk Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix format Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * change parameter name Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove dynamic reconfigure Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * delete unused file Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unnecessary comments Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unnecessary includes Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add launcher for compare map Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add launch and config for dynamic obstacle stop planner Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix finding package name Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * handle the change of util Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * relay points for simulation Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * update parameter Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix position and color of stop line marker Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * pre-commit fixes Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unnecessary comments Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix Copyright Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix Copyright Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix typo Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add documents for dynamic obstacle stop module Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * ci(pre-commit): autofix Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * update documents Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * docs: begin a sentence with a capital letter Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * docs: replace predicted with smoothed for path velocity Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * create interface class to switch method Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * run compare map filter only when points method is used Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * delete unused functions Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * rename functions for inserting velocity Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * rename parameter of path_size to max_prediction_time Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix(behavior_velocity_planner): dynamic obstacle stop planner docs Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix(behavior_velocity_planner): add ego vehicle description Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix(behavior_velocity_planner): change space to hyphen Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * change smoothed to expected target velocity Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * Start a sentence in figure with a capital letter Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix typo Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * use voxel distance based compare map Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * select detection method from param file Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * do not launch as default for now Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * rename dynamic_obstacle_stop to run_out Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unnecessary change Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unnecessary changes Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * remove unnecessary changes Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix typo Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * change default to false Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * update cmake to build run_out module Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add launch_run_out parameter Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * Add note for compare map filtered points Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * handle the change for virtual wall marker Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * rename the parameters for smoother Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix build error in humble Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix build error in humble Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * launch compare map only when run out module is enabled Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * update a document Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add calculation time for modify path Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * update a document Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Esteve Fernandez <esteve.fernandez@tier4.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
autowarefoundation · Jun 1, 2022 · 82c0f7e · 82c0f7e
1 parent 47eda5f
commit 82c0f7e
Show file tree

Hide file tree

Showing 32 changed files with 3,452 additions and 9 deletions.
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
@@ -9,6 +9,7 @@
     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

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/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml
@@ -3,6 +3,10 @@
 
   <arg name="vehicle_info_param_file"/>
 
+  <!-- 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 -->
@@ -16,6 +20,8 @@
       <push-ros-namespace namespace="scenario_planning"/>
       <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
         <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
+        <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/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml
@@ -3,6 +3,10 @@
   <arg name="common_param_path"/>
   <arg name="vehicle_info_param_file"/>
 
+  <!-- pointcloud container -->
+  <arg name="use_pointcloud_container"/>
+  <arg name="pointcloud_container_name"/>
+
   <!-- lane_driving scenario -->
   <group>
     <push-ros-namespace namespace="lane_driving"/>
@@ -12,6 +16,8 @@
       <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py">
         <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
         <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
@@ -19,11 +19,14 @@
 from launch.actions import DeclareLaunchArgument
 from launch.actions import ExecuteProcess
 from launch.actions import GroupAction
+from launch.actions import IncludeLaunchDescription
 from launch.actions import OpaqueFunction
 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
@@ -188,6 +191,39 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # smoother param
+    common_param_path = os.path.join(
+        get_package_share_directory("tier4_planning_launch"),
+        "config",
+        "scenario_planning",
+        "common",
+        "common.param.yaml",
+    )
+    with open(common_param_path, "r") as f:
+        common_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+    motion_velocity_smoother_param_path = os.path.join(
+        get_package_share_directory("tier4_planning_launch"),
+        "config",
+        "scenario_planning",
+        "common",
+        "motion_velocity_smoother",
+        "motion_velocity_smoother.param.yaml",
+    )
+    with open(motion_velocity_smoother_param_path, "r") as f:
+        motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+    smoother_type_param_path = os.path.join(
+        get_package_share_directory("tier4_planning_launch"),
+        "config",
+        "scenario_planning",
+        "common",
+        "motion_velocity_smoother",
+        "Analytical.param.yaml",
+    )
+    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(
         get_package_share_directory("tier4_planning_launch"),
@@ -297,6 +333,18 @@ def launch_setup(context, *args, **kwargs):
     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("tier4_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("tier4_planning_launch"),
         "config",
@@ -323,6 +371,10 @@ def launch_setup(context, *args, **kwargs):
                 "~/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",
@@ -331,6 +383,10 @@ def launch_setup(context, *args, **kwargs):
                 "~/input/external_traffic_signals",
                 "/external/traffic_light_recognition/traffic_signals",
             ),
+            (
+                "~/input/external_velocity_limit_mps",
+                "/planning/scenario_planning/max_velocity_default",
+            ),
             ("~/input/virtual_traffic_light_states", "/awapi/tmp/virtual_traffic_light_states"),
             (
                 "~/input/occupancy_grid",
@@ -356,6 +412,10 @@ def launch_setup(context, *args, **kwargs):
             occlusion_spot_param,
             no_stopping_area_param,
             vehicle_info_param,
+            run_out_param,
+            common_param,
+            motion_velocity_smoother_param,
+            smoother_type_param,
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
@@ -372,9 +432,39 @@ def launch_setup(context, *args, **kwargs):
         output="screen",
     )
 
+    # load compare map for dynamic obstacle stop module
+    load_compare_map = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                FindPackageShare("tier4_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'",
+                ]
+            )
+        ),
+    )
+
     group = GroupAction(
         [
             container,
+            load_compare_map,
             ExecuteProcess(
                 cmd=[
                     "ros2",
@@ -420,6 +510,10 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("use_intra_process", "false", "use ROS2 component container communication")
     add_launch_arg("use_multithread", "false", "use multithread")
 
+    # for compare map
+    add_launch_arg("use_pointcloud_container", "true")
+    add_launch_arg("container_name", "pointcloud_container")
+
     set_container_executable = SetLaunchConfiguration(
         "container_executable",
         "component_container",

diff --git a/...ning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py b/...ning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py
@@ -0,0 +1,89 @@
+# Copyright 2022 TIER IV, Inc. All rights reserved.
+#
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import SetLaunchConfiguration
+from launch.conditions import IfCondition
+from launch.conditions import UnlessCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    def add_launch_arg(name: str, default_value=None):
+        return DeclareLaunchArgument(name, default_value=default_value)
+
+    set_container_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container",
+        condition=UnlessCondition(LaunchConfiguration("use_multithread")),
+    )
+
+    set_container_mt_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container_mt",
+        condition=IfCondition(LaunchConfiguration("use_multithread")),
+    )
+
+    composable_nodes = [
+        ComposableNode(
+            package="compare_map_segmentation",
+            plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent",
+            name="voxel_distance_based_compare_map_filter_node",
+            remappings=[
+                ("input", "/perception/obstacle_segmentation/pointcloud"),
+                ("map", "/map/pointcloud_map"),
+                ("output", "compare_map_filtered/pointcloud"),
+            ],
+            parameters=[
+                {
+                    "distance_threshold": 0.7,
+                }
+            ],
+            extra_arguments=[
+                {"use_intra_process_comms": False}  # this node has QoS of transient local
+            ],
+        ),
+    ]
+
+    compare_map_container = ComposableNodeContainer(
+        name=LaunchConfiguration("container_name"),
+        namespace="",
+        package="rclcpp_components",
+        executable=LaunchConfiguration("container_executable"),
+        composable_node_descriptions=composable_nodes,
+        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
+        output="screen",
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        composable_node_descriptions=composable_nodes,
+        target_container=LaunchConfiguration("container_name"),
+        condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
+    )
+
+    return LaunchDescription(
+        [
+            add_launch_arg("use_multithread", "true"),
+            add_launch_arg("use_pointcloud_container", "true"),
+            add_launch_arg("container_name", "compare_map_container"),
+            set_container_executable,
+            set_container_mt_executable,
+            compare_map_container,
+            load_composable_nodes,
+        ]
+    )
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -3,6 +3,10 @@
   <arg name="common_param_path" default="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/common/common.param.yaml"/>
   <arg name="vehicle_info_param_file"/>
 
+  <!-- pointcloud container -->
+  <arg name="use_pointcloud_container"/>
+  <arg name="pointcloud_container_name"/>
+
   <!-- scenario selector -->
   <include file="$(find-pkg-share scenario_selector)/launch/scenario_selector.launch.xml">
     <arg name="input_lane_driving_trajectory" value="/planning/scenario_planning/lane_driving/trajectory"/>
@@ -41,6 +45,8 @@
     <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving.launch.xml">
       <arg name="common_param_path" value="$(var common_param_path)"/>
       <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
     </include>
     <!-- parking -->
     <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/parking.launch.py">

diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt
@@ -8,7 +8,7 @@ autoware_package()
 find_package(Boost REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(PCL REQUIRED COMPONENTS common)
+find_package(PCL REQUIRED COMPONENTS common filters)
 find_package(OpenCV REQUIRED)
 
 set(scene_modules
@@ -21,6 +21,7 @@ set(scene_modules
   no_stopping_area
   virtual_traffic_light
   occlusion_spot
+  run_out
 )
 
 foreach(scene_module IN LISTS scene_modules)
@@ -50,6 +51,8 @@ ament_target_dependencies(behavior_velocity_planner
   PCL
 )
 
+target_link_libraries(behavior_velocity_planner ${PCL_LIBRARIES})
+
 rclcpp_components_register_node(behavior_velocity_planner
   PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode"
   EXECUTABLE behavior_velocity_planner_node