From 21f632b2ae6dce7ff39dce02cad54410f76a41b7 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 11 Jul 2022 14:55:15 +0900 Subject: [PATCH] feat: remove detection preprocess (#285) Signed-off-by: tomoya.kimura --- .../detection/detection_preprocess.param.yaml | 5 - .../pointcloud_map_filter.param.yaml | 2 +- ...ra_lidar_fusion_based_detection.launch.xml | 5 +- .../detection/detection_preprocess.launch.py | 136 ------------------ .../lidar_based_detection.launch.xml | 6 +- 5 files changed, 7 insertions(+), 147 deletions(-) delete mode 100644 perception_launch/config/object_recognition/detection/detection_preprocess.param.yaml delete mode 100644 perception_launch/launch/object_recognition/detection/detection_preprocess.launch.py diff --git a/perception_launch/config/object_recognition/detection/detection_preprocess.param.yaml b/perception_launch/config/object_recognition/detection/detection_preprocess.param.yaml deleted file mode 100644 index 815e70e392..0000000000 --- a/perception_launch/config/object_recognition/detection/detection_preprocess.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - use_down_sample_filter: True - down_sample_voxel_size: 0.1 - distance_threshold: 0.5 diff --git a/perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml b/perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml index a07a9416c2..815e70e392 100644 --- a/perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml +++ b/perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: - use_down_sample_filter: False + use_down_sample_filter: True down_sample_voxel_size: 0.1 distance_threshold: 0.5 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 index 0031f3663b..b0731edd74 100644 --- 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 @@ -39,11 +39,12 @@ --> - + - + + diff --git a/perception_launch/launch/object_recognition/detection/detection_preprocess.launch.py b/perception_launch/launch/object_recognition/detection/detection_preprocess.launch.py deleted file mode 100644 index 0ed9100a48..0000000000 --- a/perception_launch/launch/object_recognition/detection/detection_preprocess.launch.py +++ /dev/null @@ -1,136 +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 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 - - -class DetectionPreProcessPipeline: - def __init__(self, context): - self.output_topic = ( - "/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" - ) - - def create_common_pipeline(self, output_topic): - components = [] - downsample_topic = "/perception/obstacle_segmentation/downsampled/pointcloud" - components.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name="voxel_grid_downsample_filter", - remappings=[ - ("input", LaunchConfiguration("input_topic")), - ("output", downsample_topic), - ], - parameters=[ - { - "voxel_size_x": 0.1, - "voxel_size_y": 0.1, - "voxel_size_z": 0.1, - } - ], - 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", downsample_topic), - ("map", "/map/pointcloud_map"), - ("output", output_topic), - ], - parameters=[ - { - "distance_threshold": 0.5, - } - ], - extra_arguments=[ - {"use_intra_process_comms": False}, - ], - ) - ) - return components - - -def launch_setup(context, *args, **kwargs): - pipeline = DetectionPreProcessPipeline(context) - - components = [] - components.extend( - pipeline.create_common_pipeline( - 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("input_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", "detection_preprocess_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/lidar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index da55d0b355..2bb79fdb33 100644 --- a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -9,18 +9,18 @@ - + - + + -