Skip to content

Commit af805ef

Browse files
YoshiRikarishma1911
authored andcommitted
feat(object_lanelet_filter): add velocity direction based object lanelet filter (autowarefoundation#7107)
* chore: refactor lanelet filter function Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: add lanelet direction filter Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix: do not change default settings Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: skip orientation unavailable objects Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix: feature variable name Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
1 parent 0039105 commit af805ef

File tree

3 files changed

+129
-22
lines changed

3 files changed

+129
-22
lines changed

perception/detected_object_validation/config/object_lanelet_filter.param.yaml

+10
Original file line numberDiff line numberDiff line change
@@ -9,3 +9,13 @@
99
MOTORCYCLE : false
1010
BICYCLE : false
1111
PEDESTRIAN : false
12+
13+
filter_settings:
14+
# polygon overlap based filter
15+
polygon_overlap_filter:
16+
enabled: true
17+
# velocity direction based filter
18+
lanelet_direction_filter:
19+
enabled: false
20+
velocity_yaw_threshold: 0.785398 # [rad] (45 deg)
21+
object_speed_threshold: 3.0 # [m/s]

perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp

+18-1
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include "detected_object_validation/utils/utils.hpp"
1919

20+
#include <lanelet2_extension/utility/utilities.hpp>
2021
#include <rclcpp/rclcpp.hpp>
2122
#include <tier4_autoware_utils/geometry/geometry.hpp>
2223
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
@@ -62,11 +63,27 @@ class ObjectLaneletFilterNode : public rclcpp::Node
6263
tf2_ros::TransformListener tf_listener_;
6364

6465
utils::FilterTargetLabel filter_target_;
65-
66+
struct FilterSettings
67+
{
68+
bool polygon_overlap_filter;
69+
bool lanelet_direction_filter;
70+
double lanelet_direction_filter_velocity_yaw_threshold;
71+
double lanelet_direction_filter_object_speed_threshold;
72+
} filter_settings_;
73+
74+
bool filterObject(
75+
const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
76+
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
77+
const lanelet::ConstLanelets & intersected_road_lanelets,
78+
const lanelet::ConstLanelets & intersected_shoulder_lanelets,
79+
autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg);
6680
LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
6781
lanelet::ConstLanelets getIntersectedLanelets(
6882
const LinearRing2d &, const lanelet::ConstLanelets &);
6983
bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
84+
bool isSameDirectionWithLanelets(
85+
const lanelet::ConstLanelets & lanelets,
86+
const autoware_auto_perception_msgs::msg::DetectedObject & object);
7087
geometry_msgs::msg::Polygon setFootprint(
7188
const autoware_auto_perception_msgs::msg::DetectedObject &);
7289

perception/detected_object_validation/src/object_lanelet_filter.cpp

+101-21
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
4343
filter_target_.MOTORCYCLE = declare_parameter<bool>("filter_target_label.MOTORCYCLE", false);
4444
filter_target_.BICYCLE = declare_parameter<bool>("filter_target_label.BICYCLE", false);
4545
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");
4655

4756
// Set publisher/subscriber
4857
map_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
@@ -97,27 +106,13 @@ void ObjectLaneletFilterNode::objectCallback(
97106
lanelet::ConstLanelets intersected_shoulder_lanelets =
98107
getIntersectedLanelets(convex_hull, shoulder_lanelets_);
99108

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);
121116
}
122117
object_pub_->publish(output_object_msg);
123118
published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
@@ -132,6 +127,55 @@ void ObjectLaneletFilterNode::objectCallback(
132127
"debug/pipeline_latency_ms", pipeline_latency);
133128
}
134129

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+
135179
geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint(
136180
const autoware_auto_perception_msgs::msg::DetectedObject & detected_object)
137181
{
@@ -201,6 +245,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets(
201245
return false;
202246
}
203247

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+
204284
} // namespace object_lanelet_filter
205285

206286
#include <rclcpp_components/register_node_macro.hpp>

0 commit comments

Comments
 (0)