From c5ab77af08e18d009fe462a214758342e66690ec Mon Sep 17 00:00:00 2001 From: Batuhan Beytekin <71197983+batuhanbeytekin@users.noreply.github.com> Date: Fri, 20 Sep 2024 14:47:14 +0300 Subject: [PATCH] refactor(detected_object_validation): rework parameters (#7750) * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * style(pre-commit): autofix Signed-off-by: Batuhan Beytekin * Update perception/detected_object_validation/schema/object_lanelet_filter.schema.json Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * Update perception/detected_object_validation/schema/object_position_filter.schema.json Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * style(pre-commit): autofix Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * style(pre-commit): autofix --------- Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 18 +++ .../occupancy_grid_based_validator.param.yaml | 1 + .../schema/object_lanelet_filter.schema.json | 121 ++++++++++++++++++ .../schema/object_position_filter.schema.json | 106 +++++++++++++++ ...cle_pointcloud_based_validator.schema.json | 68 ++++++++++ ...occupancy_grid_based_validator.schema.json | 38 ++++++ .../src/lanelet_filter/lanelet_filter.cpp | 16 +-- .../obstacle_pointcloud_validator.cpp | 2 +- .../occupancy_grid_map_validator.cpp | 4 +- .../src/position_filter/position_filter.cpp | 24 ++-- 10 files changed, 375 insertions(+), 23 deletions(-) create mode 100644 perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json create mode 100644 perception/autoware_detected_object_validation/schema/object_position_filter.schema.json create mode 100644 perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json create mode 100644 perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json diff --git a/perception/autoware_detected_object_validation/README.md b/perception/autoware_detected_object_validation/README.md index 12fad4ad9759f..adca7c0fd9678 100644 --- a/perception/autoware_detected_object_validation/README.md +++ b/perception/autoware_detected_object_validation/README.md @@ -10,3 +10,21 @@ The purpose of this package is to eliminate obvious false positives of DetectedO - [Occupancy grid based validator](occupancy-grid-based-validator.md) - [Object lanelet filter](object-lanelet-filter.md) - [Object position filter](object-position-filter.md) + +### Node Parameters + +#### object_lanelet_filter + +{{ json_to_markdown("perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json") }} + +#### object_position_filter + +{{ json_to_markdown("perception/autoware_detected_object_validation/schema/object_position_filter.schema.json") }} + +#### obstacle_pointcloud_based_validator + +{{ json_to_markdown("perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json") }} + +#### occupancy_grid_based_validator + +{{ json_to_markdown("perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json") }} diff --git a/perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml b/perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml index fc5c634735e23..bb996f1197155 100644 --- a/perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml +++ b/perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: + mean_threshold: 0.6 enable_debug: false diff --git a/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json new file mode 100644 index 0000000000000..fd4e39f0ca02e --- /dev/null +++ b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json @@ -0,0 +1,121 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Object Lanelet Filter", + "type": "object", + "definitions": { + "object_lanelet_filter": { + "type": "object", + "properties": { + "filter_target_label": { + "type": "object", + "properties": { + "UNKNOWN": { + "type": "boolean", + "default": true, + "description": "If true, unknown objects are filtered." + }, + "CAR": { + "type": "boolean", + "default": false, + "description": "If true, car objects are filtered." + }, + "TRUCK": { + "type": "boolean", + "default": false, + "description": "If true, truck objects are filtered." + }, + "BUS": { + "type": "boolean", + "default": false, + "description": "If true, bus objects are filtered." + }, + "TRAILER": { + "type": "boolean", + "default": false, + "description": "If true, trailer objects are filtered." + }, + "MOTORCYCLE": { + "type": "boolean", + "default": false, + "description": "If true, motorcycle objects are filtered." + }, + "BICYCLE": { + "type": "boolean", + "default": false, + "description": "If true, bicycle objects are filtered." + }, + "PEDESTRIAN": { + "type": "boolean", + "default": false, + "description": "If true, pedestrian objects are filtered." + } + }, + "required": [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "MOTORCYCLE", + "BICYCLE", + "PEDESTRIAN" + ] + }, + "filter_settings": { + "type": "object", + "properties": { + "polygon_overlap_filter": { + "type": "object", + "properties": { + "enabled": { + "type": "boolean", + "default": true, + "description": "If true, objects that are not in the lanelet polygon are filtered." + } + }, + "required": ["enabled"] + }, + "lanelet_direction_filter": { + "type": "object", + "properties": { + "enabled": { + "type": "boolean", + "default": false, + "description": "If true, objects that are not in the same direction as the lanelet are filtered." + }, + "velocity_yaw_threshold": { + "type": "number", + "default": 0.785398, + "description": "If the yaw difference between the object and the lanelet is greater than this value, the object is filtered." + }, + "object_speed_threshold": { + "type": "number", + "default": 3.0, + "description": "If the object speed is greater than this value, the object is filtered." + } + }, + "required": ["enabled", "velocity_yaw_threshold", "object_speed_threshold"] + } + }, + "required": ["polygon_overlap_filter", "lanelet_direction_filter"] + } + }, + "required": ["filter_target_label", "filter_settings"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/object_lanelet_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/autoware_detected_object_validation/schema/object_position_filter.schema.json b/perception/autoware_detected_object_validation/schema/object_position_filter.schema.json new file mode 100644 index 0000000000000..9c35280d403ea --- /dev/null +++ b/perception/autoware_detected_object_validation/schema/object_position_filter.schema.json @@ -0,0 +1,106 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Object Position Filter", + "type": "object", + "definitions": { + "object_position_filter_params": { + "type": "object", + "properties": { + "filter_target_label": { + "type": "object", + "properties": { + "UNKNOWN": { + "type": "boolean", + "default": true, + "description": "Filter for UNKNOWN label" + }, + "CAR": { + "type": "boolean", + "default": false, + "description": "Filter for CAR label" + }, + "TRUCK": { + "type": "boolean", + "default": false, + "description": "Filter for TRUCK label" + }, + "BUS": { + "type": "boolean", + "default": false, + "description": "Filter for BUS label" + }, + "TRAILER": { + "type": "boolean", + "default": false, + "description": "Filter for TRAILER label" + }, + "MOTORCYCLE": { + "type": "boolean", + "default": false, + "description": "Filter for MOTORCYCLE label" + }, + "BICYCLE": { + "type": "boolean", + "default": false, + "description": "Filter for BICYCLE label" + }, + "PEDESTRIAN": { + "type": "boolean", + "default": false, + "description": "Filter for PEDESTRIAN label" + } + }, + "required": [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "MOTORCYCLE", + "BICYCLE", + "PEDESTRIAN" + ] + }, + "upper_bound_x": { + "type": "number", + "default": 100.0, + "description": "Upper bound for X coordinate" + }, + "lower_bound_x": { + "type": "number", + "default": 0.0, + "description": "Lower bound for X coordinate" + }, + "upper_bound_y": { + "type": "number", + "default": 10.0, + "description": "Upper bound for Y coordinate" + }, + "lower_bound_y": { + "type": "number", + "default": -10.0, + "description": "Lower bound for Y coordinate" + } + }, + "required": [ + "filter_target_label", + "upper_bound_x", + "lower_bound_x", + "upper_bound_y", + "lower_bound_y" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/object_position_filter_params" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json new file mode 100644 index 0000000000000..d7aa970993ca1 --- /dev/null +++ b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json @@ -0,0 +1,68 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Obstacle Pointcloud Based Validator", + "type": "object", + "definitions": { + "obstacle_pointcloud_based_validator": { + "type": "object", + "properties": { + "min_points_num": { + "type": "array", + "items": { + "type": "integer" + }, + "default": [10, 10, 10, 10, 10, 10, 10, 10], + "description": "The minimum number of obstacle point clouds in DetectedObjects" + }, + "max_points_num": { + "type": "array", + "items": { + "type": "integer" + }, + "default": [10, 10, 10, 10, 10, 10, 10, 10], + "description": "The max number of obstacle point clouds in DetectedObjects" + }, + "min_points_and_distance_ratio": { + "type": "array", + "items": { + "type": "number" + }, + "default": [800, 800, 800, 800, 800, 800, 800, 800], + "description": "Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink." + }, + "using_2d_validator": { + "type": "boolean", + "default": false, + "description": "The xy-plane projected (2D) obstacle point clouds will be used for validation" + }, + "enable_debugger": { + "type": "boolean", + "default": false, + "description": "Whether to create debug topics or not?" + } + }, + "required": [ + "min_points_num", + "max_points_num", + "min_points_and_distance_ratio", + "using_2d_validator", + "enable_debugger" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/obstacle_pointcloud_based_validator" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json b/perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json new file mode 100644 index 0000000000000..918e94cd9847e --- /dev/null +++ b/perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Occupancy Grid Based Validator", + "type": "object", + "definitions": { + "occupancy_grid_based_validator": { + "type": "object", + "properties": { + "mean_threshold": { + "type": "number", + "default": 0.6, + "description": "The percentage threshold of allowed non-freespace." + }, + "enable_debug": { + "type": "boolean", + "default": false, + "description": "Whether to display debug images or not?" + } + }, + "required": ["mean_threshold", "enable_debug"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/occupancy_grid_based_validator" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index aed0ba5ab85ea..59de872ae9b90 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -42,14 +42,14 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod using std::placeholders::_1; // Set parameters - filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN", false); - filter_target_.CAR = declare_parameter("filter_target_label.CAR", false); - filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK", false); - filter_target_.BUS = declare_parameter("filter_target_label.BUS", false); - filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER", false); - filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE", false); - filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE", false); - filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN", false); + filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN"); + filter_target_.CAR = declare_parameter("filter_target_label.CAR"); + filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK"); + filter_target_.BUS = declare_parameter("filter_target_label.BUS"); + filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER"); + filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE"); + filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE"); + filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN"); // Set filter settings filter_settings_.polygon_overlap_filter = declare_parameter("filter_settings.polygon_overlap_filter.enabled"); diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index c5ecce714735b..7f55c86080fd2 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -310,7 +310,7 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( debug_publisher_ = std::make_unique( this, "obstacle_pointcloud_based_validator"); - const bool enable_debugger = declare_parameter("enable_debugger", false); + const bool enable_debugger = declare_parameter("enable_debugger"); if (enable_debugger) debugger_ = std::make_shared(this); published_time_publisher_ = std::make_unique(this); diff --git a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index c082b4b0f03f1..51df1929a300c 100644 --- a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp +++ b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -53,8 +53,8 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - mean_threshold_ = declare_parameter("mean_threshold", 0.6); - enable_debug_ = declare_parameter("enable_debug", false); + mean_threshold_ = declare_parameter("mean_threshold"); + enable_debug_ = declare_parameter("enable_debug"); published_time_publisher_ = std::make_unique(this); } diff --git a/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp b/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp index ac0bab404c476..3b88628aa6d84 100644 --- a/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp +++ b/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp @@ -26,18 +26,18 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n using std::placeholders::_1; // Set parameters - upper_bound_x_ = declare_parameter("upper_bound_x", 100.0); - upper_bound_y_ = declare_parameter("upper_bound_y", 50.0); - lower_bound_x_ = declare_parameter("lower_bound_x", 0.0); - lower_bound_y_ = declare_parameter("lower_bound_y", -50.0); - filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN", false); - filter_target_.CAR = declare_parameter("filter_target_label.CAR", false); - filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK", false); - filter_target_.BUS = declare_parameter("filter_target_label.BUS", false); - filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER", false); - filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE", false); - filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE", false); - filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN", false); + upper_bound_x_ = declare_parameter("upper_bound_x"); + upper_bound_y_ = declare_parameter("upper_bound_y"); + lower_bound_x_ = declare_parameter("lower_bound_x"); + lower_bound_y_ = declare_parameter("lower_bound_y"); + filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN"); + filter_target_.CAR = declare_parameter("filter_target_label.CAR"); + filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK"); + filter_target_.BUS = declare_parameter("filter_target_label.BUS"); + filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER"); + filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE"); + filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE"); + filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN"); // Set publisher/subscriber object_sub_ = this->create_subscription(