diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 541d4e4d4c920..b25c8f3de9a5b 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -23,6 +23,7 @@ from launch.substitutions import PathJoinSubstitution from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile from launch_ros.substitutions import FindPackageShare import yaml @@ -327,7 +328,9 @@ def create_single_frame_obstacle_segmentation_components(self, input_topic, outp return components @staticmethod - def create_time_series_outlier_filter_components(input_topic, output_topic): + def create_time_series_outlier_filter_components( + input_topic, output_topic, ogm_outlier_filter_param + ): components = [] components.append( ComposableNode( @@ -339,6 +342,7 @@ def create_time_series_outlier_filter_components(input_topic, output_topic): ("~/input/pointcloud", input_topic), ("~/output/pointcloud", output_topic), ], + parameters=[ogm_outlier_filter_param], extra_arguments=[ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} ], @@ -545,6 +549,9 @@ def launch_setup(context, *args, **kwargs): else pipeline.single_frame_obstacle_seg_output ), output_topic=pipeline.output_topic, + ogm_outlier_filter_param=ParameterFile( + LaunchConfiguration("ogm_outlier_filter_param_path").perform(context) + ), ) ) pointcloud_container_loader = LoadComposableNodes( @@ -565,6 +572,13 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("pointcloud_container_name", "pointcloud_container") add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud") + add_launch_arg( + "ogm_outlier_filter_param_path", + [ + FindPackageShare("autoware_launch"), + "/config/perception/obstacle_segmentation/occupancy_grid_based_outlier_filter/occupancy_grid_map_outlier_filter.param.yaml", + ], + ) set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/CMakeLists.txt b/perception/autoware_occupancy_grid_map_outlier_filter/CMakeLists.txt index 0fc73d43e54c9..29eb1bea67e73 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/CMakeLists.txt +++ b/perception/autoware_occupancy_grid_map_outlier_filter/CMakeLists.txt @@ -43,4 +43,7 @@ rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent" EXECUTABLE ${PROJECT_NAME}_node) -ament_auto_package(INSTALL_TO_SHARE) +ament_auto_package( + INSTALL_TO_SHARE + config +) diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/README.md b/perception/autoware_occupancy_grid_map_outlier_filter/README.md index 28974edf65053..7de2cc1dce92c 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/README.md +++ b/perception/autoware_occupancy_grid_map_outlier_filter/README.md @@ -40,17 +40,7 @@ The following video is a sample. Yellow points are high occupancy probability, g ## Parameters -| Name | Type | Description | -| ------------------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `map_frame` | string | map frame id | -| `base_link_frame` | string | base link frame id | -| `cost_threshold` | int | Cost threshold of occupancy grid map (0~100). 100 means 100% probability that there is an obstacle, close to 50 means that it is indistinguishable whether it is an obstacle or free space, 0 means that there is no obstacle. | -| `enable_debugger` | bool | Whether to output the point cloud for debugging. | -| `use_radius_search_2d_filter` | bool | Whether or not to apply density-based outlier filters to objects that are judged to have low probability of occupancy on the occupancy grid map. | -| `radius_search_2d_filter/search_radius` | float | Radius when calculating the density | -| `radius_search_2d_filter/min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. | -| `radius_search_2d_filter/min_points` | int | Minimum number of point clouds per radius | -| `radius_search_2d_filter/max_points` | int | Maximum number of point clouds per radius | +{{ json_to_markdown("perception/occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json") }} ## Assumptions / Known limits diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/config/occupancy_grid_map_outlier_filter.param.yaml b/perception/autoware_occupancy_grid_map_outlier_filter/config/occupancy_grid_map_outlier_filter.param.yaml new file mode 100644 index 0000000000000..61cd3a2dc19e2 --- /dev/null +++ b/perception/autoware_occupancy_grid_map_outlier_filter/config/occupancy_grid_map_outlier_filter.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + radius_search_2d_filter.search_radius: 1.0 + radius_search_2d_filter.min_points_and_distance_ratio: 400.0 + radius_search_2d_filter.min_points: 4 + radius_search_2d_filter.max_points: 70 + radius_search_2d_filter.max_filter_points_nb: 15000 + map_frame: "map" + base_link_frame: "base_link" + cost_threshold: 45 + use_radius_search_2d_filter: true + enable_debugger: false diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json b/perception/autoware_occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json new file mode 100644 index 0000000000000..d31f77bf27968 --- /dev/null +++ b/perception/autoware_occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json @@ -0,0 +1,86 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for occupancy_grid_map_outlier", + "type": "object", + "definitions": { + "occupancy_grid_map_outlier": { + "type": "object", + "properties": { + "radius_search_2d_filter.search_radius": { + "type": "number", + "default": 1.0, + "description": "Radius when calculating the density" + }, + "radius_search_2d_filter.min_points_and_distance_ratio": { + "type": "number", + "default": 400.0, + "description": "Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink" + }, + "radius_search_2d_filter.min_points": { + "type": "number", + "default": 4, + "description": "Minimum number of point clouds per radius" + }, + "radius_search_2d_filter.max_points": { + "type": "number", + "default": 70, + "description": "Maximum number of point clouds per radius" + }, + "radius_search_2d_filter.max_filter_points_nb": { + "type": "number", + "default": 15000, + "description": "Maximum number of point clouds to be filtered" + }, + "map_frame": { + "type": "string", + "default": "map", + "description": "map frame id" + }, + "base_link_frame": { + "type": "string", + "default": "base_link", + "description": "base link frame id" + }, + "cost_threshold": { + "type": "number", + "default": 45, + "description": "Cost threshold of occupancy grid map (0~100). 100 means 100% probability that there is an obstacle, close to 50 means that it is indistinguishable whether it is an obstacle or free space, 0 means that there is no obstacle" + }, + "use_radius_search_2d_filter": { + "type": "boolean", + "default": true, + "description": "Whether or not to apply density-based outlier filters to objects that are judged to have low probability of occupancy on the occupancy grid map" + }, + "enable_debugger": { + "type": "boolean", + "default": false, + "description": "Whether to output the point cloud for debugging" + } + }, + "required": [ + "radius_search_2d_filter.search_radius", + "radius_search_2d_filter.min_points_and_distance_ratio", + "radius_search_2d_filter.min_points", + "radius_search_2d_filter.max_points", + "radius_search_2d_filter.max_filter_points_nb", + "map_frame", + "base_link_frame", + "cost_threshold", + "use_radius_search_2d_filter", + "enable_debugger" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/occupancy_grid_map_outlier" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp index 1536db8610b76..046598e445ba8 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp +++ b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp @@ -106,13 +106,13 @@ namespace autoware::occupancy_grid_map_outlier_filter { RadiusSearch2dFilter::RadiusSearch2dFilter(rclcpp::Node & node) { - search_radius_ = node.declare_parameter("radius_search_2d_filter.search_radius", 1.0f); + search_radius_ = node.declare_parameter("radius_search_2d_filter.search_radius"); min_points_and_distance_ratio_ = - node.declare_parameter("radius_search_2d_filter.min_points_and_distance_ratio", 400.0f); - min_points_ = node.declare_parameter("radius_search_2d_filter.min_points", 4); - max_points_ = node.declare_parameter("radius_search_2d_filter.max_points", 70); + node.declare_parameter("radius_search_2d_filter.min_points_and_distance_ratio"); + min_points_ = node.declare_parameter("radius_search_2d_filter.min_points"); + max_points_ = node.declare_parameter("radius_search_2d_filter.max_points"); max_filter_points_nb_ = - node.declare_parameter("radius_search_2d_filter.max_filter_points_nb", 15000); + node.declare_parameter("radius_search_2d_filter.max_filter_points_nb"); kd_tree_ = pcl::make_shared>(false); } @@ -235,11 +235,11 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( } /* params */ - map_frame_ = declare_parameter("map_frame", "map"); - base_link_frame_ = declare_parameter("base_link_frame", "base_link"); - cost_threshold_ = declare_parameter("cost_threshold", 45); - auto use_radius_search_2d_filter = declare_parameter("use_radius_search_2d_filter", true); - auto enable_debugger = declare_parameter("enable_debugger", false); + map_frame_ = declare_parameter("map_frame"); + base_link_frame_ = declare_parameter("base_link_frame"); + cost_threshold_ = declare_parameter("cost_threshold"); + auto use_radius_search_2d_filter = declare_parameter("use_radius_search_2d_filter"); + auto enable_debugger = declare_parameter("enable_debugger"); /* tf */ tf2_ = std::make_shared(get_clock());