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

feat(out_of_lane): add option to ignore overlaps in lane changes (#6991) #1292

Merged
merged 1 commit into from
May 13, 2024
Merged

feat(out_of_lane): add option to ignore overlaps in lane changes (#6991) #1292

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
1 change: 1 addition & 0 deletions planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
out_of_lane: # module to stop or slowdown before overlapping another lane with other objects
mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc"
skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane
ignore_overlaps_over_lane_changeable_lanelets: true # if true, overlaps on lane changeable lanelets are ignored

threshold:
time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time
Expand Down
11 changes: 11 additions & 0 deletions planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,15 @@ lanelet::ConstLanelets calculate_path_lanelets(
return path_lanelets;
}

void add_lane_changeable_lanelets(
lanelet::ConstLanelets & lanelets_to_ignore, const lanelet::ConstLanelets & path_lanelets,
const route_handler::RouteHandler & route_handler)
{
for (const auto & path_lanelet : path_lanelets)
for (const auto & ll : route_handler.getLaneChangeableNeighbors(path_lanelet))
if (!contains_lanelet(lanelets_to_ignore, ll.id())) lanelets_to_ignore.push_back(ll);
}

lanelet::ConstLanelets calculate_ignored_lanelets(
const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets,
const route_handler::RouteHandler & route_handler, const PlannerParam & params)
Expand All @@ -93,6 +102,8 @@ lanelet::ConstLanelets calculate_ignored_lanelets(
const auto is_path_lanelet = contains_lanelet(path_lanelets, l.second.id());
if (!is_path_lanelet) ignored_lanelets.push_back(l.second);
}
if (params.ignore_overlaps_over_lane_changeable_lanelets)
add_lane_changeable_lanelets(ignored_lanelets, path_lanelets, route_handler);
return ignored_lanelets;
}

Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_velocity_out_of_lane_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
pp.mode = getOrDeclareParameter<std::string>(node, ns + ".mode");
pp.skip_if_already_overlapping =
getOrDeclareParameter<bool>(node, ns + ".skip_if_already_overlapping");
pp.ignore_overlaps_over_lane_changeable_lanelets =
getOrDeclareParameter<bool>(node, ns + ".ignore_overlaps_over_lane_changeable_lanelets");

pp.time_threshold = getOrDeclareParameter<double>(node, ns + ".threshold.time_threshold");
pp.intervals_ego_buffer = getOrDeclareParameter<double>(node, ns + ".intervals.ego_time_buffer");
Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_velocity_out_of_lane_module/src/types.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@ struct PlannerParam
std::string mode; // mode used to consider a conflict with an object
bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps
// another lane
bool ignore_overlaps_over_lane_changeable_lanelets; // if true, overlaps on lane changeable
// lanelets are ignored

double time_threshold; // [s](mode="threshold") objects time threshold
double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range
Expand Down
Loading