Skip to content

Commit

Permalink
feat(object_lanelet_filter): add velocity direction based object lane…
Browse files Browse the repository at this point in the history 
…let filter (#7107)

* chore: refactor lanelet filter function

Signed-off-by: yoshiri <[email protected]>

* feat: add lanelet direction filter

Signed-off-by: yoshiri <[email protected]>

* fix: do not change default settings

Signed-off-by: yoshiri <[email protected]>

* feat: skip orientation unavailable objects

Signed-off-by: yoshiri <[email protected]>

* fix: feature variable name

Signed-off-by: yoshiri <[email protected]>

---------

Signed-off-by: yoshiri <[email protected]>
  • Loading branch information
@YoshiRi
YoshiRi authored May 30, 2024
1 parent b93f173 commit 1b5a6b2
Show file tree
Hide file tree
Showing 3 changed files with 129 additions and 22 deletions.
10 changes: 10 additions & 0 deletions perception/detected_object_validation/config/object_lanelet_filter.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,3 +9,13 @@
MOTORCYCLE : false
BICYCLE : false
PEDESTRIAN : false

filter_settings:
# polygon overlap based filter
polygon_overlap_filter:
enabled: true
# velocity direction based filter
lanelet_direction_filter:
enabled: false
velocity_yaw_threshold: 0.785398 # [rad] (45 deg)
object_speed_threshold: 3.0 # [m/s]
19 changes: 18 additions & 1 deletion ...ation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include "detected_object_validation/utils/utils.hpp"

#include <lanelet2_extension/utility/utilities.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
Expand Down Expand Up @@ -62,11 +63,27 @@ class ObjectLaneletFilterNode : public rclcpp::Node
tf2_ros::TransformListener tf_listener_;

utils::FilterTargetLabel filter_target_;

struct FilterSettings
{
bool polygon_overlap_filter;
bool lanelet_direction_filter;
double lanelet_direction_filter_velocity_yaw_threshold;
double lanelet_direction_filter_object_speed_threshold;
} filter_settings_;

bool filterObject(
const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
const lanelet::ConstLanelets & intersected_road_lanelets,
const lanelet::ConstLanelets & intersected_shoulder_lanelets,
autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg);
LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
lanelet::ConstLanelets getIntersectedLanelets(
const LinearRing2d &, const lanelet::ConstLanelets &);
bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
bool isSameDirectionWithLanelets(
const lanelet::ConstLanelets & lanelets,
const autoware_auto_perception_msgs::msg::DetectedObject & object);
geometry_msgs::msg::Polygon setFootprint(
const autoware_auto_perception_msgs::msg::DetectedObject &);

Expand Down
122 changes: 101 additions & 21 deletions perception/detected_object_validation/src/object_lanelet_filter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
filter_target_.MOTORCYCLE = declare_parameter<bool>("filter_target_label.MOTORCYCLE", false);
filter_target_.BICYCLE = declare_parameter<bool>("filter_target_label.BICYCLE", false);
filter_target_.PEDESTRIAN = declare_parameter<bool>("filter_target_label.PEDESTRIAN", false);
// Set filter settings
filter_settings_.polygon_overlap_filter =
declare_parameter<bool>("filter_settings.polygon_overlap_filter.enabled");
filter_settings_.lanelet_direction_filter =
declare_parameter<bool>("filter_settings.lanelet_direction_filter.enabled");
filter_settings_.lanelet_direction_filter_velocity_yaw_threshold =
declare_parameter<double>("filter_settings.lanelet_direction_filter.velocity_yaw_threshold");
filter_settings_.lanelet_direction_filter_object_speed_threshold =
declare_parameter<double>("filter_settings.lanelet_direction_filter.object_speed_threshold");

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

int index = 0;
for (const auto & object : transformed_objects.objects) {
const auto footprint = setFootprint(object);
const auto & label = object.classification.front().label;
if (filter_target_.isTarget(label)) {
Polygon2d polygon;
for (const auto & point : footprint.points) {
const geometry_msgs::msg::Point32 point_transformed =
tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose);
polygon.outer().emplace_back(point_transformed.x, point_transformed.y);
}
polygon.outer().push_back(polygon.outer().front());
if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) {
output_object_msg.objects.emplace_back(input_msg->objects.at(index));
} else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) {
output_object_msg.objects.emplace_back(input_msg->objects.at(index));
}
} else {
output_object_msg.objects.emplace_back(input_msg->objects.at(index));
}
++index;
// filtering process
for (size_t index = 0; index < transformed_objects.objects.size(); ++index) {
const auto & transformed_object = transformed_objects.objects.at(index);
const auto & input_object = input_msg->objects.at(index);
filterObject(
transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets,
output_object_msg);
}
object_pub_->publish(output_object_msg);
published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
Expand All @@ -132,6 +127,55 @@ void ObjectLaneletFilterNode::objectCallback(
"debug/pipeline_latency_ms", pipeline_latency);
}

