diff --git a/launch/perception_launch/CMakeLists.txt b/launch/perception_launch/CMakeLists.txt new file mode 100644 index 0000000000000..ade04cfcd8d8b --- /dev/null +++ b/launch/perception_launch/CMakeLists.txt @@ -0,0 +1,16 @@ +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/launch/perception_launch/README.md b/launch/perception_launch/README.md new file mode 100644 index 0000000000000..9cc11ca6f78ba --- /dev/null +++ b/launch/perception_launch/README.md @@ -0,0 +1,20 @@ +# 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/launch/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/launch/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml new file mode 100644 index 0000000000000..393bb949d048d --- /dev/null +++ b/launch/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -0,0 +1,52 @@ +/**: + ros__parameters: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 1, 1, 1, 1, 0, 0, 0, 0, #CAR + 1, 1, 1, 1, 0, 0, 0, 0, #TRUCK + 1, 1, 1, 1, 0, 0, 0, 0, #BUS + 0, 0, 0, 0, 1, 1, 1, 0, #BICYCLE + 0, 0, 0, 0, 1, 1, 1, 0, #MOTORBIKE + 0, 0, 0, 0, 1, 1, 1, 0, #PEDESTRIAN + 0, 0, 0, 0, 0, 0, 0, 1] #ANIMAL + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #CAR + 4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #BUS + 3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #BICYCLE + 3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #MOTORBIKE + 2.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #PEDESTRIAN + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #ANIMAL + max_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN + 12.10, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #CAR + 19.75, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #TRUCK + 32.40, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #BUS + 2.50, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, 10000.00, #BICYCLE + 3.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, 10000.00, #MOTORBIKE + 2.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00, 10000.00, #PEDESTRIAN + 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00] #ANIMAL + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 3.600, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #CAR + 6.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #TRUCK + 10.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #BUS + 0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #BICYCLE + 0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #MOTORBIKE + 0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #PEDESTRIAN + 0.500, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #ANIMAL + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #BUS + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #PEDESTRIAN + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #ANIMAL diff --git a/launch/perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml b/launch/perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml new file mode 100644 index 0000000000000..955a9e197ea0b --- /dev/null +++ b/launch/perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml @@ -0,0 +1,30 @@ +pcl_grid_map_extraction: + num_processing_threads: 12 + cloud_transform: + translation: + x: 0.0 + y: 0.0 + z: 0.0 + rotation: #intrinsic rotation X-Y-Z (r-p-y)sequence + r: 0.0 + p: 0.0 + y: 0.0 + cluster_extraction: + cluster_tolerance: 0.2 + min_num_points: 3 + max_num_points: 1000000 + outlier_removal: + is_remove_outliers: false + mean_K: 10 + stddev_threshold: 1.0 + downsampling: + is_downsample_cloud: false + voxel_size: + x: 0.02 + y: 0.02 + z: 0.02 + grid_map: + min_num_points_per_cell: 3 + resolution: 0.3 + height_type: 1 + height_thresh: 1.0 diff --git a/launch/perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/launch/perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml new file mode 100644 index 0000000000000..33addb108dded --- /dev/null +++ b/launch/perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + additional_lidars: [] + ransac_input_topics: [] + use_single_frame_filter: False + use_time_series_filter: True + + common_crop_box_filter: + parameters: + min_x: -50.0 + max_x: 100.0 + min_y: -50.0 + max_y: 50.0 + negative: False + + common_ground_filter: + plugin: "ground_segmentation::ScanGroundFilterComponent" + parameters: + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 30.0 + split_points_distance_tolerance: 0.2 + split_height_distance: 0.2 + use_virtual_ground_point: False diff --git a/launch/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml new file mode 100644 index 0000000000000..dade43a98bd98 --- /dev/null +++ b/launch/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/perception_launch/launch/object_recognition/detection/detection.launch.xml new file mode 100644 index 0000000000000..55006f9fa1ac3 --- /dev/null +++ b/launch/perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml new file mode 100644 index 0000000000000..a836c4198f608 --- /dev/null +++ b/launch/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/perception_launch/launch/object_recognition/prediction/prediction.launch.xml b/launch/perception_launch/launch/object_recognition/prediction/prediction.launch.xml new file mode 100644 index 0000000000000..bd322ff67954e --- /dev/null +++ b/launch/perception_launch/launch/object_recognition/prediction/prediction.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/launch/perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/perception_launch/launch/object_recognition/tracking/tracking.launch.xml new file mode 100644 index 0000000000000..64bcad42fa31a --- /dev/null +++ b/launch/perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/launch/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py new file mode 100644 index 0000000000000..111856cdd749c --- /dev/null +++ b/launch/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -0,0 +1,537 @@ +# 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): + path = LaunchConfiguration("vehicle_param_file").perform(self.context) + with open(path, "r") as f: + p = yaml.safe_load(f)["/**"]["ros__parameters"] + p["vehicle_length"] = p["front_overhang"] + p["wheel_base"] + p["rear_overhang"] + p["vehicle_width"] = p["wheel_tread"] + p["left_overhang"] + p["right_overhang"] + p["min_longitudinal_offset"] = -p["rear_overhang"] + p["max_longitudinal_offset"] = p["front_overhang"] + p["wheel_base"] + p["min_lateral_offset"] = -(p["wheel_tread"] / 2.0 + p["right_overhang"]) + p["max_lateral_offset"] = p["wheel_tread"] / 2.0 + p["left_overhang"] + p["min_height_offset"] = 0.0 + p["max_height_offset"] = p["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"), + "min_z": self.vehicle_info["min_height_offset"], + "max_z": self.vehicle_info["max_height_offset"], + }, + 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"), + "min_z": self.vehicle_info["min_height_offset"], + "max_z": self.vehicle_info["max_height_offset"], + }, + 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, + ], + 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] + + list(map(lambda x: f"{x}/pointcloud"), 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="/sensing/lidar/concatenated/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, + ) + ) + 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("vehicle_param_file") + 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") + + 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/launch/perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py b/launch/perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py new file mode 100644 index 0000000000000..57e172ffda42d --- /dev/null +++ b/launch/perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py @@ -0,0 +1,138 @@ +# 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.00436332222, # 0.25*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="laserscan_to_occupancy_grid_map", + plugin="occupancy_grid_map::OccupancyGridMapNode", + 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/launch/perception_launch/launch/perception.launch.xml b/launch/perception_launch/launch/perception.launch.xml new file mode 100644 index 0000000000000..b95f0ed4b87e8 --- /dev/null +++ b/launch/perception_launch/launch/perception.launch.xml @@ -0,0 +1,117 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml new file mode 100644 index 0000000000000..d86a8ccf1912c --- /dev/null +++ b/launch/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py new file mode 100644 index 0000000000000..89935303432dd --- /dev/null +++ b/launch/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -0,0 +1,201 @@ +# 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 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_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_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="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")} + ], + ), + 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", "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", + ) + + 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") + + 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=launch.conditions.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, + loader, + ] + ) diff --git a/launch/perception_launch/package.xml b/launch/perception_launch/package.xml new file mode 100644 index 0000000000000..bf643b5c10434 --- /dev/null +++ b/launch/perception_launch/package.xml @@ -0,0 +1,39 @@ + + + + perception_launch + 0.1.0 + The perception_launch package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + + compare_map_segmentation + euclidean_cluster + ground_segmentation + 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 + roi_cluster_fusion + 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/launch/perception_launch/perception_launch.drawio.svg b/launch/perception_launch/perception_launch.drawio.svg new file mode 100644 index 0000000000000..49bfa1f7004c8 --- /dev/null +++ b/launch/perception_launch/perception_launch.drawio.svg @@ -0,0 +1,309 @@ + + + + + + + + + + + +
+
+
+ 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