Skip to content

Commit

Permalink
Localization util preprocessor component (autowarefoundation#188)
Browse files Browse the repository at this point in the history 
Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>
  • Loading branch information
@wep21
wep21 authored Apr 12, 2021
1 parent c97d953 commit e5a1926
Show file tree
Hide file tree
Showing 2 changed files with 105 additions and 31 deletions.
98 changes: 98 additions & 0 deletions localization_launch/launch/util/util.launch.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
# 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, OpaqueFunction
from launch.conditions import IfCondition, UnlessCondition, LaunchConfigurationNotEquals
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes
from launch_ros.descriptions import ComposableNode
from launch.substitutions import EnvironmentVariable


def generate_launch_description():
crop_box_component = ComposableNode(
package='pointcloud_preprocessor',
plugin='pointcloud_preprocessor::CropBoxFilterComponent',
name='crop_box_filter_measurement_range',
remappings=[
('input', LaunchConfiguration('input_sensor_points_topic')),
('output',
'mesurement_range/pointcloud'),
],
parameters=[{
'input_frame': LaunchConfiguration('base_frame'),
'output_frame': LaunchConfiguration('base_frame'),
'min_x': -60.0,
'max_x': 60.0,
'min_y': -60.0,
'max_y': 60.0,
'min_z': -30.0,
'max_z': 50.0,
'negative': False,
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
}],
extra_arguments=[{
'use_intra_process_comms': LaunchConfiguration('use_intra_process')
}],
)
voxel_grid_downsample_component = ComposableNode(
package='pointcloud_preprocessor',
plugin='pointcloud_preprocessor::VoxelGridDownsampleFilterComponent',
name='voxel_grid_downsample_filter',
remappings=[
('input',
'mesurement_range/pointcloud'),
('output',
LaunchConfiguration('output_voxel_grid_downsample_sensor_points_topic')),
],
parameters=[{
'voxel_size_x': 3.0,
'voxel_size_y': 3.0,
'voxel_size_z': 3.0,
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
}],
extra_arguments=[{
'use_intra_process_comms': LaunchConfiguration('use_intra_process')
}],
)
random_downsample_component = ComposableNode(
package='pointcloud_preprocessor',
plugin='pointcloud_preprocessor::VoxelGridDownsampleFilterComponent',
name='random_downsample_filter',
remappings=[
('input',
LaunchConfiguration('output_voxel_grid_downsample_sensor_points_topic')),
('output',
LaunchConfiguration('output_downsample_sensor_points_topic')),
],
parameters=[{
'sample_num': 1500,
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
}],
extra_arguments=[{
'use_intra_process_comms': LaunchConfiguration('use_intra_process')
}],
)

composable_nodes = [crop_box_component,
voxel_grid_downsample_component,
random_downsample_component]

load_composable_nodes = LoadComposableNodes(
condition=LaunchConfigurationNotEquals('container', ''),
composable_node_descriptions=composable_nodes,
target_container=LaunchConfiguration('container'),
)
return launch.LaunchDescription([load_composable_nodes])
38 changes: 7 additions & 31 deletions localization_launch/launch/util/util.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,41 +10,17 @@
<!-- frame_ids -->
<arg name="base_frame" default="base_link" description="Vehicle reference frame" />

<!-- container -->
<arg name="container" default="/sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container" />

<!-- option -->
<arg name="use_intra_process" default="false" />

<!-- pose_initializer -->
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
<arg name="ndt_align_server_name" value="/localization/pose_estimator/ndt_align_srv" />
</include>

<node pkg="pointcloud_preprocessor" exec="crop_box_filter_node" name="crop_box_filter_measurement_range" output="log">
<param name="use_sim_time" value="$(env AW_ROS2_USE_SIM_TIME false)" />
<remap from="input" to="$(var input_sensor_points_topic)" />
<remap from="output" to="$(var output_measurement_range_sensor_points_topic)" />
<param name="min_x" value="-60.0" />
<param name="max_x" value="60.0" />
<param name="min_y" value="-60.0" />
<param name="max_y" value="60.0" />
<param name="min_z" value="-30.0" />
<param name="max_z" value="50.0" />
<param name="negative" value="False" />
<param name="input_frame" value="$(var base_frame)" />
<param name="output_frame" value="$(var base_frame)" />
</node>

<!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->
<node pkg="pointcloud_preprocessor" exec="voxel_grid_downsample_filter_node" name="voxel_grid_filter" output="log">
<param name="use_sim_time" value="$(env AW_ROS2_USE_SIM_TIME false)"/>
<remap from="input" to="$(var output_measurement_range_sensor_points_topic)" />
<remap from="output" to="$(var output_voxel_grid_downsample_sensor_points_topic)" />
<param name="voxel_size_x" value="3.0" />
<param name="voxel_size_y" value="3.0" />
<param name="voxel_size_z" value="3.0" />
</node>

<node pkg="pointcloud_preprocessor" exec="random_downsample_filter_node" name="random_filter" output="log">
<param name="use_sim_time" value="$(env AW_ROS2_USE_SIM_TIME false)"/>
<remap from="input" to="$(var output_voxel_grid_downsample_sensor_points_topic)" />
<remap from="output" to="$(var output_downsample_sensor_points_topic)" />
<param name="sample_num" value="1500" />
</node>
<include file="$(find-pkg-share localization_launch)/launch/util/util.launch.py" />

</launch>

0 comments on commit e5a1926

Please sign in to comment.