Skip to content

Commit 02d5869

Browse files
committed
fix(out_of_lane): ignore overlaps over all lanelets crossed by the ego path (autowarefoundation#6334)
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent e7dc9e1 commit 02d5869

File tree

5 files changed

+45
-26
lines changed

5 files changed

+45
-26
lines changed

planning/behavior_velocity_out_of_lane_module/src/footprint.cpp

+7-1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include <lanelet2_core/geometry/Polygon.h>
2222
#include <tf2/utils.h>
2323

24+
#include <algorithm>
2425
#include <vector>
2526

2627
namespace behavior_velocity_planner::out_of_lane
@@ -58,14 +59,19 @@ std::vector<lanelet::BasicPolygon2d> calculate_path_footprints(
5859
const auto base_footprint = make_base_footprint(params);
5960
std::vector<lanelet::BasicPolygon2d> path_footprints;
6061
path_footprints.reserve(ego_data.path.points.size());
61-
for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) {
62+
double length = 0.0;
63+
const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) +
64+
params.front_offset + params.extra_front_offset;
65+
for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size() && length < range; ++i) {
6266
const auto & path_pose = ego_data.path.points[i].point.pose;
6367
const auto angle = tf2::getYaw(path_pose.orientation);
6468
const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle);
6569
lanelet::BasicPolygon2d footprint;
6670
for (const auto & p : rotated_footprint.outer())
6771
footprint.emplace_back(p.x() + path_pose.position.x, p.y() + path_pose.position.y);
6872
path_footprints.push_back(footprint);
73+
if (i + 1 < ego_data.path.points.size())
74+
length += tier4_autoware_utils::calcDistance2d(path_pose, ego_data.path.points[i + 1].point);
6975
}
7076
return path_footprints;
7177
}

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp

+20-19
Original file line numberDiff line numberDiff line change
@@ -24,15 +24,28 @@
2424

2525
namespace behavior_velocity_planner::out_of_lane
2626
{
27+
lanelet::ConstLanelets calculate_path_lanelets(
28+
const EgoData & ego_data, const route_handler::RouteHandler & route_handler)
29+
{
30+
const auto lanelet_map_ptr = route_handler.getLaneletMapPtr();
31+
lanelet::ConstLanelets path_lanelets =
32+
planning_utils::getLaneletsOnPath(ego_data.path, lanelet_map_ptr, ego_data.pose);
33+
lanelet::BasicLineString2d path_ls;
34+
for (const auto & p : ego_data.path.points)
35+
path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y);
36+
for (const auto & dist_lanelet :
37+
lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, path_ls)) {
38+
if (!contains_lanelet(path_lanelets, dist_lanelet.second.id()))
39+
path_lanelets.push_back(dist_lanelet.second);
40+
}
41+
return path_lanelets;
42+
}
43+
2744
lanelet::ConstLanelets calculate_ignored_lanelets(
2845
const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets,
2946
const route_handler::RouteHandler & route_handler, const PlannerParam & params)
3047
{
3148
lanelet::ConstLanelets ignored_lanelets;
32-
// ignore lanelets that follows path lanelets
33-
for (const auto & path_lanelet : path_lanelets)
34-
for (const auto & following : route_handler.getRoutingGraphPtr()->following(path_lanelet))
35-
if (!contains_lanelet(path_lanelets, following.id())) ignored_lanelets.push_back(following);
3649
// ignore lanelets directly behind ego
3750
const auto behind =
3851
planning_utils::calculateOffsetPoint2d(ego_data.pose, params.rear_offset, 0.0);
@@ -55,26 +68,14 @@ lanelet::ConstLanelets calculate_other_lanelets(
5568
const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y);
5669
const auto lanelets_within_range = lanelet::geometry::findWithin2d(
5770
route_handler.getLaneletMapPtr()->laneletLayer, ego_point,
58-
std::max(params.slow_dist_threshold, params.stop_dist_threshold));
59-
const auto is_overlapped_by_path_lanelets = [&](const auto & l) {
60-
for (const auto & path_ll : path_lanelets) {
61-
if (
62-
boost::geometry::overlaps(
63-
path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) ||
64-
boost::geometry::within(path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) ||
65-
boost::geometry::within(l.polygon2d().basicPolygon(), path_ll.polygon2d().basicPolygon())) {
66-
return true;
67-
}
68-
}
69-
return false;
70-
};
71+
std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset +
72+
params.extra_front_offset);
7173
for (const auto & ll : lanelets_within_range) {
7274
if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road")
7375
continue;
7476
const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id());
7577
const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id());
76-
if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second))
77-
other_lanelets.push_back(ll.second);
78+
if (!is_path_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second);
7879
}
7980
return other_lanelets;
8081
}

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp

