@@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
43
43
filter_target_.MOTORCYCLE = declare_parameter<bool >(" filter_target_label.MOTORCYCLE" , false );
44
44
filter_target_.BICYCLE = declare_parameter<bool >(" filter_target_label.BICYCLE" , false );
45
45
filter_target_.PEDESTRIAN = declare_parameter<bool >(" filter_target_label.PEDESTRIAN" , false );
46
+ // Set filter settings
47
+ filter_settings_.polygon_overlap_filter =
48
+ declare_parameter<bool >(" filter_settings.polygon_overlap_filter.enabled" );
49
+ filter_settings_.lanelet_direction_filter =
50
+ declare_parameter<bool >(" filter_settings.lanelet_direction_filter.enabled" );
51
+ filter_settings_.lanelet_direction_filter_velocity_yaw_threshold =
52
+ declare_parameter<double >(" filter_settings.lanelet_direction_filter.velocity_yaw_threshold" );
53
+ filter_settings_.lanelet_direction_filter_object_speed_threshold =
54
+ declare_parameter<double >(" filter_settings.lanelet_direction_filter.object_speed_threshold" );
46
55
47
56
// Set publisher/subscriber
48
57
map_sub_ = this ->create_subscription <autoware_auto_mapping_msgs::msg::HADMapBin>(
@@ -97,27 +106,13 @@ void ObjectLaneletFilterNode::objectCallback(
97
106
lanelet::ConstLanelets intersected_shoulder_lanelets =
98
107
getIntersectedLanelets (convex_hull, shoulder_lanelets_);
99
108
100
- int index = 0 ;
101
- for (const auto & object : transformed_objects.objects ) {
102
- const auto footprint = setFootprint (object);
103
- const auto & label = object.classification .front ().label ;
104
- if (filter_target_.isTarget (label)) {
105
- Polygon2d polygon;
106
- for (const auto & point : footprint.points ) {
107
- const geometry_msgs::msg::Point32 point_transformed =
108
- tier4_autoware_utils::transformPoint (point, object.kinematics .pose_with_covariance .pose );
109
- polygon.outer ().emplace_back (point_transformed.x , point_transformed.y );
110
- }
111
- polygon.outer ().push_back (polygon.outer ().front ());
112
- if (isPolygonOverlapLanelets (polygon, intersected_road_lanelets)) {
113
- output_object_msg.objects .emplace_back (input_msg->objects .at (index ));
114
- } else if (isPolygonOverlapLanelets (polygon, intersected_shoulder_lanelets)) {
115
- output_object_msg.objects .emplace_back (input_msg->objects .at (index ));
116
- }
117
- } else {
118
- output_object_msg.objects .emplace_back (input_msg->objects .at (index ));
119
- }
120
- ++index ;
109
+ // filtering process
110
+ for (size_t index = 0 ; index < transformed_objects.objects .size (); ++index ) {
111
+ const auto & transformed_object = transformed_objects.objects .at (index );
112
+ const auto & input_object = input_msg->objects .at (index );
113
+ filterObject (
114
+ transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets,
115
+ output_object_msg);
121
116
}
122
117
object_pub_->publish (output_object_msg);
123
118
published_time_publisher_->publish_if_subscribed (object_pub_, output_object_msg.header .stamp );
@@ -132,6 +127,55 @@ void ObjectLaneletFilterNode::objectCallback(
132
127
" debug/pipeline_latency_ms" , pipeline_latency);
133
128
}
134
129
130
+ bool ObjectLaneletFilterNode::filterObject (
131
+ const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
132
+ const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
133
+ const lanelet::ConstLanelets & intersected_road_lanelets,
134
+ const lanelet::ConstLanelets & intersected_shoulder_lanelets,
135
+ autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg)
136
+ {
137
+ const auto & label = transformed_object.classification .front ().label ;
138
+ if (filter_target_.isTarget (label)) {
139
+ bool filter_pass = true ;
140
+ // 1. is polygon overlap with road lanelets or shoulder lanelets
141
+ if (filter_settings_.polygon_overlap_filter ) {
142
+ Polygon2d polygon;
143
+ const auto footprint = setFootprint (transformed_object);
144
+ for (const auto & point : footprint.points ) {
145
+ const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint (
146
+ point, transformed_object.kinematics .pose_with_covariance .pose );
147
+ polygon.outer ().emplace_back (point_transformed.x , point_transformed.y );
148
+ }
149
+ polygon.outer ().push_back (polygon.outer ().front ());
150
+ const bool is_polygon_overlap =
151
+ isPolygonOverlapLanelets (polygon, intersected_road_lanelets) ||
152
+ isPolygonOverlapLanelets (polygon, intersected_shoulder_lanelets);
153
+ filter_pass = filter_pass && is_polygon_overlap;
154
+ }
155
+
156
+ // 2. check if objects velocity is the same with the lanelet direction
157
+ const bool orientation_not_available =
158
+ transformed_object.kinematics .orientation_availability ==
159
+ autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE;
160
+ if (filter_settings_.lanelet_direction_filter && !orientation_not_available) {
161
+ const bool is_same_direction =
162
+ isSameDirectionWithLanelets (intersected_road_lanelets, transformed_object) ||
163
+ isSameDirectionWithLanelets (intersected_shoulder_lanelets, transformed_object);
164
+ filter_pass = filter_pass && is_same_direction;
165
+ }
166
+
167
+ // push back to output object
168
+ if (filter_pass) {
169
+ output_object_msg.objects .emplace_back (input_object);
170
+ return true ;
171
+ }
172
+ } else {
173
+ output_object_msg.objects .emplace_back (input_object);
174
+ return true ;
175
+ }
176
+ return false ;
177
+ }
178
+
135
179
geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint (
136
180
const autoware_auto_perception_msgs::msg::DetectedObject & detected_object)
137
181
{
@@ -201,6 +245,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets(
201
245
return false ;
202
246
}
203
247
248
+ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets (
249
+ const lanelet::ConstLanelets & lanelets,
250
+ const autoware_auto_perception_msgs::msg::DetectedObject & object)
251
+ {
252
+ const double object_yaw = tf2::getYaw (object.kinematics .pose_with_covariance .pose .orientation );
253
+ const double object_velocity_norm = std::hypot (
254
+ object.kinematics .twist_with_covariance .twist .linear .x ,
255
+ object.kinematics .twist_with_covariance .twist .linear .y );
256
+ const double object_velocity_yaw = std::atan2 (
257
+ object.kinematics .twist_with_covariance .twist .linear .y ,
258
+ object.kinematics .twist_with_covariance .twist .linear .x ) +
259
+ object_yaw;
260
+
261
+ if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold ) {
262
+ return true ;
263
+ }
264
+ for (const auto & lanelet : lanelets) {
265
+ const bool is_in_lanelet =
266
+ lanelet::utils::isInLanelet (object.kinematics .pose_with_covariance .pose , lanelet, 0.0 );
267
+ if (!is_in_lanelet) {
268
+ continue ;
269
+ }
270
+ const double lane_yaw = lanelet::utils::getLaneletAngle (
271
+ lanelet, object.kinematics .pose_with_covariance .pose .position );
272
+ const double delta_yaw = object_velocity_yaw - lane_yaw;
273
+ const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian (delta_yaw);
274
+ const double abs_norm_delta_yaw = std::fabs (normalized_delta_yaw);
275
+
276
+ if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold ) {
277
+ return true ;
278
+ }
279
+ }
280
+
281
+ return false ;
282
+ }
283
+
204
284
} // namespace object_lanelet_filter
205
285
206
286
#include < rclcpp_components/register_node_macro.hpp>
0 commit comments