Skip to content

Commit

Permalink
Sync with Ros2 v0.8.0 beta (autowarefoundation#71)
Browse files Browse the repository at this point in the history 
* update sensing launch to support aip_x1 (autowarefoundation#69)

Signed-off-by: taichiH <azumade.30@gmail.com>

* fix logging_simulator_bug (autowarefoundation#68)

Signed-off-by: taichiH <azumade.30@gmail.com>

* fix aip_x1 param (autowarefoundation#70)

Signed-off-by: taichiH <azumade.30@gmail.com>

Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
  • Loading branch information
@kenji-miyake @1222-takeshi
2 people authored and 1222-takeshi committed Dec 10, 2021
1 parent 2a3e6f6 commit a127b4b
Showing 1 changed file with 97 additions and 15 deletions.
112 changes: 97 additions & 15 deletions launch/sensing_launch/launch/aip_x1/pointcloud_preprocessor.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
from launch_ros.descriptions import ComposableNode
from launch.substitutions import EnvironmentVariable


def get_vehicle_info(context):
path = LaunchConfiguration('vehicle_param_file').perform(context)
with open(path, 'r') as f:
Expand All @@ -38,14 +39,14 @@ def get_vehicle_info(context):
p['max_height_offset'] = p['vehicle_height']
return p


def get_vehicle_mirror_info(context):
path = LaunchConfiguration('vehicle_mirror_param_file').perform(context)
with open(path, 'r') as f:
p = yaml.safe_load(f)['/**']['ros__parameters']
return p



def launch_setup(context, *args, **kwargs):

pkg = 'pointcloud_preprocessor'
Expand All @@ -60,10 +61,10 @@ def launch_setup(context, *args, **kwargs):
remappings=[('/output', 'concatenated/pointcloud')],
parameters=[{
'input_topics': ['/sensing/lidar/top/outlier_filtered/pointcloud',
'/sensing/lidar/front_left/mirror_cropped/pointcloud',
'/sensing/lidar/front_right/mirror_cropped/pointcloud',
'/sensing/lidar/front_center/mirror_cropped/pointcloud'],
'output_frame': 'base_link',
'/sensing/lidar/front_left/mirror_cropped/pointcloud',
'/sensing/lidar/front_right/mirror_cropped/pointcloud',
'/sensing/lidar/front_center/mirror_cropped/pointcloud'],
'output_frame': LaunchConfiguration('base_frame'),
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
}]
)
Expand All @@ -78,7 +79,7 @@ def launch_setup(context, *args, **kwargs):
('output', 'concatenated/pointcloud'),
],
parameters=[{
'output_frame': 'base_link',
'output_frame': LaunchConfiguration('base_frame'),
'min_z': vehicle_info['min_height_offset'],
'max_z': vehicle_info['max_height_offset'],
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
Expand All @@ -101,26 +102,28 @@ def launch_setup(context, *args, **kwargs):
'max_x': 100.0,
'min_y': -50.0,
'max_y': 50.0,
'min_z': vehicle_info['min_height_offset'],
'min_z': -0.5,
'max_z': vehicle_info['max_height_offset'],
'negative': False,
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
}]
)

