@@ -41,6 +41,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
41
41
filter_target_.MOTORCYCLE = declare_parameter<bool >(" filter_target_label.MOTORCYCLE" , false );
42
42
filter_target_.BICYCLE = declare_parameter<bool >(" filter_target_label.BICYCLE" , false );
43
43
filter_target_.PEDESTRIAN = declare_parameter<bool >(" filter_target_label.PEDESTRIAN" , false );
44
+ // Set filter settings
45
+ filter_settings_.polygon_overlap_filter =
46
+ declare_parameter<bool >(" filter_settings.polygon_overlap_filter.enabled" );
47
+ filter_settings_.lanelet_direction_filter =
48
+ declare_parameter<bool >(" filter_settings.lanelet_direction_filter.enabled" );
49
+ filter_settings_.lanelet_direction_filter_velocity_yaw_threshold =
50
+ declare_parameter<double >(" filter_settings.lanelet_direction_filter.velocity_yaw_threshold" );
51
+ filter_settings_.lanelet_direction_filter_object_speed_threshold =
52
+ declare_parameter<double >(" filter_settings.lanelet_direction_filter.object_speed_threshold" );
44
53
45
54
// Set publisher/subscriber
46
55
map_sub_ = this ->create_subscription <autoware_auto_mapping_msgs::msg::HADMapBin>(
@@ -87,27 +96,64 @@ void ObjectLaneletFilterNode::objectCallback(
87
96
// get intersected lanelets
88
97
lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets (convex_hull, road_lanelets_);
89
98
90
- int index = 0 ;
91
- for (const auto & object : transformed_objects.objects ) {
92
- const auto footprint = setFootprint (object);
93
- const auto & label = object.classification .front ().label ;
94
- if (filter_target_.isTarget (label)) {
99
+ // filtering process
100
+ for (size_t index = 0 ; index < transformed_objects.objects .size (); ++index ) {
101
+ const auto & transformed_object = transformed_objects.objects .at (index );
102
+ const auto & input_object = input_msg->objects .at (index );
103
+ filterObject (
104
+ transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets,
105
+ output_object_msg);
106
+ }
107
+ object_pub_->publish (output_object_msg);
108
+ }
109
+
110
+ bool ObjectLaneletFilterNode::filterObject (
111
+ const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
112
+ const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
113
+ const lanelet::ConstLanelets & intersected_road_lanelets,
114
+ const lanelet::ConstLanelets & intersected_shoulder_lanelets,
115
+ autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg)
116
+ {
117
+ const auto & label = transformed_object.classification .front ().label ;
118
+ if (filter_target_.isTarget (label)) {
119
+ bool filter_pass = true ;
120
+ // 1. is polygon overlap with road lanelets or shoulder lanelets
121
+ if (filter_settings_.polygon_overlap_filter ) {
95
122
Polygon2d polygon;
123
+ const auto footprint = setFootprint (transformed_object);
96
124
for (const auto & point : footprint.points ) {
97
- const geometry_msgs::msg::Point32 point_transformed =
98
- tier4_autoware_utils::transformPoint ( point, object .kinematics .pose_with_covariance .pose );
125
+ const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint (
126
+ point, transformed_object .kinematics .pose_with_covariance .pose );
99
127
polygon.outer ().emplace_back (point_transformed.x , point_transformed.y );
100
128
}
101
129
polygon.outer ().push_back (polygon.outer ().front ());
102
- if (isPolygonOverlapLanelets (polygon, intersected_lanelets)) {
103
- output_object_msg.objects .emplace_back (input_msg->objects .at (index ));
104
- }
105
- } else {
106
- output_object_msg.objects .emplace_back (input_msg->objects .at (index ));
130
+ const bool is_polygon_overlap =
131
+ isPolygonOverlapLanelets (polygon, intersected_road_lanelets) ||
132
+ isPolygonOverlapLanelets (polygon, intersected_shoulder_lanelets);
133
+ filter_pass = filter_pass && is_polygon_overlap;
107
134
}
108
- ++index ;
135
+
136
+ // 2. check if objects velocity is the same with the lanelet direction
137
+ const bool orientation_not_available =
138
+ transformed_object.kinematics .orientation_availability ==
139
+ autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE;
140
+ if (filter_settings_.lanelet_direction_filter && !orientation_not_available) {
141
+ const bool is_same_direction =
142
+ isSameDirectionWithLanelets (intersected_road_lanelets, transformed_object) ||
143
+ isSameDirectionWithLanelets (intersected_shoulder_lanelets, transformed_object);
144
+ filter_pass = filter_pass && is_same_direction;
145
+ }
146
+
147
+ // push back to output object
148
+ if (filter_pass) {
149
+ output_object_msg.objects .emplace_back (input_object);
150
+ return true ;
151
+ }
152
+ } else {
153
+ output_object_msg.objects .emplace_back (input_object);
154
+ return true ;
109
155
}
110
- object_pub_-> publish (output_object_msg) ;
156
+ return false ;
111
157
}
112
158
113
159
geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint (
@@ -176,6 +222,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets(
176
222
return false ;
177
223
}
178
224
225
+ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets (
226
+ const lanelet::ConstLanelets & lanelets,
227
+ const autoware_auto_perception_msgs::msg::DetectedObject & object)
228
+ {
229
+ const double object_yaw = tf2::getYaw (object.kinematics .pose_with_covariance .pose .orientation );
230
+ const double object_velocity_norm = std::hypot (
231
+ object.kinematics .twist_with_covariance .twist .linear .x ,
232
+ object.kinematics .twist_with_covariance .twist .linear .y );
233
+ const double object_velocity_yaw = std::atan2 (
234
+ object.kinematics .twist_with_covariance .twist .linear .y ,
235
+ object.kinematics .twist_with_covariance .twist .linear .x ) +
236
+ object_yaw;
237
+
238
+ if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold ) {
239
+ return true ;
240
+ }
241
+ for (const auto & lanelet : lanelets) {
242
+ const bool is_in_lanelet =
243
+ lanelet::utils::isInLanelet (object.kinematics .pose_with_covariance .pose , lanelet, 0.0 );
244
+ if (!is_in_lanelet) {
245
+ continue ;
246
+ }
247
+ const double lane_yaw = lanelet::utils::getLaneletAngle (
248
+ lanelet, object.kinematics .pose_with_covariance .pose .position );
249
+ const double delta_yaw = object_velocity_yaw - lane_yaw;
250
+ const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian (delta_yaw);
251
+ const double abs_norm_delta_yaw = std::fabs (normalized_delta_yaw);
252
+
253
+ if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold ) {
254
+ return true ;
255
+ }
256
+ }
257
+
258
+ return false ;
259
+ }
260
+
179
261
} // namespace object_lanelet_filter
180
262
181
263
#include < rclcpp_components/register_node_macro.hpp>
0 commit comments