File tree 4 files changed +16
-0
lines changed
planning/behavior_velocity_out_of_lane_module
4 files changed +16
-0
lines changed Original file line number Diff line number Diff line change 3
3
out_of_lane : # module to stop or slowdown before overlapping another lane with other objects
4
4
mode : ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc"
5
5
skip_if_already_overlapping : true # do not run this module when ego already overlaps another lane
6
+ ignore_overlaps_over_lane_changeable_lanelets : true # if true, overlaps on lane changeable lanelets are ignored
6
7
7
8
threshold :
8
9
time_threshold : 5.0 # [s] consider objects that will reach an overlap within this time
Original file line number Diff line number Diff line change @@ -78,6 +78,15 @@ lanelet::ConstLanelets calculate_path_lanelets(
78
78
return path_lanelets;
79
79
}
80
80
81
+ void add_lane_changeable_lanelets (
82
+ lanelet::ConstLanelets & lanelets_to_ignore, const lanelet::ConstLanelets & path_lanelets,
83
+ const route_handler::RouteHandler & route_handler)
84
+ {
85
+ for (const auto & path_lanelet : path_lanelets)
86
+ for (const auto & ll : route_handler.getLaneChangeableNeighbors (path_lanelet))
87
+ if (!contains_lanelet (lanelets_to_ignore, ll.id ())) lanelets_to_ignore.push_back (ll);
88
+ }
89
+
81
90
lanelet::ConstLanelets calculate_ignored_lanelets (
82
91
const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets,
83
92
const route_handler::RouteHandler & route_handler, const PlannerParam & params)
@@ -93,6 +102,8 @@ lanelet::ConstLanelets calculate_ignored_lanelets(
93
102
const auto is_path_lanelet = contains_lanelet (path_lanelets, l.second .id ());
94
103
if (!is_path_lanelet) ignored_lanelets.push_back (l.second );
95
104
}
105
+ if (params.ignore_overlaps_over_lane_changeable_lanelets )
106
+ add_lane_changeable_lanelets (ignored_lanelets, path_lanelets, route_handler);
96
107
return ignored_lanelets;
97
108
}
98
109
Original file line number Diff line number Diff line change @@ -38,6 +38,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
38
38
pp.mode = getOrDeclareParameter<std::string>(node, ns + " .mode" );
39
39
pp.skip_if_already_overlapping =
40
40
getOrDeclareParameter<bool >(node, ns + " .skip_if_already_overlapping" );
41
+ pp.ignore_overlaps_over_lane_changeable_lanelets =
42
+ getOrDeclareParameter<bool >(node, ns + " .ignore_overlaps_over_lane_changeable_lanelets" );
41
43
42
44
pp.time_threshold = getOrDeclareParameter<double >(node, ns + " .threshold.time_threshold" );
43
45
pp.intervals_ego_buffer = getOrDeclareParameter<double >(node, ns + " .intervals.ego_time_buffer" );
Original file line number Diff line number Diff line change @@ -39,6 +39,8 @@ struct PlannerParam
39
39
std::string mode; // mode used to consider a conflict with an object
40
40
bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps
41
41
// another lane
42
+ bool ignore_overlaps_over_lane_changeable_lanelets; // if true, overlaps on lane changeable
43
+ // lanelets are ignored
42
44
43
45
double time_threshold; // [s](mode="threshold") objects time threshold
44
46
double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range
You can’t perform that action at this time.
0 commit comments