|
| 1 | +// Copyright 2023-2024 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "filter_predicted_objects.hpp" |
| 16 | + |
| 17 | +#include <motion_utils/trajectory/trajectory.hpp> |
| 18 | +#include <traffic_light_utils/traffic_light_utils.hpp> |
| 19 | + |
| 20 | +#include <boost/geometry/algorithms/intersects.hpp> |
| 21 | + |
| 22 | +#include <lanelet2_core/geometry/LaneletMap.h> |
| 23 | +#include <lanelet2_core/primitives/BasicRegulatoryElements.h> |
| 24 | + |
| 25 | +#include <algorithm> |
| 26 | + |
| 27 | +namespace behavior_velocity_planner::out_of_lane |
| 28 | +{ |
| 29 | +void cut_predicted_path_beyond_line( |
| 30 | + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, |
| 31 | + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) |
| 32 | +{ |
| 33 | + auto stop_line_idx = 0UL; |
| 34 | + bool found = false; |
| 35 | + lanelet::BasicSegment2d path_segment; |
| 36 | + path_segment.first.x() = predicted_path.path.front().position.x; |
| 37 | + path_segment.first.y() = predicted_path.path.front().position.y; |
| 38 | + for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) { |
| 39 | + path_segment.second.x() = predicted_path.path[stop_line_idx].position.x; |
| 40 | + path_segment.second.y() = predicted_path.path[stop_line_idx].position.y; |
| 41 | + if (boost::geometry::intersects(stop_line, path_segment)) { |
| 42 | + found = true; |
| 43 | + break; |
| 44 | + } |
| 45 | + path_segment.first = path_segment.second; |
| 46 | + } |
| 47 | + if (found) { |
| 48 | + auto cut_idx = stop_line_idx; |
| 49 | + double arc_length = 0; |
| 50 | + while (cut_idx > 0 && arc_length < object_front_overhang) { |
| 51 | + arc_length += tier4_autoware_utils::calcDistance2d( |
| 52 | + predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); |
| 53 | + --cut_idx; |
| 54 | + } |
| 55 | + predicted_path.path.resize(cut_idx); |
| 56 | + } |
| 57 | +} |
| 58 | + |
| 59 | +std::optional<const lanelet::BasicLineString2d> find_next_stop_line( |
| 60 | + const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) |
| 61 | +{ |
| 62 | + lanelet::ConstLanelets lanelets; |
| 63 | + lanelet::BasicLineString2d query_line; |
| 64 | + for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y); |
| 65 | + const auto query_results = lanelet::geometry::findWithin2d( |
| 66 | + planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_line); |
| 67 | + for (const auto & r : query_results) lanelets.push_back(r.second); |
| 68 | + for (const auto & ll : lanelets) { |
| 69 | + for (const auto & element : ll.regulatoryElementsAs<lanelet::TrafficLight>()) { |
| 70 | + const auto traffic_signal_stamped = planner_data.getTrafficSignal(element->id()); |
| 71 | + if ( |
| 72 | + traffic_signal_stamped.has_value() && element->stopLine().has_value() && |
| 73 | + traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) { |
| 74 | + lanelet::BasicLineString2d stop_line; |
| 75 | + for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y()); |
| 76 | + return stop_line; |
| 77 | + } |
| 78 | + } |
| 79 | + } |
| 80 | + return std::nullopt; |
| 81 | +} |
| 82 | + |
| 83 | +void cut_predicted_path_beyond_red_lights( |
| 84 | + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, |
| 85 | + const PlannerData & planner_data, const double object_front_overhang) |
| 86 | +{ |
| 87 | + const auto stop_line = find_next_stop_line(predicted_path, planner_data); |
| 88 | + if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang); |
| 89 | +} |
| 90 | + |
| 91 | +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( |
| 92 | + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) |
| 93 | +{ |
| 94 | + autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; |
| 95 | + filtered_objects.header = planner_data.predicted_objects->header; |
| 96 | + for (const auto & object : planner_data.predicted_objects->objects) { |
| 97 | + const auto is_pedestrian = |
| 98 | + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { |
| 99 | + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; |
| 100 | + }) != object.classification.end(); |
| 101 | + if (is_pedestrian) continue; |
| 102 | + |
| 103 | + auto filtered_object = object; |
| 104 | + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { |
| 105 | + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; |
| 106 | + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); |
| 107 | + if (no_overlap_path.size() <= 1) return true; |
| 108 | + const auto lat_offset_to_current_ego = |
| 109 | + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); |
| 110 | + const auto is_crossing_ego = |
| 111 | + lat_offset_to_current_ego <= |
| 112 | + object.shape.dimensions.y / 2.0 + std::max( |
| 113 | + params.left_offset + params.extra_left_offset, |
| 114 | + params.right_offset + params.extra_right_offset); |
| 115 | + return is_low_confidence || is_crossing_ego; |
| 116 | + }; |
| 117 | + if (params.objects_use_predicted_paths) { |
| 118 | + auto & predicted_paths = filtered_object.kinematics.predicted_paths; |
| 119 | + const auto new_end = |
| 120 | + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); |
| 121 | + predicted_paths.erase(new_end, predicted_paths.end()); |
| 122 | + if (params.objects_cut_predicted_paths_beyond_red_lights) |
| 123 | + for (auto & predicted_path : predicted_paths) |
| 124 | + cut_predicted_path_beyond_red_lights( |
| 125 | + predicted_path, planner_data, filtered_object.shape.dimensions.x); |
| 126 | + predicted_paths.erase( |
| 127 | + std::remove_if( |
| 128 | + predicted_paths.begin(), predicted_paths.end(), |
| 129 | + [](const auto & p) { return p.path.empty(); }), |
| 130 | + predicted_paths.end()); |
| 131 | + } |
| 132 | + |
| 133 | + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) |
| 134 | + filtered_objects.objects.push_back(filtered_object); |
| 135 | + } |
| 136 | + return filtered_objects; |
| 137 | +} |
| 138 | + |
| 139 | +} // namespace behavior_velocity_planner::out_of_lane |
0 commit comments