+8
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,14 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane
3333
return l.id() == id;
3434
}) != lanelets.end();
3535
};
36+
37+
/// @brief calculate lanelets crossed by the ego path
38+
/// @details calculated from the ids of the path msg, the lanelets containing path points
39+
/// @param [in] ego_data data about the ego vehicle
40+
/// @param [in] route_handler route handler
41+
/// @return lanelets crossed by the ego vehicle
42+
lanelet::ConstLanelets calculate_path_lanelets(
43+
const EgoData & ego_data, const route_handler::RouteHandler & route_handler);
3644
/// @brief calculate lanelets that should be ignored
3745
/// @param [in] ego_data data about the ego vehicle
3846
/// @param [in] path_lanelets lanelets driven by the ego vehicle

planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ Overlap calculate_overlap(
3434
Overlap overlap;
3535
const auto & left_bound = lanelet.leftBound2d().basicLineString();
3636
const auto & right_bound = lanelet.rightBound2d().basicLineString();
37+
// TODO(Maxime): these intersects (for each path footprint, for each lanelet) are too expensive
3738
const auto overlap_left = boost::geometry::intersects(path_footprint, left_bound);
3839
const auto overlap_right = boost::geometry::intersects(path_footprint, right_bound);
3940

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp

+9-6
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
6666
ego_data.pose = planner_data_->current_odometry->pose;
6767
ego_data.path.points = path->points;
6868
ego_data.first_path_idx =
69-
motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position);
69+
motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position);
7070
motion_utils::removeOverlapPoints(ego_data.path.points);
7171
ego_data.velocity = planner_data_->current_velocity->twist.linear.x;
7272
ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold;
@@ -75,13 +75,13 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
7575
const auto path_footprints = calculate_path_footprints(ego_data, params_);
7676
const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints");
7777
// Calculate lanelets to ignore and consider
78-
const auto path_lanelets = planning_utils::getLaneletsOnPath(
79-
ego_data.path, planner_data_->route_handler_->getLaneletMapPtr(),
80-
planner_data_->current_odometry->pose);
78+
stopwatch.tic("calculate_lanelets");
79+
const auto path_lanelets = calculate_path_lanelets(ego_data, *planner_data_->route_handler_);
8180
const auto ignored_lanelets =
8281
calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_);
8382
const auto other_lanelets = calculate_other_lanelets(
8483
ego_data, path_lanelets, ignored_lanelets, *planner_data_->route_handler_, params_);
84+
const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets");
8585

8686
debug_data_.footprints = path_footprints;
8787
debug_data_.path_lanelets = path_lanelets;
@@ -98,6 +98,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
9898
});
9999
if (overlapped_lanelet_it != other_lanelets.end()) {
100100
debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it);
101+
// TODO(Maxime): we may want to just add the overlapped lane to the ignored lanelets
101102
RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module ()\n");
102103
return true;
103104
}
@@ -183,13 +184,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
183184
RCLCPP_DEBUG(
184185
logger_,
185186
"Total time = %2.2fus\n"
187+
"\tcalculate_lanelets = %2.0fus\n"
186188
"\tcalculate_path_footprints = %2.0fus\n"
187189
"\tcalculate_overlapping_ranges = %2.0fus\n"
188190
"\tcalculate_decisions = %2.0fus\n"
189191
"\tcalc_slowdown_points = %2.0fus\n"
190192
"\tinsert_slowdown_points = %2.0fus\n",
191-
total_time_us, calculate_path_footprints_us, calculate_overlapping_ranges_us,
192-
calculate_decisions_us, calc_slowdown_points_us, insert_slowdown_points_us);
193+
total_time_us, calculate_lanelets_us, calculate_path_footprints_us,
194+
calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us,
195+
insert_slowdown_points_us);
193196
return true;
194197
}
195198

0 commit comments

Comments
 (0)