Skip to content

Commit 30ab90e

Browse files
YoshiRiYoshiRi
authored
feat(autoware_detected_object_validation): add height filter in lanelet filtering (#10003)
* feat: add height filter param Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: use ego base height Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix: build error Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: add lanelet filter test Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: add height filter test Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * docs: update README and lanelet filter Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix: do not getTransform when flag is off Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
1 parent 8ee41be commit 30ab90e

File tree

8 files changed

+461
-5
lines changed

8 files changed

+461
-5
lines changed

‎perception/autoware_detected_object_validation/CMakeLists.txt

+4
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,10 @@ if(BUILD_TESTING)
8383
test/test_utils.cpp
8484
test/object_position_filter/test_object_position_filter.cpp
8585
)
86+
ament_auto_add_gtest(object_lanelet_filter_tests
87+
test/test_utils.cpp
88+
test/lanelet_filter/test_lanelet_filter.cpp
89+
)
8690
endif()
8791

8892
ament_auto_package(INSTALL_TO_SHARE

‎perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml

+3
Original file line numberDiff line numberDiff line change
@@ -21,3 +21,6 @@
2121
object_speed_threshold: 3.0 # [m/s]
2222
debug: false
2323
lanelet_extra_margin: 0.0
24+
use_height_threshold: false
25+
max_height_threshold: 10.0 # [m] from the base_link height in map frame
26+
min_height_threshold: -1.0 # [m] from the base_link height in map frame

‎perception/autoware_detected_object_validation/object-lanelet-filter.md

+13-4
Original file line numberDiff line numberDiff line change
@@ -24,10 +24,19 @@ The objects only inside of the vector map will be published.
2424

2525
## Parameters
2626

27-
| Name | Type | Description |
28-
| ---------------------- | -------- | ------------------------------------------------------------------------------------ |
29-
| `debug` | `bool` | if true, publish debug markers |
30-
| `lanelet_extra_margin` | `double` | `if > 0` lanelet polygons are expanded by extra margin, `if <= 0` margin is disabled |
27+
Description of the `filter_settings` in the parameters of the `object_lanelet_filter` node.
28+
29+
| Name | Type | Description |
30+
| ------------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------- |
31+
| `debug` | `bool` | If `true`, publishes additional debug information, including visualization markers on the `~/debug/marker` topic for tools like RViz. |
32+
| `lanelet_extra_margin` | `double` | If `> 0`, expands the lanelet polygons used for overlap checks by this margin (in meters). If `<= 0`, polygon expansion is disabled. |
33+
| `polygon_overlap_filter.enabled` | `bool` | If `true`, enables filtering of objects based on their overlap with lanelet polygons. |
34+
| `lanelet_direction_filter.enabled` | `bool` | If `true`, enables filtering of objects based on their velocity direction relative to the lanelet. |
35+
| `lanelet_direction_filter.velocity_yaw_threshold` | `double` | The yaw angle difference threshold (in radians) between the object’s velocity vector and the lanelet direction. |
36+
| `lanelet_direction_filter.object_speed_threshold` | `double` | The minimum speed (in m/s) of an object required for the direction filter to be applied. |
37+
| `use_height_threshold` | `bool` | If `true`, enables filtering of objects based on their height relative to the base_link frame. |
38+
| `max_height_threshold` | `double` | The maximum allowable height (in meters) of an object relative to the base_link in the map frame. |
39+
| `min_height_threshold` | `double` | The minimum allowable height (in meters) of an object relative to the base_link in the map frame. |
3140

3241
### Core Parameters
3342

‎perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json

+34-1
Original file line numberDiff line numberDiff line change
@@ -95,9 +95,42 @@
9595
}
9696
},
9797
"required": ["enabled", "velocity_yaw_threshold", "object_speed_threshold"]
98+
},
99+
"debug": {
100+
"type": "boolean",
101+
"default": false,
102+
"description": "If true, debug information is enabled."
103+
},
104+
"lanelet_extra_margin": {
105+
"type": "number",
106+
"default": 0.0,
107+
"description": "Extra margin added to the lanelet boundaries."
108+
},
109+
"use_height_threshold": {
110+
"type": "boolean",
111+
"default": false,
112+
"description": "If true, height thresholds are used to filter objects."
113+
},
114+
"max_height_threshold": {
115+
"type": "number",
116+
"default": 10.0,
117+
"description": "Maximum height threshold for filtering objects (in meters)."
118+
},
119+
"min_height_threshold": {
120+
"type": "number",
121+
"default": -1.0,
122+
"description": "Minimum height threshold for filtering objects (in meters)."
98123
}
99124
},
100-
"required": ["polygon_overlap_filter", "lanelet_direction_filter"]
125+
"required": [
126+
"polygon_overlap_filter",
127+
"lanelet_direction_filter",
128+
"debug",
129+
"lanelet_extra_margin",
130+
"use_height_threshold",
131+
"max_height_threshold",
132+
"min_height_threshold"
133+
]
101134
}
102135
},
103136
"required": ["filter_target_label", "filter_settings"],

