From 0efb63d0e6cb9cca047354b44a72ad917d657b76 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 2 Dec 2021 14:31:47 +0900 Subject: [PATCH] fix/move occupancy grid map outlier filter (#143) * fix package name * add occupancy grid map launch * update launcher * rename filter package --- .../ground_segmentation.launch.py | 32 +--- .../occupancy_grid_map.launch.py | 139 ++++++++++++++++++ .../launch/perception.launch.xml | 10 ++ perception_launch/package.xml | 2 + 4 files changed, 153 insertions(+), 30 deletions(-) create mode 100644 perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py 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 index 8bd6345ad8755..065a6fb9e34a4 100644 --- a/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -16,17 +16,14 @@ from ament_index_python.packages import get_package_share_directory import launch from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare import yaml @@ -302,33 +299,9 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - laserscan_to_occupancy_grid_map_loader = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - FindPackageShare("laserscan_to_occupancy_grid_map"), - "/launch/laserscan_to_occupancy_grid_map.launch.py", - ] - ), - launch_arguments={ - "container": "/perception/obstacle_segmentation/ground_segmentation/perception_pipeline_container", - "input/obstacle_pointcloud": "no_ground/oneshot/pointcloud" - if bool(ground_segmentation_param["additional_lidars"]) - else "no_ground/pointcloud", - "input/raw_pointcloud": "/sensing/lidar/concatenated/pointcloud", - "output": "/perception/occupancy_grid_map/map", - "use_intra_process": LaunchConfiguration("use_intra_process"), - }.items(), - condition=UnlessCondition( - LaunchConfiguration( - "use_compare_map_pipeline", - default=ground_segmentation_param["use_compare_map_pipeline"], - ) - ), - ) - occupancy_outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::OccupancyGridMapOutlierFilterComponent", + 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"), @@ -406,7 +379,6 @@ def launch_setup(context, *args, **kwargs): compare_map_component_loader, concat_data_component_loader, occupancy_grid_outlier_filter_component_loader, - laserscan_to_occupancy_grid_map_loader, ] diff --git a/perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py b/perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py new file mode 100644 index 0000000000000..fc7b9323ad183 --- /dev/null +++ b/perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py @@ -0,0 +1,139 @@ +# 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 LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +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( + condition=LaunchConfigurationEquals("container", ""), + name="occupancy_grid_map_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=composable_nodes, + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + condition=LaunchConfigurationNotEquals("container", ""), + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container"), + ) + + return LaunchDescription( + [ + add_launch_arg("container", ""), + 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"), + 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 index 92aca9080c908..dcf1498e21e9c 100644 --- a/perception_launch/launch/perception.launch.xml +++ b/perception_launch/launch/perception.launch.xml @@ -43,6 +43,16 @@ + + + + + + + + + + diff --git a/perception_launch/package.xml b/perception_launch/package.xml index 37ac98bb9c3ed..f47fc42182deb 100644 --- a/perception_launch/package.xml +++ b/perception_launch/package.xml @@ -19,6 +19,8 @@ multi_object_tracker object_merger object_range_splitter + occupancy_grid_map_outlier_filter + pointcloud_to_laserscan roi_cluster_fusion shape_estimation tensorrt_yolo