Skip to content

Commit 60f91a2

Browse files
zulfaqar-azmi-t4pre-commit-ci[bot]
zulfaqar-azmi-t4
and
pre-commit-ci[bot]
committed
perf(lane_change): rework object filter (autowarefoundation#6847)
* refactor(lane_change): rework object filter Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Use preceeding lanes Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * style(pre-commit): autofix * fix preceding lanes Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Adds flow chart Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * prioritize on coming object check instead Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Fix flow chart and rearrange code to early return Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Colorize flow chart Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 6dcacf0 commit 60f91a2

File tree

17 files changed

+524
-233
lines changed

17 files changed

+524
-233
lines changed

‎planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -145,7 +145,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
145145
const auto [object_within_target_lane, object_outside_target_lane] =
146146
utils::path_safety_checker::separateObjectsByLanelets(
147147
*planner_data_->dynamic_object, data.current_lanelets,
148-
utils::path_safety_checker::isPolygonOverlapLanelet);
148+
[](const auto & obj, const auto & lane) {
149+
return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
150+
});
149151

150152
// Assume that the maximum allocation for data.other object is the sum of
151153
// objects_within_target_lane and object_outside_target_lane. The maximum allocation for

‎planning/behavior_path_goal_planner_module/src/util.cpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -145,7 +145,9 @@ PredictedObjects extractObjectsInExpandedPullOverLanes(
145145
route_handler, left_side, backward_distance, forward_distance, bound_offset);
146146

147147
const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets(
148-
objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
148+
objects, lanes, [](const auto & obj, const auto & lanelet) {
149+
return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet);
150+
});
149151

150152
return objects_in_lanes;
151153
}

‎planning/behavior_path_lane_change_module/README.md

+165
Original file line numberDiff line numberDiff line change
@@ -319,6 +319,171 @@ The detection area for the target lane can be expanded beyond its original bound
319319
</table>
320320
</div>
321321

