diff --git a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml index d15b2c81cf6db..26320d0a9cdf9 100644 --- a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml @@ -21,3 +21,6 @@ object_speed_threshold: 3.0 # [m/s] debug: false lanelet_extra_margin: 0.0 + use_height_threshold: false + max_height_threshold: 10.0 # [m] + min_height_threshold: -1.0 # [m] 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 index fd4e39f0ca02e..8e1b641eebbf8 100644 --- a/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json +++ b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json @@ -95,9 +95,42 @@ } }, "required": ["enabled", "velocity_yaw_threshold", "object_speed_threshold"] + }, + "debug": { + "type": "boolean", + "default": false, + "description": "If true, debug information is enabled." + }, + "lanelet_extra_margin": { + "type": "number", + "default": 0.0, + "description": "Extra margin added to the lanelet boundaries." + }, + "use_height_threshold": { + "type": "boolean", + "default": false, + "description": "If true, height thresholds are used to filter objects." + }, + "max_height_threshold": { + "type": "number", + "default": 10.0, + "description": "Maximum height threshold for filtering objects (in meters)." + }, + "min_height_threshold": { + "type": "number", + "default": -1.0, + "description": "Minimum height threshold for filtering objects (in meters)." } }, - "required": ["polygon_overlap_filter", "lanelet_direction_filter"] + "required": [ + "polygon_overlap_filter", + "lanelet_direction_filter", + "debug", + "lanelet_extra_margin", + "use_height_threshold", + "max_height_threshold", + "min_height_threshold" + ] } }, "required": ["filter_target_label", "filter_settings"], 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 f91c4e2036227..ede420d073011 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 @@ -66,6 +66,12 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod filter_settings_.debug = declare_parameter("filter_settings.debug"); filter_settings_.lanelet_extra_margin = declare_parameter("filter_settings.lanelet_extra_margin"); + filter_settings_.use_height_threshold = + declare_parameter("filter_settings.use_height_threshold"); + filter_settings_.max_height_threshold = + declare_parameter("filter_settings.max_height_threshold"); + filter_settings_.min_height_threshold = + declare_parameter("filter_settings.min_height_threshold"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -199,6 +205,16 @@ bool ObjectLaneletFilterNode::filterObject( return false; } + // 0. check height threshold + if (filter_settings_.use_height_threshold) { + const double object_height = transformed_object.shape.dimensions.z; + if ( + object_height > filter_settings_.max_height_threshold || + object_height < filter_settings_.min_height_threshold) { + return false; + } + } + bool filter_pass = true; // 1. is polygon overlap with road lanelets or shoulder lanelets if (filter_settings_.polygon_overlap_filter) { diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 724e60df27e49..63d684b35ba06 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -96,6 +97,9 @@ class ObjectLaneletFilterNode : public rclcpp::Node double lanelet_direction_filter_object_speed_threshold; bool debug; double lanelet_extra_margin; + bool use_height_threshold; + double max_height_threshold = std::numeric_limits::infinity(); + double min_height_threshold = -std::numeric_limits::infinity(); } filter_settings_; bool filterObject(