bool ObjectLaneletFilterNode::filterObject(
const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
const lanelet::ConstLanelets & intersected_road_lanelets,
const lanelet::ConstLanelets & intersected_shoulder_lanelets,
autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg)
{
const auto & label = transformed_object.classification.front().label;
if (filter_target_.isTarget(label)) {
bool filter_pass = true;
// 1. is polygon overlap with road lanelets or shoulder lanelets
if (filter_settings_.polygon_overlap_filter) {
Polygon2d polygon;
const auto footprint = setFootprint(transformed_object);
for (const auto & point : footprint.points) {
const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint(
point, transformed_object.kinematics.pose_with_covariance.pose);
polygon.outer().emplace_back(point_transformed.x, point_transformed.y);
}
polygon.outer().push_back(polygon.outer().front());
const bool is_polygon_overlap =
isPolygonOverlapLanelets(polygon, intersected_road_lanelets) ||
isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets);
filter_pass = filter_pass && is_polygon_overlap;
}

// 2. check if objects velocity is the same with the lanelet direction
const bool orientation_not_available =
transformed_object.kinematics.orientation_availability ==
autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE;
if (filter_settings_.lanelet_direction_filter && !orientation_not_available) {
const bool is_same_direction =
isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) ||
isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object);
filter_pass = filter_pass && is_same_direction;
}

// push back to output object
if (filter_pass) {
output_object_msg.objects.emplace_back(input_object);
return true;
}
} else {
output_object_msg.objects.emplace_back(input_object);
return true;
}
return false;
}

geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint(
const autoware_auto_perception_msgs::msg::DetectedObject & detected_object)
{
Expand Down Expand Up @@ -201,6 +245,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets(
return false;
}

bool ObjectLaneletFilterNode::isSameDirectionWithLanelets(
const lanelet::ConstLanelets & lanelets,
const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
const double object_velocity_norm = std::hypot(
object.kinematics.twist_with_covariance.twist.linear.x,
object.kinematics.twist_with_covariance.twist.linear.y);
const double object_velocity_yaw = std::atan2(
object.kinematics.twist_with_covariance.twist.linear.y,
object.kinematics.twist_with_covariance.twist.linear.x) +
object_yaw;

if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) {
return true;
}
for (const auto & lanelet : lanelets) {
const bool is_in_lanelet =
lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0);
if (!is_in_lanelet) {
continue;
}
const double lane_yaw = lanelet::utils::getLaneletAngle(
lanelet, object.kinematics.pose_with_covariance.pose.position);
const double delta_yaw = object_velocity_yaw - lane_yaw;
const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw);
const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw);

if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) {
return true;
}
}

return false;
}

} // namespace object_lanelet_filter

#include <rclcpp_components/register_node_macro.hpp>
Expand Down

0 comments on commit 1b5a6b2

Please sign in to comment.