322+
##### Object filtering
323+
324+
```plantuml
325+
@startuml
326+
skinparam defaultTextAlignment center
327+
skinparam backgroundColor #WHITE
328+
329+
title NormalLaneChange::filterObjects Method Execution Flow
330+
331+
start
332+
333+
group "Filter Objects by Class" {
334+
:Iterate through each object in objects list;
335+
while (has not finished iterating through object list) is (TRUE)
336+
if (current object type != param.object_types_to_check?) then (TRUE)
337+
#LightPink:Remove current object;
338+
else (FALSE)
339+
:Keep current object;
340+
endif
341+
end while
342+
end group
343+
344+
if (object list is empty?) then (TRUE)
345+
:Return empty result;
346+
stop
347+
else (FALSE)
348+
endif
349+
350+
group "Filter Oncoming Objects" #PowderBlue {
351+
:Iterate through each object in target lane objects list;
352+
while (has not finished iterating through object list?) is (TRUE)
353+
:check object's yaw with reference to ego's yaw.;
354+
if (yaw difference < 90 degree?) then (TRUE)
355+
:Keep current object;
356+
else (FALSE)
357+
if (object is stopping?) then (TRUE)
358+
:Keep current object;
359+
else (FALSE)
360+
#LightPink:Remove current object;
361+
endif
362+
endif
363+
endwhile
364+
end group
365+
366+
if (object list is empty?) then (TRUE)
367+
:Return empty result;
368+
stop
369+
else (FALSE)
370+
endif
371+
372+
group "Filter Objects Ahead Terminal" #Beige {
373+
:Calculate lateral distance from ego to current lanes center;
374+
375+
:Iterate through each object in objects list;
376+
while (has not finished iterating through object list) is (TRUE)
377+
:Get current object's polygon;
378+
:Initialize distance to terminal from object to max;
379+
while (has not finished iterating through object polygon's vertices) is (TRUE)
380+
:Calculate object's lateral distance to end of lane;
381+
:Update minimum distance to terminal from object;
382+
end while
383+
if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE)
384+
#LightPink:Remove current object;
385+
else (FALSE)
386+
endif
387+
end while
388+
end group
389+
390+
if (object list is empty?) then (TRUE)
391+
:Return empty result;
392+
stop
393+
else (FALSE)
394+
endif
395+
396+
group "Filter Objects By Lanelets" #LightGreen {
397+
398+
:Iterate through each object in objects list;
399+
while (has not finished iterating through object list) is (TRUE)
400+
:lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.;
401+
if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE)
402+
:Add to target_lane_objects;
403+
else (FALSE)
404+
if (Object overlaps with backward target lanes?) then (TRUE)
405+
:Add to target_lane_objects;
406+
else (FALSE)
407+
if (Object in current lane polygon?) then (TRUE)
408+
:Add to current_lane_objects;
409+
else (FALSE)
410+
:Add to other_lane_objects;
411+
endif
412+
endif
413+
endif
414+
end while
415+
416+
:Return target lanes object, current lanes object and other lanes object;
417+
end group
418+
419+
:Generate path from current lanes;
420+
421+
if (path empty?) then (TRUE)
422+
:Return empty result;
423+
stop
424+
else (FALSE)
425+
endif
426+
427+
group "Filter Target Lanes' objects" #LightCyan {
428+
429+
:Iterate through each object in target lane objects list;
430+
while (has not finished iterating through object list) is (TRUE)
431+
:check object's velocity;
432+
if(velocity is within threshold?) then (TRUE)
433+
:Keep current object;
434+
else (FALSE)
435+
:check whether object is ahead of ego;
436+
if(object is ahead of ego?) then (TRUE)
437+
:keep current object;
438+
else (FALSE)
439+
#LightPink:remove current object;
440+
endif
441+
endif
442+
endwhile
443+
end group
444+
445+
group "Filter Current Lanes' objects" #LightYellow {
446+
447+
:Iterate through each object in current lane objects list;
448+
while (has not finished iterating through object list) is (TRUE)
449+
:check object's velocity;
450+
if(velocity is within threshold?) then (TRUE)
451+
:check whether object is ahead of ego;
452+
if(object is ahead of ego?) then (TRUE)
453+
:keep current object;
454+
else (FALSE)
455+
#LightPink:remove current object;
456+
endif
457+
else (FALSE)
458+
#LightPink:remove current object;
459+
endif
460+
endwhile
461+
end group
462+
463+
group "Filter Other Lanes' objects" #Lavender {
464+
465+
:Iterate through each object in other lane objects list;
466+
while (has not finished iterating through object list) is (TRUE)
467+
:check object's velocity;
468+
if(velocity is within threshold?) then (TRUE)
469+
:check whether object is ahead of ego;
470+
if(object is ahead of ego?) then (TRUE)
471+
:keep current object;
472+
else (FALSE)
473+
#LightPink:remove current object;
474+
endif
475+
else (FALSE)
476+
#LightPink:remove current object;
477+
endif
478+
endwhile
479+
end group
480+
481+
:Trasform the objects into extended predicted object and return them as lane_change_target_objects;
482+
stop
483+
484+
@enduml
485+
```
486+
322487
##### Collision check in prepare phase
323488

324489
The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases.

‎planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

+22-11
Original file line numberDiff line numberDiff line change
@@ -30,10 +30,12 @@ using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject
3030
using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
3131
using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
3232
using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
33+
using data::lane_change::LanesPolygon;
3334
using geometry_msgs::msg::Point;
3435
using geometry_msgs::msg::Pose;
3536
using geometry_msgs::msg::Twist;
3637
using route_handler::Direction;
38+
using utils::path_safety_checker::ExtendedPredictedObjects;
3739

3840
class NormalLaneChange : public LaneChangeBase
3941
{
@@ -117,10 +119,26 @@ class NormalLaneChange : public LaneChangeBase
117119
const lanelet::ConstLanelets & current_lanes,
118120
const lanelet::ConstLanelets & target_lanes) const;
119121

