Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(lanelet_filte): add crosswalk for lanelet_filter #1622

Open
wants to merge 1 commit into
base: beta/v0.19.1-sp-develop
Choose a base branch
from
+10 −2
Open

fix(lanelet_filte): add crosswalk for lanelet_filter #1622

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions ...ation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
lanelet::LaneletMapPtr lanelet_map_ptr_;
lanelet::ConstLanelets road_lanelets_;
lanelet::ConstLanelets shoulder_lanelets_;
lanelet::ConstLanelets crosswalk_lanelets_;
std::string lanelet_frame_id_;

tf2_ros::Buffer tf_buffer_;
Expand All @@ -75,6 +76,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
const lanelet::ConstLanelets & intersected_road_lanelets,
const lanelet::ConstLanelets & intersected_shoulder_lanelets,
const lanelet::ConstLanelets & intersected_crosswalk_lanelets,
autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg);
LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
lanelet::ConstLanelets getIntersectedLanelets(
Expand Down
10 changes: 8 additions & 2 deletions perception/detected_object_validation/src/object_lanelet_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ void ObjectLaneletFilterNode::mapCallback(
const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_);
road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets);
shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
crosswalk_lanelets_ = lanelet::utils::query::crosswalkLanelets(all_lanelets);
}

void ObjectLaneletFilterNode::objectCallback(
Expand Down Expand Up @@ -104,14 +105,17 @@ void ObjectLaneletFilterNode::objectCallback(
getIntersectedLanelets(convex_hull, road_lanelets_);
lanelet::ConstLanelets intersected_shoulder_lanelets =
getIntersectedLanelets(convex_hull, shoulder_lanelets_);
lanelet::ConstLanelets intersected_crosswalk_lanelets =
getIntersectedLanelets(convex_hull, crosswalk_lanelets_);


// 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);
intersected_crosswalk_lanelets, output_object_msg);
}
object_pub_->publish(output_object_msg);

Expand All @@ -130,6 +134,7 @@ bool ObjectLaneletFilterNode::filterObject(
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
const lanelet::ConstLanelets & intersected_road_lanelets,
const lanelet::ConstLanelets & intersected_shoulder_lanelets,
const lanelet::ConstLanelets & intersected_crosswalk_lanelets,
autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg)
{
const auto & label = transformed_object.classification.front().label;
Expand All @@ -139,7 +144,8 @@ bool ObjectLaneletFilterNode::filterObject(
if (filter_settings_.polygon_overlap_filter) {
const bool is_polygon_overlap =
isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) ||
isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets);
isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets) ||
isObjectOverlapLanelets(transformed_object, intersected_crosswalk_lanelets);
filter_pass = filter_pass && is_polygon_overlap;
}

Expand Down
Loading