refactor(perception/occupancy_grid_map_outlier_filter): rework parame…

…ters (#6745) * add param and schema file, edit readme Signed-off-by: oguzkaganozt <[email protected]> * . Signed-off-by: Oguz Ozturk <[email protected]> * correct linter errors Signed-off-by: oguzkaganozt <[email protected]> --------- Signed-off-by: oguzkaganozt <[email protected]> Signed-off-by: Oguz Ozturk <[email protected]>
autowarefoundation · Aug 30, 2024 · 3e4a945 · 3e4a945
1 parent 5020617
commit 3e4a945
Show file tree

Hide file tree

Showing 6 changed files with 128 additions and 23 deletions.
diff --git a/...ion_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/...ion_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
@@ -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
@@ -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/...are_occupancy_grid_map_outlier_filter/config/occupancy_grid_map_outlier_filter.param.yaml b/...are_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/...re_occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json b/...re_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/...autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp b/...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<float>("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<float>("radius_search_2d_filter.min_points_and_distance_ratio");
+  min_points_ = node.declare_parameter<int>("radius_search_2d_filter.min_points");
+  max_points_ = node.declare_parameter<int>("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<int>("radius_search_2d_filter.max_filter_points_nb");
   kd_tree_ = pcl::make_shared<pcl::search::KdTree<pcl::PointXY>>(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<std::string>("map_frame");
+  base_link_frame_ = declare_parameter<std::string>("base_link_frame");
+  cost_threshold_ = declare_parameter<int>("cost_threshold");
+  auto use_radius_search_2d_filter = declare_parameter<bool>("use_radius_search_2d_filter");
+  auto enable_debugger = declare_parameter<bool>("enable_debugger");
 
   /* tf */
   tf2_ = std::make_shared<tf2_ros::Buffer>(get_clock());