120-
LaneChangeTargetObjects getTargetObjects(
122+
ExtendedPredictedObjects getTargetObjects(
123+
const LaneChangeLanesFilteredObjects & predicted_objects,
124+
const lanelet::ConstLanelets & current_lanes) const;
125+
126+
LaneChangeLanesFilteredObjects filterObjects(
121127
const lanelet::ConstLanelets & current_lanes,
122128
const lanelet::ConstLanelets & target_lanes) const;
123129

130+
void filterOncomingObjects(PredictedObjects & objects) const;
131+
132+
void filterAheadTerminalObjects(
133+
PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const;
134+
135+
void filterObjectsByLanelets(
136+
const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
137+
const lanelet::ConstLanelets & target_lanes,
138+
std::vector<PredictedObject> & current_lane_objects,
139+
std::vector<PredictedObject> & target_lane_objects,
140+
std::vector<PredictedObject> & other_lane_objects) const;
141+
124142
PathWithLaneId getPrepareSegment(
125143
const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
126144
const double prepare_length) const override;
@@ -156,18 +174,11 @@ class NormalLaneChange : public LaneChangeBase
156174
bool isValidPath(const PathWithLaneId & path) const override;
157175

158176
PathSafetyStatus isLaneChangePathSafe(
159-
const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
160-
const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck,
177+
const LaneChangePath & lane_change_path,
178+
const ExtendedPredictedObjects & collision_check_objects,
179+
const utils::path_safety_checker::RSSparams & rss_params,
161180
CollisionCheckDebugMap & debug_data) const;
162181

163-
LaneChangeTargetObjectIndices filterObject(
164-
const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
165-
const lanelet::ConstLanelets & target_lanes,
166-
const lanelet::ConstLanelets & target_backward_lanes) const;
167-
168-
std::vector<ExtendedPredictedObject> filterObjectsInTargetLane(
169-
const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const;
170-
171182
//! @brief Check if the ego vehicle is in stuck by a stationary obstacle.
172183
//! @param obstacle_check_distance Distance to check ahead for any objects that might be
173184
//! obstructing ego path. It makes sense to use values like the maximum lane change distance.

‎planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp

+12-4
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <interpolation/linear_interpolation.hpp>
2121

2222
#include <lanelet2_core/primitives/Lanelet.h>
23+
#include <lanelet2_core/primitives/Polygon.h>
2324

2425
#include <utility>
2526
#include <vector>
@@ -195,11 +196,11 @@ struct LaneChangeTargetObjectIndices
195196
std::vector<size_t> other_lane{};
196197
};
197198

198-
struct LaneChangeTargetObjects
199+
struct LaneChangeLanesFilteredObjects
199200
{
200-
std::vector<utils::path_safety_checker::ExtendedPredictedObject> current_lane{};
201-
std::vector<utils::path_safety_checker::ExtendedPredictedObject> target_lane{};
202-
std::vector<utils::path_safety_checker::ExtendedPredictedObject> other_lane{};
201+
utils::path_safety_checker::ExtendedPredictedObjects current_lane{};
202+
utils::path_safety_checker::ExtendedPredictedObjects target_lane{};
203+
utils::path_safety_checker::ExtendedPredictedObjects other_lane{};
203204
};
204205

205206
enum class LaneChangeModuleType {
@@ -216,6 +217,13 @@ struct PathSafetyStatus
216217
bool is_safe{true};
217218
bool is_object_coming_from_rear{false};
218219
};
220+
221+
struct LanesPolygon
222+
{
223+
std::optional<lanelet::BasicPolygon2d> current;
224+
std::optional<lanelet::BasicPolygon2d> target;
225+
std::vector<lanelet::BasicPolygon2d> target_backward;
226+
};
219227
} // namespace behavior_path_planner::data::lane_change
220228

221229
#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_

‎planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ struct Debug
3535
LaneChangePaths valid_paths;
3636
CollisionCheckDebugMap collision_check_objects;
3737
CollisionCheckDebugMap collision_check_objects_after_approval;
38-
LaneChangeTargetObjects filtered_objects;
38+
LaneChangeLanesFilteredObjects filtered_objects;
3939
geometry_msgs::msg::Polygon execution_area;
4040
geometry_msgs::msg::Pose ego_pose;
4141
lanelet::ConstLanelets current_lanes;

‎planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

+5
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject
4646
using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
4747
using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
4848
using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
49+
using data::lane_change::LanesPolygon;
4950
using data::lane_change::PathSafetyStatus;
5051
using geometry_msgs::msg::Point;
5152
using geometry_msgs::msg::Pose;
@@ -292,6 +293,10 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol
292293
double calcPhaseLength(
293294
const double velocity, const double maximum_velocity, const double acceleration,
294295
const double time);
296+
297+
LanesPolygon createLanesPolygon(
298+
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
299+
const std::vector<lanelet::ConstLanelets> & target_backward_lanes);
295300
} // namespace behavior_path_planner::utils::lane_change
296301

297302
namespace behavior_path_planner::utils::lane_change::debug

0 commit comments

Comments
 (0)