ground_component = ComposableNode(
ray_ground_filter_component = ComposableNode(
package=pkg,
plugin='pointcloud_preprocessor::RayGroundFilterComponent',
name='ray_ground_filter',
remappings=[
('input', 'measurement_range_cropped/pointcloud'),
('output', 'no_ground/pointcloud_with_outlier'),
('output', 'rough/no_ground/pointcloud'),
],
parameters=[{
"initial_max_slope": 1.0,
"initial_max_slope": 10.0,
"general_max_slope": 10.0,
"local_max_slope": 10.0,
"min_height_threshold": 0.1,
"min_height_threshold": 0.3,
"radial_devider_angle": 1.0,
"concentric_devider_distance": 0.0,
"use_vehicle_footprint": True,
"min_x": vehicle_info['min_longitudinal_offset'],
"max_x": vehicle_info['max_longitudinal_offset'],
Expand All @@ -130,13 +133,87 @@ def launch_setup(context, *args, **kwargs):
}]
)

short_height_obstacle_detection_area_filter_component = ComposableNode(
package=pkg,
plugin='pointcloud_preprocessor::CropBoxFilterComponent',
name='short_height_obstacle_detection_area_filter',
remappings=[
('input', 'front_center/mirror_cropped/pointcloud'),
('output', 'short_height_obstacle_detection_area/pointcloud'),
],
parameters=[{
'input_frame': LaunchConfiguration('base_frame'),
'output_frame': LaunchConfiguration('base_frame'),
'min_x': 0.0,
'max_x': 15.6, # max_x: 14.0m + base_link2livox_front_center distance 1.6m
'min_y': -4.0,
'max_y': 4.0,
'min_z': -0.5,
'max_z': 0.5,
'negative': False,
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
}]
)

vector_map_filter_component = ComposableNode(
package=pkg,
plugin='pointcloud_preprocessor::Lanelet2MapFilterComponent',
name='vector_map_filter',
remappings=[
('input/pointcloud', 'short_height_obstacle_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,
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
}]
)

ransac_ground_filter_component = ComposableNode(
package=pkg,
plugin='pointcloud_preprocessor::RANSACGroundFilterComponent',
name='ransac_ground_filter',
remappings=[
('input', 'vector_map_filtered/pointcloud'),
('output', 'short_height/no_ground/pointcloud'),
],
parameters=[{
'outlier_threshold': 0.1,
'min_points': 400,
'min_inliers': 200,
'max_iterations': 50,
'height_threshold': 0.12,
'plane_slope_threshold': 10.0,
'voxel_size_x': 0.2,
'voxel_size_y': 0.2,
'voxel_size_z': 0.2,
'debug': False,
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
}]
)

concat_no_ground_component = ComposableNode(
package=pkg,
plugin='pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent',
name='concatenate_no_ground_data',
remappings=[('output', 'no_ground/concatenated/pointcloud')],
parameters=[{
'input_topics': ['/sensing/lidar/rough/no_ground/pointcloud',
'/sensing/lidar/short_height/no_ground/pointcloud'],
'output_frame': LaunchConfiguration('base_frame'),
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
}]
)

voxel_grid_filter_component = ComposableNode(
package=pkg,
plugin='pointcloud_preprocessor::VoxelGridDownsampleFilterComponent',
name='voxel_grid_filter',
remappings=[
('/input', 'no_ground/pointcloud_with_outlier'),
('/output', 'voxel_grid_filtered/pointcloud'),
('input', 'no_ground/concatenated/pointcloud'),
('output', 'voxel_grid_filtered/pointcloud'),
],
parameters=[{
"input_frame": LaunchConfiguration('base_frame'),
Expand Down Expand Up @@ -185,7 +262,7 @@ def launch_setup(context, *args, **kwargs):
plugin='topic_tools::RelayNode',
name='relay',
parameters=[{
"input_topic": "/sensing/lidar/top/rectified/pointcloud",
"input_topic": "/sensing/lidar/top/outlier_filtered/pointcloud",
"output_topic": "/sensing/lidar/pointcloud",
"type": "sensor_msgs/msg/PointCloud2",
'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False'),
Expand All @@ -200,7 +277,12 @@ def launch_setup(context, *args, **kwargs):
executable='component_container',
composable_node_descriptions=[
cropbox_component,
ground_component,
ray_ground_filter_component,
short_height_obstacle_detection_area_filter_component,
vector_map_filter_component,
ransac_ground_filter_component,
concat_no_ground_component,
voxel_grid_filter_component,
relay_component,
],
output='screen',
Expand Down

0 comments on commit a127b4b

Please sign in to comment.