From 247e5c7309fa6c10044910a05f74d206f8a1b312 Mon Sep 17 00:00:00 2001 From: Boyang <144167190+tby-udel@users.noreply.github.com> Date: Thu, 18 Jul 2024 08:46:12 -0400 Subject: [PATCH] refactor(probabilistic_occupancy_grid_map): rework parameters (#7339) * refactor(probabilistic_occupancy_grid_map): rework parameters Signed-off-by: Boyang * style(pre-commit): autofix * Solved the CI's complainment Signed-off-by: Boyang --------- Signed-off-by: Boyang Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 60 +---- ...rid_map_param.yaml => grid_map.param.yaml} | 0 .../binary_bayes_filter_updater.schema.json | 61 ++++++ .../schema/grid_map.schema.json | 76 +++++++ ...rscan_based_occupancy_grid_map.schema.json | 94 ++++++++ ...cloud_based_occupancy_grid_map.schema.json | 207 ++++++++++++++++++ ...cloud_based_occupancy_grid_map.schema.json | 150 +++++++++++++ ...chronized_grid_map_fusion_node.schema.json | 105 +++++++++ 8 files changed, 701 insertions(+), 52 deletions(-) rename perception/probabilistic_occupancy_grid_map/config/{grid_map_param.yaml => grid_map.param.yaml} (100%) create mode 100644 perception/probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json create mode 100644 perception/probabilistic_occupancy_grid_map/schema/grid_map.schema.json create mode 100644 perception/probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json create mode 100644 perception/probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json create mode 100644 perception/probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json create mode 100644 perception/probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 97d66bf9f648c..4200836181276 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -19,58 +19,14 @@ You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which mean ![image_for_frame_parameter_visualization](./image/gridmap_frame_settings.drawio.svg) -### Each config parameters - -Config parameters are managed in `config/*.yaml` and here shows its outline. - -- Pointcloud based occupancy grid map - -| Ros param name | Default value | -| -------------------------------------------- | ------------- | -| map_frame | "map" | -| base_link_frame | "base_link" | -| scan_origin_frame | "base_link" | -| gridmap_origin_frame | "base_link" | -| use_height_filter | true | -| enable_single_frame_mode | false | -| filter_obstacle_pointcloud_by_raw_pointcloud | false | -| map_length | 150.0 [m] | -| map_resolution | 0.5 [m] | -| use_projection | false | -| projection_dz_threshold | 0.01 | -| obstacle_separation_threshold | 1.0 | -| input_obstacle_pointcloud | true | -| input_obstacle_and_raw_pointcloud | true | - -- Laserscan based occupancy grid map - -| Ros param name | Default value | -| ------------------------ | ------------- | -| map_length | 150 [m] | -| map_width | 150 [m] | -| map_resolution | 0.5 [m] | -| use_height_filter | true | -| enable_single_frame_mode | false | -| map_frame | "map" | -| base_link_frame | "base_link" | -| scan_origin_frame | "base_link" | -| gridmap_origin_frame | "base_link" | - -## Other parameters - -Additional argument is shown below: - -| Name | Default | Description | -| ----------------------------------- | ------------------------------ | --------------------------------------------------------------------------------------------- | -| `use_multithread` | `false` | whether to use multithread | -| `use_intra_process` | `false` | | -| `map_origin` | `` | parameter to override `map_origin_frame` which means grid map origin | -| `scan_origin` | `` | parameter to override `scan_origin_frame` which means scanning center | -| `output` | `occupancy_grid` | output name | -| `use_pointcloud_container` | `false` | | -| `container_name` | `occupancy_grid_map_container` | | -| `input_obstacle_pointcloud` | `false` | only for laserscan based method. If true, the node subscribe obstacle pointcloud | -| `input_obstacle_and_raw_pointcloud` | `true` | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud | +### Parameters + +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/grid_map.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json") }} ### Downsample input pointcloud(Optional) diff --git a/perception/probabilistic_occupancy_grid_map/config/grid_map_param.yaml b/perception/probabilistic_occupancy_grid_map/config/grid_map.param.yaml similarity index 100% rename from perception/probabilistic_occupancy_grid_map/config/grid_map_param.yaml rename to perception/probabilistic_occupancy_grid_map/config/grid_map.param.yaml diff --git a/perception/probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json b/perception/probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json new file mode 100644 index 0000000000000..d087095ce980b --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json @@ -0,0 +1,61 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Binary Bayes Filter Updater", + "type": "object", + "definitions": { + "binary_bayes_filter_updater": { + "type": "object", + "properties": { + "probability_matrix": { + "type": "object", + "properties": { + "occupied_to_occupied": { + "type": "number", + "description": "Probability of transitioning from occupied to occupied state.", + "default": 0.95 + }, + "occupied_to_free": { + "type": "number", + "description": "Probability of transitioning from occupied to free state.", + "default": 0.05 + }, + "free_to_occupied": { + "type": "number", + "description": "Probability of transitioning from free to occupied state.", + "default": 0.2 + }, + "free_to_free": { + "type": "number", + "description": "Probability of transitioning from free to free state.", + "default": 0.8 + } + }, + "required": [ + "occupied_to_occupied", + "occupied_to_free", + "free_to_occupied", + "free_to_free" + ] + }, + "v_ratio": { + "type": "number", + "description": "Ratio of the variance used in the filter.", + "default": 0.1 + } + }, + "required": ["probability_matrix", "v_ratio"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/binary_bayes_filter_updater" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/grid_map.schema.json b/perception/probabilistic_occupancy_grid_map/schema/grid_map.schema.json new file mode 100644 index 0000000000000..e874431ec2601 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/grid_map.schema.json @@ -0,0 +1,76 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Grid Map", + "type": "object", + "definitions": { + "visualization_params": { + "type": "object", + "properties": { + "type": { + "type": "string", + "description": "The type of grid map visualization.", + "default": "occupancy_grid" + }, + "params": { + "type": "object", + "properties": { + "layer": { + "type": "string", + "description": "The layer of the grid map visualization.", + "default": "filled_free_to_farthest" + }, + "data_min": { + "type": "number", + "description": "The minimum data value for the visualization.", + "default": 0.0 + }, + "data_max": { + "type": "number", + "description": "The maximum data value for the visualization.", + "default": 100.0 + } + }, + "required": ["layer", "data_min", "data_max"] + } + }, + "required": ["type", "params"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "grid_map_topic": { + "type": "string", + "description": "The topic name for the grid map.", + "default": "/perception/occupancy_grid_map/occupancy_grid_map_node/debug/grid_map" + }, + "grid_map_visualizations": { + "type": "array", + "description": "List of grid map visualizations.", + "items": { + "type": "string" + }, + "default": ["grid_1st_step", "grid_2nd_step", "grid_3rd_step"] + }, + "grid_1st_step": { "$ref": "#/definitions/visualization_params" }, + "grid_2nd_step": { "$ref": "#/definitions/visualization_params" }, + "grid_3rd_step": { "$ref": "#/definitions/visualization_params" } + }, + "required": [ + "grid_map_topic", + "grid_map_visualizations", + "grid_1st_step", + "grid_2nd_step", + "grid_3rd_step" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json b/perception/probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json new file mode 100644 index 0000000000000..2b6befd1ae1fd --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json @@ -0,0 +1,94 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Laserscan Based Occupancy Grid Map", + "type": "object", + "definitions": { + "height_filter_params": { + "type": "object", + "properties": { + "use_height_filter": { + "type": "boolean", + "description": "Flag to use height filter.", + "default": true + }, + "min_height": { + "type": "number", + "description": "Minimum height for the height filter.", + "default": -1.0 + }, + "max_height": { + "type": "number", + "description": "Maximum height for the height filter.", + "default": 2.0 + } + }, + "required": ["use_height_filter", "min_height", "max_height"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "map_frame": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "gridmap_origin_frame": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "scan_origin_frame": { + "type": "string", + "description": "The frame ID of the scan origin.", + "default": "base_link" + }, + "height_filter": { "$ref": "#/definitions/height_filter_params" }, + "enable_single_frame_mode": { + "type": "boolean", + "description": "Flag to enable single frame mode.", + "default": false + }, + "map_length": { + "type": "number", + "description": "The length of the map.", + "default": 150.0 + }, + "map_width": { + "type": "number", + "description": "The width of the map.", + "default": 150.0 + }, + "map_resolution": { + "type": "number", + "description": "The resolution of the map.", + "default": 0.5 + } + }, + "required": [ + "map_frame", + "base_link_frame", + "gridmap_origin_frame", + "scan_origin_frame", + "height_filter", + "enable_single_frame_mode", + "map_length", + "map_width", + "map_resolution" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json b/perception/probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json new file mode 100644 index 0000000000000..e522e486e88cc --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json @@ -0,0 +1,207 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Multi Lidar Pointcloud Based Occupancy Grid Map", + "type": "object", + "definitions": { + "height_filter_params": { + "type": "object", + "properties": { + "use_height_filter": { + "type": "boolean", + "description": "Flag to use height filter.", + "default": true + }, + "min_height": { + "type": "number", + "description": "Minimum height for the height filter.", + "default": -1.0 + }, + "max_height": { + "type": "number", + "description": "Maximum height for the height filter.", + "default": 2.0 + } + }, + "required": ["use_height_filter", "min_height", "max_height"] + }, + "occupancy_grid_map_fixed_blind_spot_params": { + "type": "object", + "properties": { + "distance_margin": { + "type": "number", + "description": "Distance margin for the fixed blind spot.", + "default": 1.0 + } + }, + "required": ["distance_margin"] + }, + "occupancy_grid_map_projective_blind_spot_params": { + "type": "object", + "properties": { + "projection_dz_threshold": { + "type": "number", + "description": "Projection dz threshold to avoid null division.", + "default": 0.01 + }, + "obstacle_separation_threshold": { + "type": "number", + "description": "Threshold to fill the interval between obstacles with unknown.", + "default": 1.0 + }, + "pub_debug_grid": { + "type": "boolean", + "description": "Flag to publish the debug grid.", + "default": false + } + }, + "required": ["projection_dz_threshold", "obstacle_separation_threshold", "pub_debug_grid"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "shared_config": { + "type": "object", + "properties": { + "map_frame": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "gridmap_origin_frame": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "map_resolution": { + "type": "number", + "description": "The resolution of the map.", + "default": 0.5 + }, + "map_length_x": { + "type": "number", + "description": "The length of the map in the x direction.", + "default": 150.0 + }, + "map_length_y": { + "type": "number", + "description": "The length of the map in the y direction.", + "default": 150.0 + } + }, + "required": [ + "map_frame", + "base_link_frame", + "gridmap_origin_frame", + "map_resolution", + "map_length_x", + "map_length_y" + ] + }, + "downsample_input_pointcloud": { + "type": "boolean", + "description": "Flag to downsample the input pointcloud.", + "default": true + }, + "downsample_voxel_size": { + "type": "number", + "description": "The voxel size for downsampling.", + "default": 0.25 + }, + "ogm_creation_config": { + "type": "object", + "properties": { + "height_filter": { "$ref": "#/definitions/height_filter_params" }, + "enable_single_frame_mode": { + "type": "boolean", + "description": "Flag to enable single frame mode.", + "default": true + }, + "filter_obstacle_pointcloud_by_raw_pointcloud": { + "type": "boolean", + "description": "Flag to filter obstacle pointcloud by raw pointcloud.", + "default": false + }, + "grid_map_type": { + "type": "string", + "description": "Type of the grid map.", + "default": "OccupancyGridMapFixedBlindSpot" + }, + "OccupancyGridMapFixedBlindSpot": { + "$ref": "#/definitions/occupancy_grid_map_fixed_blind_spot_params" + }, + "OccupancyGridMapProjectiveBlindSpot": { + "$ref": "#/definitions/occupancy_grid_map_projective_blind_spot_params" + } + }, + "required": [ + "height_filter", + "enable_single_frame_mode", + "filter_obstacle_pointcloud_by_raw_pointcloud", + "grid_map_type" + ] + }, + "fusion_config": { + "type": "object", + "properties": { + "raw_pointcloud_topics": { + "type": "array", + "description": "List of raw pointcloud topics.", + "items": { + "type": "string" + } + }, + "fusion_input_ogm_topics": { + "type": "array", + "description": "List of fusion input occupancy grid map topics.", + "items": { + "type": "string" + } + }, + "input_ogm_reliabilities": { + "type": "array", + "description": "Reliability of each sensor for fusion.", + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0 + } + }, + "fusion_method": { + "type": "string", + "description": "Method for occupancy grid map fusion.", + "enum": ["overwrite", "log-odds", "dempster-shafer"], + "default": "overwrite" + } + }, + "required": [ + "raw_pointcloud_topics", + "fusion_input_ogm_topics", + "input_ogm_reliabilities", + "fusion_method" + ] + } + }, + "required": [ + "shared_config", + "downsample_input_pointcloud", + "downsample_voxel_size", + "ogm_creation_config", + "fusion_config" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json b/perception/probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json new file mode 100644 index 0000000000000..e79c094820ec4 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json @@ -0,0 +1,150 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Pointcloud Based Occupancy Grid Map", + "type": "object", + "definitions": { + "height_filter_params": { + "type": "object", + "properties": { + "use_height_filter": { + "type": "boolean", + "description": "Flag to use height filter.", + "default": true + }, + "min_height": { + "type": "number", + "description": "Minimum height for the height filter.", + "default": -1.0 + }, + "max_height": { + "type": "number", + "description": "Maximum height for the height filter.", + "default": 2.0 + } + }, + "required": ["use_height_filter", "min_height", "max_height"] + }, + "occupancy_grid_map_fixed_blind_spot_params": { + "type": "object", + "properties": { + "distance_margin": { + "type": "number", + "description": "Distance margin for the fixed blind spot.", + "default": 1.0 + } + }, + "required": ["distance_margin"] + }, + "occupancy_grid_map_projective_blind_spot_params": { + "type": "object", + "properties": { + "projection_dz_threshold": { + "type": "number", + "description": "Projection dz threshold to avoid null division.", + "default": 0.01 + }, + "obstacle_separation_threshold": { + "type": "number", + "description": "Threshold to fill the interval between obstacles with unknown.", + "default": 1.0 + }, + "pub_debug_grid": { + "type": "boolean", + "description": "Flag to publish the debug grid.", + "default": false + } + }, + "required": ["projection_dz_threshold", "obstacle_separation_threshold", "pub_debug_grid"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "map_length": { + "type": "number", + "description": "The length of the map.", + "default": 150.0 + }, + "map_resolution": { + "type": "number", + "description": "The resolution of the map.", + "default": 0.5 + }, + "height_filter": { "$ref": "#/definitions/height_filter_params" }, + "downsample_input_pointcloud": { + "type": "boolean", + "description": "Flag to downsample the input pointcloud.", + "default": true + }, + "downsample_voxel_size": { + "type": "number", + "description": "The voxel size for downsampling.", + "default": 0.25 + }, + "enable_single_frame_mode": { + "type": "boolean", + "description": "Flag to enable single frame mode.", + "default": false + }, + "filter_obstacle_pointcloud_by_raw_pointcloud": { + "type": "boolean", + "description": "Flag to filter obstacle pointcloud by raw pointcloud.", + "default": false + }, + "map_frame": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "gridmap_origin_frame": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "scan_origin_frame": { + "type": "string", + "description": "The frame ID of the scan origin.", + "default": "base_link" + }, + "grid_map_type": { + "type": "string", + "description": "Type of the grid map.", + "default": "OccupancyGridMapFixedBlindSpot" + }, + "OccupancyGridMapFixedBlindSpot": { + "$ref": "#/definitions/occupancy_grid_map_fixed_blind_spot_params" + }, + "OccupancyGridMapProjectiveBlindSpot": { + "$ref": "#/definitions/occupancy_grid_map_projective_blind_spot_params" + } + }, + "required": [ + "map_length", + "map_resolution", + "height_filter", + "downsample_input_pointcloud", + "downsample_voxel_size", + "enable_single_frame_mode", + "filter_obstacle_pointcloud_by_raw_pointcloud", + "map_frame", + "base_link_frame", + "gridmap_origin_frame", + "scan_origin_frame", + "grid_map_type" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json b/perception/probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json new file mode 100644 index 0000000000000..14f4305f55de8 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json @@ -0,0 +1,105 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Synchronized Grid Map Fusion Node", + "type": "object", + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "fusion_input_ogm_topics": { + "type": "array", + "description": "List of fusion input occupancy grid map topics.", + "items": { + "type": "string" + }, + "default": ["topic1", "topic2"] + }, + "input_ogm_reliabilities": { + "type": "array", + "description": "Reliability of each sensor for fusion.", + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0 + }, + "default": [0.8, 0.2] + }, + "fusion_method": { + "type": "string", + "description": "Method for occupancy grid map fusion.", + "enum": ["overwrite", "log-odds", "dempster-shafer"], + "default": "overwrite" + }, + "match_threshold_sec": { + "type": "number", + "description": "Time threshold for matching in seconds.", + "default": 0.01 + }, + "timeout_sec": { + "type": "number", + "description": "Timeout for synchronization in seconds.", + "default": 0.1 + }, + "input_offset_sec": { + "type": "array", + "description": "Offset for each input in seconds.", + "items": { + "type": "number" + }, + "default": [0.0, 0.0] + }, + "map_frame_": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame_": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "grid_map_origin_frame_": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "fusion_map_length_x": { + "type": "number", + "description": "The length of the fusion map in the x direction.", + "default": 100.0 + }, + "fusion_map_length_y": { + "type": "number", + "description": "The length of the fusion map in the y direction.", + "default": 100.0 + }, + "fusion_map_resolution": { + "type": "number", + "description": "The resolution of the fusion map.", + "default": 0.5 + } + }, + "required": [ + "fusion_input_ogm_topics", + "input_ogm_reliabilities", + "fusion_method", + "match_threshold_sec", + "timeout_sec", + "input_offset_sec", + "map_frame_", + "base_link_frame_", + "grid_map_origin_frame_", + "fusion_map_length_x", + "fusion_map_length_y", + "fusion_map_resolution" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +}