diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
index 02d6c3363..2b7bf1861 100644
--- a/autoware_launch/launch/autoware.launch.xml
+++ b/autoware_launch/launch/autoware.launch.xml
@@ -78,7 +78,10 @@
-
+
+
+
+
diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml
index 7fc575152..1cc48dbc8 100644
--- a/autoware_launch/launch/logging_simulator.launch.xml
+++ b/autoware_launch/launch/logging_simulator.launch.xml
@@ -94,7 +94,10 @@
-
+
+
+
+
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
index 5e57e3761..e36a04c7f 100644
--- 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/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml
new file mode 100644
index 000000000..1332e422c
--- /dev/null
+++ b/planning_launch/config/scenario_planning/lane_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/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
new file mode 100644
index 000000000..8818ed019
--- /dev/null
+++ b/planning_launch/config/scenario_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
index 640b3a234..f6a7abec7 100644
--- a/planning_launch/launch/planning.launch.xml
+++ b/planning_launch/launch/planning.launch.xml
@@ -1,5 +1,10 @@
+
+
+
+
+
@@ -13,6 +18,8 @@
+
+
diff --git a/planning_launch/launch/scenario_planning/lane_driving.launch.xml b/planning_launch/launch/scenario_planning/lane_driving.launch.xml
index e5d4d4c6b..ad3e35149 100644
--- a/planning_launch/launch/scenario_planning/lane_driving.launch.xml
+++ b/planning_launch/launch/scenario_planning/lane_driving.launch.xml
@@ -2,6 +2,10 @@
+
+
+
+
@@ -10,6 +14,8 @@
+
+
diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
index 8e2fe2629..931396032 100644
--- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
+++ b/planning_launch/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,7 +193,7 @@ 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",
@@ -198,10 +201,10 @@ def generate_launch_description():
"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",
@@ -209,8 +212,8 @@ def generate_launch_description():
"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",
diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py
new file mode 100644
index 000000000..fe3e347fd
--- /dev/null
+++ b/planning_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/planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
index 5304034d2..538607f04 100644
--- a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
+++ b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -2,6 +2,10 @@
+
+
+
+
@@ -39,6 +43,8 @@
+
+