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 @@ + +