diff --git a/README.md b/README.md index 13bdb38d3..dc6f60786 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,5 @@ ros2 launch autoware_launch planning_simulator.launch.xml map_path:=/path/to/map - [autoware_launch](./autoware_launch) - [control_launch](./control_launch) - [integration_launch](./integration_launch) -- [perception_launch](./perception_launch) - [planning_launch](./planning_launch) - [system_launch](./system_launch) diff --git a/autoware_api_launch/launch/autoware_api.launch.xml b/autoware_api_launch/launch/autoware_api.launch.xml index 509e0c4f9..ccc1527fb 100644 --- a/autoware_api_launch/launch/autoware_api.launch.xml +++ b/autoware_api_launch/launch/autoware_api.launch.xml @@ -1,38 +1,9 @@ - - - - - - - - - - - - - - - - - - - - - - - - + + + + - - + - - - - - - - - diff --git a/autoware_api_launch/launch/include/external_api_adaptor.launch.py b/autoware_api_launch/launch/include/external_api_adaptor.launch.py deleted file mode 100644 index 3aafd3d1b..000000000 --- a/autoware_api_launch/launch/include/external_api_adaptor.launch.py +++ /dev/null @@ -1,60 +0,0 @@ -# Copyright 2021 Tier IV, Inc. -# -# 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. - -import launch -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - - -def _create_api_node(node_name, class_name, **kwargs): - return ComposableNode( - namespace="external", - name=node_name, - package="autoware_iv_external_api_adaptor", - plugin="external_api::" + class_name, - **kwargs - ) - - -def generate_launch_description(): - components = [ - _create_api_node("calibration_status", "CalibrationStatus"), - _create_api_node("cpu_usage", "CpuUsage"), - _create_api_node("diagnostics", "Diagnostics"), - _create_api_node("door", "Door"), - _create_api_node("emergency", "Emergency"), - _create_api_node("engage", "Engage"), - _create_api_node("fail_safe_state", "FailSafeState"), - _create_api_node("initial_pose", "InitialPose"), - _create_api_node("localization_score", "LocalizationScore"), - _create_api_node("map", "Map"), - _create_api_node("operator", "Operator"), - _create_api_node("metadata_packages", "MetadataPackages"), - _create_api_node("route", "Route"), - _create_api_node("rtc_controller", "RTCController"), - _create_api_node("service", "Service"), - _create_api_node("start", "Start"), - _create_api_node("vehicle_status", "VehicleStatus"), - _create_api_node("velocity", "Velocity"), - _create_api_node("version", "Version"), - ] - container = ComposableNodeContainer( - namespace="external", - name="autoware_iv_adaptor", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=components, - output="screen", - ) - return launch.LaunchDescription([container]) diff --git a/autoware_api_launch/launch/include/internal_api_adaptor.launch.py b/autoware_api_launch/launch/include/internal_api_adaptor.launch.py deleted file mode 100644 index f1962908c..000000000 --- a/autoware_api_launch/launch/include/internal_api_adaptor.launch.py +++ /dev/null @@ -1,44 +0,0 @@ -# Copyright 2021 Tier IV, Inc. -# -# 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. - -import launch -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - - -def _create_api_node(node_name, class_name, **kwargs): - return ComposableNode( - namespace="internal", - name=node_name, - package="autoware_iv_internal_api_adaptor", - plugin="internal_api::" + class_name, - **kwargs - ) - - -def generate_launch_description(): - components = [ - _create_api_node("iv_msgs", "IVMsgs"), - _create_api_node("operator", "Operator"), - _create_api_node("velocity", "Velocity"), - ] - container = ComposableNodeContainer( - namespace="internal", - name="autoware_iv_adaptor", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=components, - output="screen", - ) - return launch.LaunchDescription([container]) diff --git a/autoware_api_launch/launch/include/internal_api_relay.launch.xml b/autoware_api_launch/launch/include/internal_api_relay.launch.xml deleted file mode 100644 index e850fb3dc..000000000 --- a/autoware_api_launch/launch/include/internal_api_relay.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_lanelet_filter.param.yaml similarity index 100% rename from perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml rename to autoware_launch/config/perception/object_recognition/detection/object_lanelet_filter.param.yaml diff --git a/perception_launch/config/object_recognition/detection/object_position_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_position_filter.param.yaml similarity index 100% rename from perception_launch/config/object_recognition/detection/object_position_filter.param.yaml rename to autoware_launch/config/perception/object_recognition/detection/object_position_filter.param.yaml diff --git a/perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/pointcloud_map_filter.param.yaml similarity index 100% rename from perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml rename to autoware_launch/config/perception/object_recognition/detection/pointcloud_map_filter.param.yaml diff --git a/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml similarity index 100% rename from perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml rename to autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml diff --git a/perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml similarity index 100% rename from perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml rename to autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml diff --git a/perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml similarity index 100% rename from perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml rename to autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index bd92c6d5e..f3d79b996 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -100,8 +100,9 @@ - - + + + @@ -134,17 +135,8 @@ - - - - - + + + diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml new file mode 100644 index 000000000..9d963a0c3 --- /dev/null +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware_launch/package.xml b/autoware_launch/package.xml index 165a1b0e7..76260f859 100644 --- a/autoware_launch/package.xml +++ b/autoware_launch/package.xml @@ -13,14 +13,15 @@ autoware_api_launch control_launch global_parameter_loader - perception_launch planning_launch python3-bson python3-tornado rviz2 simulator_launch system_launch + tier4_localization_launch tier4_map_launch + tier4_perception_launch tier4_sensing_launch tier4_vehicle_launch diff --git a/control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml b/control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml new file mode 100644 index 000000000..dcd985bb4 --- /dev/null +++ b/control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + ctrl_period: 0.03 + timeout_thr_sec: 0.5 diff --git a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml index 487669935..351ad877e 100644 --- a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -12,9 +12,11 @@ lon_jerk_lim: 5.0 lat_acc_lim: 5.0 lat_jerk_lim: 5.0 + actual_steer_diff_lim: 1.0 on_transition: vel_lim: 50.0 lon_acc_lim: 1.0 lon_jerk_lim: 0.5 lat_acc_lim: 1.2 lat_jerk_lim: 0.75 + actual_steer_diff_lim: 0.05 diff --git a/control_launch/launch/control.launch.py b/control_launch/launch/control.launch.py index 1962c8d52..0dc4d1e83 100644 --- a/control_launch/launch/control.launch.py +++ b/control_launch/launch/control.launch.py @@ -39,6 +39,11 @@ def launch_setup(context, *args, **kwargs): lon_controller_param_path = LaunchConfiguration("lon_controller_param_path").perform(context) with open(lon_controller_param_path, "r") as f: lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + trajectory_follower_node_param_path = LaunchConfiguration( + "trajectory_follower_node_param_path" + ).perform(context) + with open(trajectory_follower_node_param_path, "r") as f: + trajectory_follower_node_param = yaml.safe_load(f)["/**"]["ros__parameters"] vehicle_cmd_gate_param_path = LaunchConfiguration("vehicle_cmd_gate_param_path").perform( context ) @@ -83,6 +88,7 @@ def launch_setup(context, *args, **kwargs): "longitudinal_controller_mode": LaunchConfiguration("longitudinal_controller_mode"), }, nearest_search_param, + trajectory_follower_node_param, lon_controller_param, lat_controller_param, ], @@ -208,7 +214,12 @@ def launch_setup(context, *args, **kwargs): launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), ("target_container", "/control/control_container"), - ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")), + # This is temporary uncomment. I will replace control_launch with tier4_control_launch soon. + # ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")), + ( + "external_cmd_selector_param_path", + LaunchConfiguration("external_cmd_selector_param_path"), + ), ], ) @@ -284,7 +295,7 @@ def add_launch_arg(name: str, default_value=None, description=None): "lat_controller_param_path", [ FindPackageShare("control_launch"), - "/config/mpc_lateral_controller/mpc_lateral_controller.param.yaml", + "/config/trajectory_follower/lateral/mpc.param.yaml", ], "path to the parameter file of lateral controller. default is `mpc_follower`", ) @@ -292,7 +303,15 @@ def add_launch_arg(name: str, default_value=None, description=None): "lon_controller_param_path", [ FindPackageShare("control_launch"), - "/config/pid_longitudinal_controller/pid_longitudinal_controller.param.yaml", + "/config/trajectory_follower/longitudinal/pid.param.yaml", + ], + "path to the parameter file of longitudinal controller", + ) + add_launch_arg( + "trajectory_follower_node_param_path", + [ + FindPackageShare("control_launch"), + "/config/trajectory_follower/trajectory_follower_node.param.yaml", ], "path to the parameter file of longitudinal controller", ) @@ -332,6 +351,13 @@ def add_launch_arg(name: str, default_value=None, description=None): # external cmd selector add_launch_arg("initial_selector_mode", "remote", "local or remote") + add_launch_arg( + "external_cmd_selector_param_path", + [ + FindPackageShare("external_cmd_selector"), + "/config/external_cmd_selector.param.yaml", + ], + ) # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") diff --git a/perception_launch/CMakeLists.txt b/perception_launch/CMakeLists.txt deleted file mode 100644 index ade04cfcd..000000000 --- a/perception_launch/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(perception_launch) - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/perception_launch/README.md b/perception_launch/README.md deleted file mode 100644 index 9cc11ca6f..000000000 --- a/perception_launch/README.md +++ /dev/null @@ -1,20 +0,0 @@ -# perception_launch - -## Structure - -![perception_launch](./perception_launch.drawio.svg) - -## Package Dependencies - -Please see `` in `package.xml`. - -## Usage - -You can include as follows in `*.launch.xml` to use `perception.launch.xml`. - -```xml - - - - -``` diff --git a/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml deleted file mode 100644 index 7dc95ddec..000000000 --- a/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ /dev/null @@ -1,278 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml deleted file mode 100644 index 86e37f80d..000000000 --- a/perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ /dev/null @@ -1,76 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception_launch/launch/object_recognition/detection/detection.launch.xml b/perception_launch/launch/object_recognition/detection/detection.launch.xml deleted file mode 100644 index c4a9c00e4..000000000 --- a/perception_launch/launch/object_recognition/detection/detection.launch.xml +++ /dev/null @@ -1,122 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml deleted file mode 100644 index 9e772759d..000000000 --- a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ /dev/null @@ -1,147 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml deleted file mode 100644 index 8150315c9..000000000 --- a/perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py deleted file mode 100644 index afcf9021c..000000000 --- a/perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ /dev/null @@ -1,168 +0,0 @@ -# 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. -import os - -from ament_index_python.packages import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -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 -import yaml - - -class PointcloudMapFilterPipeline: - def __init__(self, context): - pointcloud_map_filter_param_path = os.path.join( - get_package_share_directory("perception_launch"), - "config/object_recognition/detection/pointcloud_map_filter.param.yaml", - ) - with open(pointcloud_map_filter_param_path, "r") as f: - self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] - self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] - self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] - self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] - - def create_pipeline(self): - if self.use_down_sample_filter: - return self.create_down_sample_pipeline() - else: - return self.create_normal_pipeline() - - def create_normal_pipeline(self): - components = [] - components.append( - ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name="voxel_based_compare_map_filter", - remappings=[ - ("input", LaunchConfiguration("input_topic")), - ("map", "/map/pointcloud_map"), - ("output", LaunchConfiguration("output_topic")), - ], - parameters=[ - { - "distance_threshold": self.distance_threshold, - } - ], - extra_arguments=[ - {"use_intra_process_comms": False}, - ], - ) - ) - return components - - def create_down_sample_pipeline(self): - components = [] - down_sample_topic = ( - "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud" - ) - components.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name="voxel_grid_downsample_filter", - remappings=[ - ("input", LaunchConfiguration("input_topic")), - ("output", down_sample_topic), - ], - parameters=[ - { - "voxel_size_x": self.voxel_size, - "voxel_size_y": self.voxel_size, - "voxel_size_z": self.voxel_size, - } - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ), - ) - components.append( - ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name="voxel_based_compare_map_filter", - remappings=[ - ("input", down_sample_topic), - ("map", "/map/pointcloud_map"), - ("output", LaunchConfiguration("output_topic")), - ], - parameters=[ - { - "distance_threshold": self.distance_threshold, - } - ], - extra_arguments=[ - {"use_intra_process_comms": False}, - ], - ) - ) - return components - - -def launch_setup(context, *args, **kwargs): - pipeline = PointcloudMapFilterPipeline(context) - components = [] - components.extend(pipeline.create_pipeline()) - individual_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=components, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - pointcloud_container_loader = LoadComposableNodes( - composable_node_descriptions=components, - target_container=LaunchConfiguration("container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), - ) - return [individual_container, pointcloud_container_loader] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg("input_topic", "") - add_launch_arg("output_topic", "") - add_launch_arg("use_multithread", "False") - add_launch_arg("use_intra_process", "True") - add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") - 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")), - ) - return launch.LaunchDescription( - launch_arguments - + [set_container_executable, set_container_mt_executable] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml deleted file mode 100644 index 21de187f1..000000000 --- a/perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/perception_launch/launch/object_recognition/prediction/prediction.launch.xml b/perception_launch/launch/object_recognition/prediction/prediction.launch.xml deleted file mode 100644 index fd9d1ba6f..000000000 --- a/perception_launch/launch/object_recognition/prediction/prediction.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/perception_launch/launch/object_recognition/tracking/tracking.launch.xml deleted file mode 100644 index 65ac62ef3..000000000 --- a/perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py deleted file mode 100644 index f63af3165..000000000 --- a/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ /dev/null @@ -1,539 +0,0 @@ -# Copyright 2020 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. -import os - -from ament_index_python.packages import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -class GroundSegmentationPipeline: - def __init__(self, context): - self.context = context - self.vehicle_info = self.get_vehicle_info() - ground_segmentation_param_path = os.path.join( - get_package_share_directory("perception_launch"), - "config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml", - ) - with open(ground_segmentation_param_path, "r") as f: - self.ground_segmentation_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - self.single_frame_obstacle_seg_output = ( - "/perception/obstacle_segmentation/single_frame/pointcloud_raw" - ) - self.output_topic = "/perception/obstacle_segmentation/pointcloud" - self.use_single_frame_filter = self.ground_segmentation_param["use_single_frame_filter"] - self.use_time_series_filter = self.ground_segmentation_param["use_time_series_filter"] - - def get_vehicle_info(self): - # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support - # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py - gp = self.context.launch_configurations.get("ros_params", {}) - if not gp: - gp = dict(self.context.launch_configurations.get("global_params", {})) - p = {} - p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] - p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] - p["min_longitudinal_offset"] = -gp["rear_overhang"] - p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] - p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) - p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] - p["min_height_offset"] = 0.0 - p["max_height_offset"] = gp["vehicle_height"] - return p - - def get_vehicle_mirror_info(self): - path = LaunchConfiguration("vehicle_mirror_param_file").perform(self.context) - with open(path, "r") as f: - p = yaml.safe_load(f) - return p - - def create_additional_pipeline(self, lidar_name): - components = [] - components.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name=f"{lidar_name}_crop_box_filter", - remappings=[ - ("input", f"/sensing/lidar/{lidar_name}/outlier_filtered/pointcloud"), - ("output", f"{lidar_name}/range_cropped/pointcloud"), - ], - parameters=[ - { - "input_frame": LaunchConfiguration("base_frame"), - "output_frame": LaunchConfiguration("base_frame"), - }, - self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"], - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - - components.append( - ComposableNode( - package="ground_segmentation", - plugin=self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["plugin"], - name=f"{lidar_name}_ground_filter", - remappings=[ - ("input", f"{lidar_name}/range_cropped/pointcloud"), - ("output", f"{lidar_name}/pointcloud"), - ], - parameters=[ - self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"] - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - - return components - - def create_ransac_pipeline(self): - components = [] - components.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concatenate_data", - namespace="plane_fitting", - remappings=[("output", "concatenated/pointcloud")], - parameters=[ - { - "input_topics": self.ground_segmentation_param["ransac_input_topics"], - "output_frame": LaunchConfiguration("base_frame"), - "timeout_sec": 1.0, - } - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - - components.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_height_obstacle_detection_area_filter", - namespace="plane_fitting", - remappings=[ - ("input", "concatenated/pointcloud"), - ("output", "detection_area/pointcloud"), - ], - parameters=[ - { - "input_frame": LaunchConfiguration("base_frame"), - "output_frame": LaunchConfiguration("base_frame"), - }, - self.ground_segmentation_param["short_height_obstacle_detection_area_filter"][ - "parameters" - ], - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - - components.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::Lanelet2MapFilterComponent", - name="vector_map_filter", - namespace="plane_fitting", - remappings=[ - ("input/pointcloud", "detection_area/pointcloud"), - ("input/vector_map", "/map/vector_map"), - ("output", "vector_map_filtered/pointcloud"), - ], - parameters=[ - { - "voxel_size_x": 0.25, - "voxel_size_y": 0.25, - } - ], - # cannot use intra process because vector map filter uses transient local. - extra_arguments=[{"use_intra_process_comms": False}], - ) - ) - - components.append( - ComposableNode( - package="ground_segmentation", - plugin="ground_segmentation::RANSACGroundFilterComponent", - name="ransac_ground_filter", - namespace="plane_fitting", - remappings=[ - ("input", "vector_map_filtered/pointcloud"), - ("output", "pointcloud"), - ], - parameters=[self.ground_segmentation_param["ransac_ground_filter"]["parameters"]], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - - return components - - def create_common_pipeline(self, input_topic, output_topic): - components = [] - components.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter", - remappings=[ - ("input", input_topic), - ("output", "range_cropped/pointcloud"), - ], - parameters=[ - { - "input_frame": LaunchConfiguration("base_frame"), - "output_frame": LaunchConfiguration("base_frame"), - }, - self.ground_segmentation_param["common_crop_box_filter"]["parameters"], - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - - components.append( - ComposableNode( - package="ground_segmentation", - plugin=self.ground_segmentation_param["common_ground_filter"]["plugin"], - name="common_ground_filter", - remappings=[ - ("input", "range_cropped/pointcloud"), - ("output", output_topic), - ], - parameters=[ - self.ground_segmentation_param["common_ground_filter"]["parameters"], - self.vehicle_info, - {"input_frame": "base_link"}, - {"output_frame": "base_link"}, - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - return components - - def create_single_frame_obstacle_segmentation_components(self, input_topic, output_topic): - - additional_lidars = self.ground_segmentation_param["additional_lidars"] - use_ransac = bool(self.ground_segmentation_param["ransac_input_topics"]) - use_additional = bool(additional_lidars) - relay_topic = "all_lidars/pointcloud" - common_pipeline_output = ( - "single_frame/pointcloud" if use_additional or use_ransac else output_topic - ) - - components = self.create_common_pipeline( - input_topic=input_topic, - output_topic=common_pipeline_output, - ) - - if use_additional: - for lidar_name in additional_lidars: - components.extend(self.create_additional_pipeline(lidar_name)) - components.append( - self.get_additional_lidars_concatenated_component( - input_topics=[common_pipeline_output] - + [f"{x}/pointcloud" for x in additional_lidars], - output_topic=relay_topic if use_ransac else output_topic, - ) - ) - - if use_ransac: - components.extend(self.create_ransac_pipeline()) - components.append( - self.get_single_frame_obstacle_segmentation_concatenated_component( - input_topics=[ - "plane_fitting/pointcloud", - relay_topic if use_additional else common_pipeline_output, - ], - output_topic=output_topic, - ) - ) - - return components - - @staticmethod - def create_time_series_outlier_filter_components(input_topic, output_topic): - components = [] - components.append( - ComposableNode( - package="occupancy_grid_map_outlier_filter", - plugin="occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent", - name="occupancy_grid_map_outlier_filter", - remappings=[ - ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), - ("~/input/pointcloud", input_topic), - ("~/output/pointcloud", output_topic), - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - - return components - - @staticmethod - def create_single_frame_outlier_filter_components(input_topic, output_topic): - components = [] - components.append( - ComposableNode( - package="elevation_map_loader", - plugin="ElevationMapLoaderNode", - name="elevation_map_loader", - namespace="elevation_map", - remappings=[ - ("output/elevation_map", "map"), - ("input/pointcloud_map", "/map/pointcloud_map"), - ("input/vector_map", "/map/vector_map"), - ], - parameters=[ - { - "use_lane_filter": False, - "use_inpaint": True, - "inpaint_radius": 1.0, - "param_file_path": PathJoinSubstitution( - [ - FindPackageShare("perception_launch"), - "config", - "obstacle_segmentation", - "ground_segmentation", - "elevation_map_parameters.yaml", - ] - ), - "elevation_map_directory": PathJoinSubstitution( - [FindPackageShare("elevation_map_loader"), "data", "elevation_maps"] - ), - "use_elevation_map_cloud_publisher": False, - } - ], - extra_arguments=[{"use_intra_process_comms": False}], - ) - ) - - components.append( - ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::CompareElevationMapFilterComponent", - name="compare_elevation_map_filter", - namespace="elevation_map", - remappings=[ - ("input", input_topic), - ("output", "map_filtered/pointcloud"), - ("input/elevation_map", "map"), - ], - parameters=[ - { - "map_frame": "map", - "map_layer_name": "elevation", - "height_diff_thresh": 0.15, - "input_frame": "map", - "output_frame": "base_link", - } - ], - extra_arguments=[ - {"use_intra_process_comms": False} - ], # can't use this with transient_local - ) - ) - - components.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name="voxel_grid_filter", - namespace="elevation_map", - remappings=[ - ("input", "map_filtered/pointcloud"), - ("output", "voxel_grid_filtered/pointcloud"), - ], - parameters=[ - { - "input_frame": LaunchConfiguration("base_frame"), - "output_frame": LaunchConfiguration("base_frame"), - "voxel_size_x": 0.04, - "voxel_size_y": 0.04, - "voxel_size_z": 0.08, - } - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - - components.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", - name="voxel_grid_outlier_filter", - namespace="elevation_map", - remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), - ("output", output_topic), - ], - parameters=[ - { - "voxel_size_x": 0.4, - "voxel_size_y": 0.4, - "voxel_size_z": 100.0, - "voxel_points_threshold": 5, - } - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ) - - return components - - @staticmethod - def get_additional_lidars_concatenated_component(input_topics, output_topic): - - return ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concatenate_data", - remappings=[("output", output_topic)], - parameters=[ - { - "input_topics": input_topics, - "output_frame": LaunchConfiguration("base_frame"), - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - @staticmethod - def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, output_topic): - return ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concatenate_no_ground_data", - remappings=[("output", output_topic)], - parameters=[ - { - "input_topics": input_topics, - "output_frame": LaunchConfiguration("base_frame"), - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - -def launch_setup(context, *args, **kwargs): - pipeline = GroundSegmentationPipeline(context) - - components = [] - components.extend( - pipeline.create_single_frame_obstacle_segmentation_components( - input_topic=LaunchConfiguration("input/pointcloud"), - output_topic=pipeline.single_frame_obstacle_seg_output, - ) - ) - - relay_topic = "single_frame/filtered/pointcloud" - if pipeline.use_single_frame_filter: - components.extend( - pipeline.create_single_frame_outlier_filter_components( - input_topic=pipeline.single_frame_obstacle_seg_output, - output_topic=relay_topic - if pipeline.use_time_series_filter - else pipeline.output_topic, - context=context, - ) - ) - if pipeline.use_time_series_filter: - components.extend( - pipeline.create_time_series_outlier_filter_components( - input_topic=relay_topic - if pipeline.use_single_frame_filter - else pipeline.single_frame_obstacle_seg_output, - output_topic=pipeline.output_topic, - ) - ) - individual_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=components, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - pointcloud_container_loader = LoadComposableNodes( - composable_node_descriptions=components, - target_container=LaunchConfiguration("container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), - ) - return [individual_container, pointcloud_container_loader] - - -def generate_launch_description(): - - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg("base_frame", "base_link") - add_launch_arg("use_multithread", "False") - add_launch_arg("use_intra_process", "True") - add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "perception_pipeline_container") - add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud") - - 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")), - ) - - return launch.LaunchDescription( - launch_arguments - + [set_container_executable, set_container_mt_executable] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py b/perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py deleted file mode 100644 index d371fa439..000000000 --- a/perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py +++ /dev/null @@ -1,138 +0,0 @@ -# Copyright 2021 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="pointcloud_to_laserscan", - plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode", - name="pointcloud_to_laserscan_node", - remappings=[ - ("~/input/pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), - ("~/output/laserscan", LaunchConfiguration("output/laserscan")), - ("~/output/pointcloud", LaunchConfiguration("output/pointcloud")), - ("~/output/ray", LaunchConfiguration("output/ray")), - ("~/output/stixel", LaunchConfiguration("output/stixel")), - ], - parameters=[ - { - "target_frame": "base_link", # Leave disabled to output scan in pointcloud frame - "transform_tolerance": 0.01, - "min_height": 0.0, - "max_height": 2.0, - "angle_min": -3.141592, # -M_PI - "angle_max": 3.141592, # M_PI - "angle_increment": 0.00349065850, # 0.20*M_PI/180.0 - "scan_time": 0.0, - "range_min": 1.0, - "range_max": 200.0, - "use_inf": True, - "inf_epsilon": 1.0, - # Concurrency level, affects number of pointclouds queued for processing - # and number of threads used - # 0 : Detect number of cores - # 1 : Single threaded - # 2->inf : Parallelism level - "concurrency_level": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ), - ComposableNode( - package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", - name="occupancy_grid_map_node", - remappings=[ - ("~/input/laserscan", LaunchConfiguration("output/laserscan")), - ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), - ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), - ("~/output/occupancy_grid_map", LaunchConfiguration("output")), - ], - parameters=[ - { - "map_resolution": 0.5, - "use_height_filter": True, - "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), - "input_obstacle_and_raw_pointcloud": LaunchConfiguration( - "input_obstacle_and_raw_pointcloud" - ), - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ), - ] - - occupancy_grid_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", "false"), - add_launch_arg("use_intra_process", "false"), - add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), - add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), - add_launch_arg("output", "occupancy_grid"), - add_launch_arg("output/laserscan", "virtual_scan/laserscan"), - add_launch_arg("output/pointcloud", "virtual_scan/pointcloud"), - add_launch_arg("output/ray", "virtual_scan/ray"), - add_launch_arg("output/stixel", "virtual_scan/stixel"), - add_launch_arg("input_obstacle_pointcloud", "false"), - add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), - add_launch_arg("use_pointcloud_container", "False"), - add_launch_arg("container_name", "occupancy_grid_map_container"), - set_container_executable, - set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, - ] - ) diff --git a/perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py b/perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py deleted file mode 100644 index 2feefdfb1..000000000 --- a/perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py +++ /dev/null @@ -1,92 +0,0 @@ -# Copyright 2021 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="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", - name="occupancy_grid_map_node", - remappings=[ - ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), - ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), - ("~/output/occupancy_grid_map", LaunchConfiguration("output")), - ], - parameters=[ - { - "map_resolution": 0.5, - "use_height_filter": True, - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ), - ] - - occupancy_grid_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", "False"), - add_launch_arg("use_intra_process", "True"), - add_launch_arg("use_pointcloud_container", "False"), - add_launch_arg("container_name", "occupancy_grid_map_container"), - add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), - add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), - add_launch_arg("output", "occupancy_grid"), - set_container_executable, - set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, - ] - ) diff --git a/perception_launch/launch/perception.launch.xml b/perception_launch/launch/perception.launch.xml deleted file mode 100644 index 91931d53b..000000000 --- a/perception_launch/launch/perception.launch.xml +++ /dev/null @@ -1,131 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml deleted file mode 100644 index 8d3ca7452..000000000 --- a/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py deleted file mode 100644 index 8827dfbb9..000000000 --- a/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ /dev/null @@ -1,252 +0,0 @@ -# Copyright 2020 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. - -import os - -from ament_index_python.packages import get_package_share_directory -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(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - # a default_value of None is equivalent to not passing that kwarg at all - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - ssd_fine_detector_share_dir = get_package_share_directory("traffic_light_ssd_fine_detector") - classifier_share_dir = get_package_share_directory("traffic_light_classifier") - add_launch_arg("enable_image_decompressor", "True") - add_launch_arg("enable_fine_detection", "True") - add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") - - # traffic_light_ssd_fine_detector - add_launch_arg( - "onnx_file", os.path.join(ssd_fine_detector_share_dir, "data", "mb2-ssd-lite-tlr.onnx") - ) - add_launch_arg( - "label_file", os.path.join(ssd_fine_detector_share_dir, "data", "voc_labels_tl.txt") - ) - add_launch_arg("fine_detector_precision", "FP32") - add_launch_arg("score_thresh", "0.7") - add_launch_arg("max_batch_size", "8") - add_launch_arg("approximate_sync", "False") - add_launch_arg("mean", "[0.5, 0.5, 0.5]") - add_launch_arg("std", "[0.5, 0.5, 0.5]") - - # traffic_light_classifier - add_launch_arg("classifier_type", "1") - add_launch_arg( - "model_file_path", - os.path.join(classifier_share_dir, "data", "traffic_light_classifier_mobilenetv2.onnx"), - ) - add_launch_arg("label_file_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt")) - add_launch_arg("precision", "fp16") - add_launch_arg("input_c", "3") - add_launch_arg("input_h", "224") - add_launch_arg("input_w", "224") - - add_launch_arg("use_crosswalk_traffic_light_estimator", "True") - add_launch_arg("use_intra_process", "False") - add_launch_arg("use_multithread", "False") - - def create_parameter_dict(*args): - result = {} - for x in args: - result[x] = LaunchConfiguration(x) - return result - - container = ComposableNodeContainer( - name="traffic_light_node_container", - namespace="/perception/traffic_light_recognition", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - ComposableNode( - package="traffic_light_classifier", - plugin="traffic_light::TrafficLightClassifierNodelet", - name="traffic_light_classifier", - parameters=[ - create_parameter_dict( - "approximate_sync", - "classifier_type", - "model_file_path", - "label_file_path", - "precision", - "input_c", - "input_h", - "input_w", - ) - ], - remappings=[ - ("~/input/image", LaunchConfiguration("input/image")), - ("~/input/rois", "rois"), - ("~/output/traffic_signals", "classified/traffic_signals"), - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ), - ComposableNode( - package="traffic_light_visualization", - plugin="traffic_light::TrafficLightRoiVisualizerNodelet", - name="traffic_light_roi_visualizer", - parameters=[create_parameter_dict("enable_fine_detection")], - remappings=[ - ("~/input/image", LaunchConfiguration("input/image")), - ("~/input/rois", "rois"), - ("~/input/rough/rois", "rough/rois"), - ("~/input/traffic_signals", "traffic_signals"), - ("~/output/image", "debug/rois"), - ("~/output/image/compressed", "debug/rois/compressed"), - ("~/output/image/compressedDepth", "debug/rois/compressedDepth"), - ("~/output/image/theora", "debug/rois/theora"), - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ), - ], - output="both", - ) - - estimator_loader = LoadComposableNodes( - composable_node_descriptions=[ - ComposableNode( - package="crosswalk_traffic_light_estimator", - plugin="traffic_light::CrosswalkTrafficLightEstimatorNode", - name="crosswalk_traffic_light_estimator", - remappings=[ - ("~/input/vector_map", "/map/vector_map"), - ("~/input/route", "/planning/mission_planning/route"), - ("~/input/classified/traffic_signals", "classified/traffic_signals"), - ("~/output/traffic_signals", "traffic_signals"), - ], - extra_arguments=[{"use_intra_process_comms": False}], - ), - ], - target_container=container, - condition=IfCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")), - ) - - relay_loader = LoadComposableNodes( - composable_node_descriptions=[ - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="classified_signals_relay", - namespace="", - parameters=[ - {"input_topic": "classified/traffic_signals"}, - {"output_topic": "traffic_signals"}, - {"type": "autoware_auto_perception_msgs/msg/TrafficSignalArray"}, - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ], - target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")), - ) - - decompressor_loader = LoadComposableNodes( - composable_node_descriptions=[ - ComposableNode( - package="image_transport_decompressor", - plugin="image_preprocessor::ImageTransportDecompressor", - name="traffic_light_image_decompressor", - parameters=[{"encoding": "rgb8"}], - remappings=[ - ( - "~/input/compressed_image", - [LaunchConfiguration("input/image"), "/compressed"], - ), - ("~/output/raw_image", LaunchConfiguration("input/image")), - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ), - ], - target_container=container, - condition=IfCondition(LaunchConfiguration("enable_image_decompressor")), - ) - - ssd_fine_detector_param = create_parameter_dict( - "onnx_file", - "label_file", - "score_thresh", - "max_batch_size", - "approximate_sync", - "mean", - "std", - ) - ssd_fine_detector_param["mode"] = LaunchConfiguration("fine_detector_precision") - - fine_detector_loader = LoadComposableNodes( - composable_node_descriptions=[ - ComposableNode( - package="traffic_light_ssd_fine_detector", - plugin="traffic_light::TrafficLightSSDFineDetectorNodelet", - name="traffic_light_ssd_fine_detector", - parameters=[ssd_fine_detector_param], - remappings=[ - ("~/input/image", LaunchConfiguration("input/image")), - ("~/input/rois", "rough/rois"), - ("~/output/rois", "rois"), - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ), - ], - target_container=container, - condition=IfCondition(LaunchConfiguration("enable_fine_detection")), - ) - - 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")), - ) - - return LaunchDescription( - [ - *launch_arguments, - set_container_executable, - set_container_mt_executable, - container, - decompressor_loader, - fine_detector_loader, - estimator_loader, - relay_loader, - ] - ) diff --git a/perception_launch/package.xml b/perception_launch/package.xml deleted file mode 100644 index b593cacef..000000000 --- a/perception_launch/package.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - perception_launch - 0.1.0 - The perception_launch package - Yukihiro Saito - Shunsuke Miura - Apache License 2.0 - - ament_cmake_auto - - compare_map_segmentation - crosswalk_traffic_light_estimator - detected_object_feature_remover - detected_object_validation - detection_by_tracker - euclidean_cluster - ground_segmentation - image_projection_based_fusion - image_transport_decompressor - lidar_apollo_instance_segmentation - map_based_prediction - multi_object_tracker - object_merger - object_range_splitter - occupancy_grid_map_outlier_filter - pointcloud_preprocessor - pointcloud_to_laserscan - probabilistic_occupancy_grid_map - shape_estimation - tensorrt_yolo - traffic_light_classifier - traffic_light_map_based_detector - traffic_light_ssd_fine_detector - traffic_light_visualization - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/perception_launch/perception_launch.drawio.svg b/perception_launch/perception_launch.drawio.svg deleted file mode 100644 index 49bfa1f70..000000000 --- a/perception_launch/perception_launch.drawio.svg +++ /dev/null @@ -1,309 +0,0 @@ - - - - - - - - - - - -
-
-
- perception.launch.xml -
-
-
- - package: perception_launch - -
-
-
-
-
- - perception.launch.xml... - -
-
- - - - - - - -
-
-
- True -
-
-
-
- - True - -
-
- - - - - - -
-
-
- launch name -
-
-
- - package: package name - -
-
-
-
-
- - launch name... - -
-
- - - - -
-
-
- ex: -
-
-
-
- - ex: - -
-
- - - - -
-
-
- node name -
-
-
- - package: package name - -
-
-
-
-
- - node name... - -
-
- - - - -
-
-
- other name -
-
-
- - package: package name - -
-
-
-
-
- - other name... - -
-
- - - - -
-
-
- $(var use_empty_dynamic_object_publisher) -
-
-
-
- - $(var use_empty_dynamic_object_publisher) - -
-
- - - - -
-
-
- detection.launch.xml -
-
-
- - package: perception_launch - -
-
-
-
-
- - detection.launch.xml... - -
-
- - - - - -
-
-
- False -
-
-
-
- - False - -
-
- - - - -
-
-
- tracking.launch.xml -
-
-
- - package: perception_launch - -
-
-
-
-
- - tracking.launch.xml... - -
-
- - - - -
-
-
- prediction.launch.xml -
-
-
- - package: perception_launch - -
-
-
-
-
- - prediction.launch.xml... - -
-
- - - - -
-
-
- empty_objects_publisher -
-
-
- - package: dummy_perception_publisher - -
-
-
-
-
- - empty_objects_publisher... - -
-
- - - - -
-
-
- traffic_light_recognition.launch.xml -
-
-
- - package: perception_launch - -
-
-
-
-
- - traffic_light_recognition.launch.xml... - -
-
- - -
- - - - - Viewer does not support full SVG 1.1 - - - -
\ No newline at end of file diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index e6461fa2b..40de19bd6 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -2,74 +2,26 @@ /**: ros__parameters: avoidance: - resample_interval_for_planning: 0.3 - resample_interval_for_output: 4.0 - detection_area_right_expand_dist: 0.0 - detection_area_left_expand_dist: 1.0 - + resample_interval_for_planning: 0.3 # [m] + resample_interval_for_output: 4.0 # [m] + detection_area_right_expand_dist: 0.0 # [m] + detection_area_left_expand_dist: 1.0 # [m] + drivable_area_right_bound_offset: 0.0 # [m] + drivable_area_left_bound_offset: 0.0 # [m] + object_envelope_buffer: 0.3 # [m] + + # avoidance module common setting + enable_bound_clipping: false enable_avoidance_over_same_direction: true enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false enable_safety_check: false - - # For target object filtering - threshold_speed_object_is_stopped: 1.0 # [m/s] - threshold_time_object_is_moving: 1.0 # [s] - - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] - - threshold_distance_object_is_on_center: 1.0 # [m] - - object_check_shiftable_ratio: 0.6 # [-] - object_check_min_road_shoulder_width: 0.5 # [m] - - # For safety check - safety_check_backward_distance: 50.0 # [m] - safety_check_time_horizon: 10.0 # [s] - safety_check_idling_time: 1.5 # [s] - safety_check_accel_for_rss: 2.5 # [m/ss] - - # For lateral margin - object_envelope_buffer: 0.3 # [m] - lateral_collision_margin: 1.0 # [m] - lateral_collision_safety_buffer: 0.7 # [m] - - # For longitudinal margin - longitudinal_collision_margin_buffer: 0.0 # [m] - - prepare_time: 2.0 # [s] - min_prepare_distance: 1.0 # [m] - min_avoidance_distance: 10.0 # [m] - - min_nominal_avoidance_speed: 7.0 # [m/s] - min_sharp_avoidance_speed: 1.0 # [m/s] - - road_shoulder_safety_margin: 0.0 # [m] - - max_right_shift_length: 5.0 - max_left_shift_length: 5.0 - - nominal_lateral_jerk: 0.2 # [m/s3] - max_lateral_jerk: 1.0 # [m/s3] - - object_last_seen_threshold: 2.0 - - # For prevention of large acceleration while avoidance - min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] - max_avoidance_acceleration: 0.5 # [m/ss] - - # bound clipping for objects - enable_bound_clipping: false + enable_yield_maneuver: false # for debug publish_debug_marker: false print_debug_info: false - # not enabled yet - longitudinal_collision_margin_min_distance: 0.0 # [m] - longitudinal_collision_margin_time: 0.0 - # avoidance is performed for the object type with true target_object: car: true @@ -81,11 +33,80 @@ motorcycle: false pedestrian: false - # ---------- advanced parameters ---------- - avoidance_execution_lateral_threshold: 0.499 + # For target object filtering + target_filtering: + # filtering moving objects + threshold_speed_object_is_stopped: 1.0 # [m/s] + threshold_time_object_is_moving: 1.0 # [s] + # detection range + object_check_forward_distance: 150.0 # [m] + object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 20.0 # [m] + # filtering parking objects + threshold_distance_object_is_on_center: 1.0 # [m] + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + # lost object compensation + object_last_seen_threshold: 2.0 + + # For safety check + safety_check: + safety_check_backward_distance: 50.0 # [m] + safety_check_time_horizon: 10.0 # [s] + safety_check_idling_time: 1.5 # [s] + safety_check_accel_for_rss: 2.5 # [m/ss] + safety_check_hysteresis_factor: 2.0 # [-] + + # For avoidance maneuver + avoidance: + # avoidance lateral parameters + lateral: + lateral_collision_margin: 1.0 # [m] + lateral_collision_safety_buffer: 0.7 # [m] + lateral_passable_safety_buffer: 0.0 # [m] + road_shoulder_safety_margin: 0.0 # [m] + avoidance_execution_lateral_threshold: 0.499 + max_right_shift_length: 5.0 + max_left_shift_length: 5.0 + # avoidance distance parameters + longitudinal: + prepare_time: 2.0 # [s] + longitudinal_collision_safety_buffer: 0.0 # [m] + min_prepare_distance: 1.0 # [m] + min_avoidance_distance: 10.0 # [m] + min_nominal_avoidance_speed: 7.0 # [m/s] + min_sharp_avoidance_speed: 1.0 # [m/s] + + # For yield maneuver + yield: + yield_velocity: 2.78 # [m/s] + + # For stop maneuver + stop: + min_distance: 10.0 # [m] + max_distance: 20.0 # [m] + + constraints: + # vehicle slows down under longitudinal constraints + use_constraints_for_decel: false # [-] + + # lateral constraints + lateral: + nominal_lateral_jerk: 0.2 # [m/s3] + max_lateral_jerk: 1.0 # [m/s3] + + # longitudinal constraints + longitudinal: + nominal_deceleration: -1.0 # [m/ss] + nominal_jerk: 0.5 # [m/sss] + max_deceleration: -2.0 # [m/ss] + max_jerk: 1.0 + # For prevention of large acceleration while avoidance + min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] + max_avoidance_acceleration: 0.5 # [m/ss] target_velocity_matrix: - col_size: 4 + col_size: 2 matrix: - [2.78, 5.56, 11.1, 13.9, # velocity [m/s] - 0.20, 0.90, 1.10, 1.20] # margin [m] + [2.78, 13.9, # velocity [m/s] + 0.50, 1.00] # margin [m] diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml index 660a4d2af..29d577a7a 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -3,18 +3,24 @@ avoidance: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] lane_change: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] lane_following: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] pull_out: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] pull_over: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] side_shift: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml index 66d66fe78..891e54a54 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -9,21 +9,26 @@ backward_length_buffer_for_end_of_lane: 2.0 lane_change_finish_judge_buffer: 3.0 # [m] - double lane_changing_lateral_jerk: 0.5 - double lane_changing_lateral_acc: 0.5 + lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_acc: 0.5 # [m/s2] minimum_lane_change_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] maximum_deceleration: 1.0 # [m/s2] lane_change_sampling_num: 10 - abort_lane_change_velocity_thresh: 0.5 - abort_lane_change_angle_thresh: 10.0 # [deg] - abort_lane_change_distance_thresh: 0.3 # [m] - prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] - - enable_abort_lane_change: true + # collision check enable_collision_check_at_prepare_phase: true + prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] use_predicted_path_outside_lanelet: true use_all_predicted_path: false + + # abort + enable_cancel_lane_change: true + enable_abort_lane_change: false + + abort_delta_time: 3.0 # [s] + abort_max_lateral_jerk: 1000.0 # [m/s3] + + # debug publish_debug_marker: false diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index 75b5facaa..4c9f4fd41 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: pull_over: - request_length: 200.0 + request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -17,7 +17,7 @@ backward_goal_search_length: 20.0 goal_search_interval: 2.0 longitudinal_margin: 3.0 - max_lateral_offset: 1.0 + max_lateral_offset: 0.5 lateral_offset_interval: 0.25 # occupancy grid map use_occupancy_grid: true @@ -34,7 +34,7 @@ maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 - after_pull_over_straight_distance: 5.0 + after_pull_over_straight_distance: 1.0 # parallel parking path enable_arc_forward_parking: true enable_arc_backward_parking: true diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml index 31f75d7f7..5af1c99f8 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml @@ -6,3 +6,4 @@ backward_length: 15.0 # [m] ignore_width_from_center_line: 0.7 # [m] max_future_movement_time: 10.0 # [second] + threshold_yaw_diff: 0.523 # [rad]