‎perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp

+29
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,12 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
6666
filter_settings_.debug = declare_parameter<bool>("filter_settings.debug");
6767
filter_settings_.lanelet_extra_margin =
6868
declare_parameter<double>("filter_settings.lanelet_extra_margin");
69+
filter_settings_.use_height_threshold =
70+
declare_parameter<bool>("filter_settings.use_height_threshold");
71+
filter_settings_.max_height_threshold =
72+
declare_parameter<double>("filter_settings.max_height_threshold");
73+
filter_settings_.min_height_threshold =
74+
declare_parameter<double>("filter_settings.min_height_threshold");
6975

7076
// Set publisher/subscriber
7177
map_sub_ = this->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
@@ -146,6 +152,19 @@ void ObjectLaneletFilterNode::objectCallback(
146152
RCLCPP_ERROR(get_logger(), "Failed transform to %s.", lanelet_frame_id_.c_str());
147153
return;
148154
}
155+
// vehicle base pose :map -> base_link
156+
if (filter_settings_.use_height_threshold) {
157+
try {
158+
ego_base_height_ = tf_buffer_
159+
.lookupTransform(
160+
lanelet_frame_id_, "base_link", transformed_objects.header.stamp,
161+
rclcpp::Duration::from_seconds(0.5))
162+
.transform.translation.z;
163+
} catch (const tf2::TransformException & ex) {
164+
RCLCPP_ERROR_STREAM(get_logger(), "Failed to get transform: " << ex.what());
165+
return;
166+
}
167+
}
149168

150169
if (!transformed_objects.objects.empty()) {
151170
// calculate convex hull
@@ -199,6 +218,16 @@ bool ObjectLaneletFilterNode::filterObject(
199218
return false;
200219
}
201220

221+
// 0. check height threshold
222+
if (filter_settings_.use_height_threshold) {
223+
const double object_height = transformed_object.shape.dimensions.z;
224+
if (
225+
object_height > ego_base_height_ + filter_settings_.max_height_threshold ||
226+
object_height < ego_base_height_ + filter_settings_.min_height_threshold) {
227+
return false;
228+
}
229+
}
230+
202231
bool filter_pass = true;
203232
// 1. is polygon overlap with road lanelets or shoulder lanelets
204233
if (filter_settings_.polygon_overlap_filter) {

‎perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp

+5
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@
3535
#include <tf2_ros/buffer.h>
3636
#include <tf2_ros/transform_listener.h>
3737

38+
#include <limits>
3839
#include <memory>
3940
#include <string>
4041
#include <utility>
@@ -88,6 +89,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
8889
tf2_ros::TransformListener tf_listener_;
8990

9091
utils::FilterTargetLabel filter_target_;
92+
double ego_base_height_ = 0.0;
9193
struct FilterSettings
9294
{
9395
bool polygon_overlap_filter;
@@ -96,6 +98,9 @@ class ObjectLaneletFilterNode : public rclcpp::Node
9698
double lanelet_direction_filter_object_speed_threshold;
9799
bool debug;
98100
double lanelet_extra_margin;
101+
bool use_height_threshold;
102+
double max_height_threshold = std::numeric_limits<double>::infinity();
103+
double min_height_threshold = -std::numeric_limits<double>::infinity();
99104
} filter_settings_;
100105

101106
bool filterObject(

0 commit comments

Comments
 (0)