Skip to content

Commit 5d12185

Browse files
maxime-clemmaxime-clem
committed
split filter_predicted_objects in hpp/cpp and add docstrings
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent 3f17ebf commit 5d12185

File tree

2 files changed

+153
-112
lines changed

2 files changed

+153
-112
lines changed

‎planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

+139
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
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

‎planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp

+14-112
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2023 TIER IV, Inc.
1+
// Copyright 2023-2024 TIER IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -17,139 +17,41 @@
1717

1818
#include "types.hpp"
1919

20-
#include <motion_utils/trajectory/trajectory.hpp>
21-
#include <traffic_light_utils/traffic_light_utils.hpp>
20+
#include <behavior_velocity_planner_common/planner_data.hpp>
2221

23-
#include <boost/geometry/algorithms/intersects.hpp>
24-
25-
#include <lanelet2_core/geometry/LaneletMap.h>
26-
#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
27-
28-
#include <algorithm>
2922
#include <optional>
30-
#include <string>
31-
#include <vector>
3223

3324
namespace behavior_velocity_planner::out_of_lane
3425
{
26+
/// @brief cut a predicted path beyond the given stop line
27+
/// @param [inout] predicted_path predicted path to cut
28+
/// @param [in] stop_line stop line used for cutting
29+
/// @param [in] object_front_overhang extra distance to cut ahead of the stop line
3530
void cut_predicted_path_beyond_line(
3631
autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
37-
const lanelet::BasicLineString2d & stop_line, const double object_front_overhang)
38-
{
39-
auto stop_line_idx = 0UL;
40-
bool found = false;
41-
lanelet::BasicSegment2d path_segment;
42-
path_segment.first.x() = predicted_path.path.front().position.x;
43-
path_segment.first.y() = predicted_path.path.front().position.y;
44-
for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) {
45-
path_segment.second.x() = predicted_path.path[stop_line_idx].position.x;
46-
path_segment.second.y() = predicted_path.path[stop_line_idx].position.y;
47-
if (boost::geometry::intersects(stop_line, path_segment)) {
48-
found = true;
49-
break;
50-
}
51-
path_segment.first = path_segment.second;
52-
}
53-
if (found) {
54-
auto cut_idx = stop_line_idx;
55-
double arc_length = 0;
56-
while (cut_idx > 0 && arc_length < object_front_overhang) {
57-
arc_length += tier4_autoware_utils::calcDistance2d(
58-
predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]);
59-
--cut_idx;
60-
}
61-
predicted_path.path.resize(cut_idx);
62-
}
63-
}
32+
const lanelet::BasicLineString2d & stop_line, const double object_front_overhang);
6433

34+
/// @brief find the next red light stop line along the given path
35+
/// @param [in] path predicted path to check for a stop line
36+
/// @param [in] planner_data planner data with stop line information
37+
/// @return the first red light stop line found along the path (if any)
6538
std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
66-
const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data)
67-
{
68-
lanelet::ConstLanelets lanelets;
69-
lanelet::BasicLineString2d query_line;
70-
for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y);
71-
const auto query_results = lanelet::geometry::findWithin2d(
72-
planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_line);
73-
for (const auto & r : query_results) lanelets.push_back(r.second);
74-
for (const auto & ll : lanelets) {
75-
for (const auto & element : ll.regulatoryElementsAs<lanelet::TrafficLight>()) {
76-
const auto traffic_signal_stamped = planner_data.getTrafficSignal(element->id());
77-
if (
78-
traffic_signal_stamped.has_value() && element->stopLine().has_value() &&
79-
traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) {
80-
lanelet::BasicLineString2d stop_line;
81-
for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y());
82-
return stop_line;
83-
}
84-
}
85-
}
86-
return std::nullopt;
87-
}
39+
const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data);
8840

8941
/// @brief cut predicted path beyond stop lines of red lights
9042
/// @param [inout] predicted_path predicted path to cut
9143
/// @param [in] planner_data planner data to get the map and traffic light information
9244
void cut_predicted_path_beyond_red_lights(
9345
autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
94-
const PlannerData & planner_data, const double object_front_overhang)
95-
{
96-
const auto stop_line = find_next_stop_line(predicted_path, planner_data);
97-
if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang);
98-
}
46+
const PlannerData & planner_data, const double object_front_overhang);
9947

10048
/// @brief filter predicted objects and their predicted paths
10149
/// @param [in] planner_data planner data
10250
/// @param [in] ego_data ego data
10351
/// @param [in] params parameters
10452
/// @return filtered predicted objects
10553
autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
106-
const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params)
107-
{
108-
autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects;
109-
filtered_objects.header = planner_data.predicted_objects->header;
110-
for (const auto & object : planner_data.predicted_objects->objects) {
111-
const auto is_pedestrian =
112-
std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) {
113-
return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
114-
}) != object.classification.end();
115-
if (is_pedestrian) continue;
116-
117-
auto filtered_object = object;
118-
const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
119-
const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;
120-
const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path);
121-
if (no_overlap_path.size() <= 1) return true;
122-
const auto lat_offset_to_current_ego =
123-
std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position));
124-
const auto is_crossing_ego =
125-
lat_offset_to_current_ego <=
126-
object.shape.dimensions.y / 2.0 + std::max(
127-
params.left_offset + params.extra_left_offset,
128-
params.right_offset + params.extra_right_offset);
129-
return is_low_confidence || is_crossing_ego;
130-
};
131-
if (params.objects_use_predicted_paths) {
132-
auto & predicted_paths = filtered_object.kinematics.predicted_paths;
133-
const auto new_end =
134-
std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path);
135-
predicted_paths.erase(new_end, predicted_paths.end());
136-
if (params.objects_cut_predicted_paths_beyond_red_lights)
137-
for (auto & predicted_path : predicted_paths)
138-
cut_predicted_path_beyond_red_lights(
139-
predicted_path, planner_data, filtered_object.shape.dimensions.x);
140-
predicted_paths.erase(
141-
std::remove_if(
142-
predicted_paths.begin(), predicted_paths.end(),
143-
[](const auto & p) { return p.path.empty(); }),
144-
predicted_paths.end());
145-
}
146-
147-
if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty())
148-
filtered_objects.objects.push_back(filtered_object);
149-
}
150-
return filtered_objects;
151-
}
152-
54+
const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params);
15355
} // namespace behavior_velocity_planner::out_of_lane
15456

15557
#endif // FILTER_PREDICTED_OBJECTS_HPP_

0 commit comments

Comments
 (0)