From e9827fc2d709de2fcb150e94d899611632cfbcba Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 17 Jan 2025 01:56:15 +0900 Subject: [PATCH 1/4] docs(lane_change): object filtering description Signed-off-by: Zulfaqar Azmi --- .../README.md | 181 +++++++----------- 1 file changed, 74 insertions(+), 107 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 601fb893f82c6..ecc5a9909cd3a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -451,139 +451,106 @@ The detection area for the target lane can be expanded beyond its original bound ##### Object filtering +Before performing safety checks, predicted objects are categorized based on their current pose and behavior at the time. These categories help determine how each object impacts the lane change process and guide the safety evaluation. + +The predicted objects are divided into four main categories: + +- **Target Lane Leading**: Objects that overlap with the target lane and are in front of the ego vehicle. This category is further divided into three subcategories: + - Moving: Objects with a velocity above a certain threshold. + - Stopped: Stationary objects within the target lane. + - Stopping at Bound: Objects outside the target lane but close to its boundaries. +- **Target Lane Trailing**: Objects that overlap with the target lane or any lanes preceding the target lane. Only moving vehicles are included in this category. +- **Current Lane**: Objects in front of the ego vehicle in the ego vehicle's current lane. +- **Others**: Any objects not classified into the above categories. + +Furthermore, for **Target Lane Leading** and **Current Lane** objects, only those positioned within the lane boundary or before the goal position are considered. Objects exceeding the end of the lane or the goal position are classified as **Others**. + +Once objects are filtered into their respective categories, they are sorted by distance closest to the ego vehicle to farthest. + +The following diagram illustrates the filtering process, + ```plantuml @startuml skinparam defaultTextAlignment center skinparam backgroundColor #WHITE -title NormalLaneChange::filterObjects Method Execution Flow +title Filter Objects main flowchart start -group "Filter Objects by Class" { -while (has not finished iterating through predicted object list) is (TRUE) - if (current object type != param.object_types_to_check?) then (TRUE) - #LightPink:Remove current object; -else (FALSE) - :Keep current object; -endif -end while -end group +:Filter predicted objects by class; +note left: Remove objects whose classes are\nnot specified in the **target_object** parameter. -if (predicted object list is empty?) then (TRUE) - :Return empty result; - stop -else (FALSE) -endif +:Filter oncoming predicted objects; +note left: Compare ego's current pose and object's current pose,\nand filter out any object whose yaw difference exceeds\n**collision_check.th_incoming_object_yaw**; -group "Filter Oncoming Objects" #PowderBlue { -while (has not finished iterating through predicted object list?) is (TRUE) -if (object's yaw with reference to ego's yaw difference < 90 degree?) then (TRUE) - :Keep current object; -else (FALSE) -if (object is stopping?) then (TRUE) - :Keep current object; -else (FALSE) - #LightPink:Remove current object; -endif -endif -endwhile -end group +:Transform predicted objects to extended predicted objects; -if (predicted object list is empty?) then (TRUE) - :Return empty result; +group "Filter target lane objects" { +if (Object's lateral distance\nfrom the centerline\nis more than\nhalf of ego's width?) then (TRUE) + if (Object's current pose\nis before the goal or\nthe end of the lane) then (TRUE) + if (Object overlaps target lane?) then (TRUE) + #LightGreen:To SUBPROCESS "Separate object based on its behavior in target lane"; + if(can separate object in SUBPROCESS) then (TRUE) stop -else (FALSE) -endif - -group "Filter Objects By Lanelets" #LightGreen { - -while (has not finished iterating through predicted object list) is (TRUE) - :Calculate lateral distance diff; - if (Object in target lane polygon?) then (TRUE) - if (lateral distance diff > half of ego's width?) then (TRUE) - if (Object's physical position is before terminal point?) then (TRUE) - :Add to target_lane_objects; - else (FALSE) - endif - else (FALSE) - endif + endif else (FALSE) + if (Object is moving\nAND\nis a vehicle class\nAND\npath overlaps target lane?) then (TRUE) + #LightGreen:To SUBPROCESS "Separate object based on its behavior in target lane"; + if(can separate object in SUBPROCESS) then (TRUE) + stop + endif + endif + endif + endif + #LightPink:From SUBPROCESS "Separate object based on its behavior in target lane"; + if (Object is in expanded target lane\nAND\nis stopped\nAND\nin front of ego?) then (TRUE) + :Add object to **filtered_object.leading_objects.stopped_at_bound** list; + stop endif - - if (Object overlaps with backward target lanes?) then (TRUE) - :Add to target_lane_objects; else (FALSE) - if (Object in current lane polygon?) then (TRUE) - :Add to current_lane_objects; - else (FALSE) - :Add to other_lane_objects; - endif + if (Object overlaps preceding target lanes?) then (TRUE) + :Add object to **filtered_object.trailing_objects** list; + stop endif -end while - -:Return target lanes object, current lanes object and other lanes object; -end group - -:Generate path from current lanes; - -if (path empty?) then (TRUE) - :Return empty result; +endif +} +if (Object is in front of ego\nAND\nis before terminal\nAND\noverlaps current lane?) then (TRUE) + :Add object to **filtered_object.current_lane** list; stop else (FALSE) + :Add object to **filtered_object.others** list; endif -group "Filter Target Lanes' objects" #LightCyan { +stop -while (has not finished iterating through target lanes' object list) is (TRUE) - if(velocity is within threshold?) then (TRUE) - :Keep current object; +#LightGreen:Start separating object based on its behavior in target lane; +if (Object is behind ego AND moving?) then (TRUE) + :Add object to **filtered_object.trailing_objects** list; + :Can separate object; + stop +else (FALSE) + if (Object is in front of ego?) then (TRUE) + if (Object is moving?) then (TRUE) + :Add object to **filtered_object.leading_objects.moving** list; + :Can separate object; + stop else (FALSE) - if(object is ahead of ego?) then (TRUE) - :keep current object; - else (FALSE) - #LightPink:remove current object; - endif - endif -endwhile -end group - -group "Filter Current Lanes' objects" #LightYellow { - -while (has not finished iterating through current lanes' object list) is (TRUE) - if(velocity is within threshold?) then (TRUE) - if(object is ahead of ego?) then (TRUE) - :keep current object; - else (FALSE) - #LightPink:remove current object; - endif - else (FALSE) - #LightPink:remove current object; - endif -endwhile -end group - -group "Filter Other Lanes' objects" #Lavender { - -while (has not finished iterating through other lanes' object list) is (TRUE) - if(velocity is within threshold?) then (TRUE) - if(object is ahead of ego?) then (TRUE) - :keep current object; - else (FALSE) - #LightPink:remove current object; - endif - else (FALSE) - #LightPink:remove current object; - endif -endwhile -end group - -:Transform the objects into extended predicted object and return them as lane_change_target_objects; -stop + :Add object to **filtered_object.leading_objects.stopped** list; + :Can separate object; + stop + endif + endif +endif +#LightPink:Cant separate proceed with other checks; @enduml ``` +!!! note + + As shown in the flowchart, oncoming objects are also filtered out. The filtering process considers the difference between the current orientation of the ego vehicle and that of the object. However, depending on the map's geometry, a certain threshold may need to be allowed. This threshold can be configured using the parameter collision_check.th_incoming_object_yaw. + ##### Collision check in prepare phase 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. From 9278b320c00db98b0846dd63cf4b237ccc4de65a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 17 Jan 2025 02:08:40 +0900 Subject: [PATCH 2/4] Move section up Signed-off-by: Zulfaqar Azmi --- .../README.md | 252 +++++++++--------- 1 file changed, 127 insertions(+), 125 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index ecc5a9909cd3a..0982ce5532e2e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -279,6 +279,133 @@ Within this range, we sample the lateral acceleration for the ego vehicle. Simil lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_acceleration) / lateral_acceleration_sampling_num ``` +##### Object filtering + +Before performing safety checks, predicted objects are categorized based on their current pose and behavior at the time. These categories help determine how each object impacts the lane change process and guide the safety evaluation. + +The predicted objects are divided into four main categories: + +- **Target Lane Leading**: Objects that overlap with the target lane and are in front of the ego vehicle. This category is further divided into three subcategories: + - Moving: Objects with a velocity above a certain threshold. + - Stopped: Stationary objects within the target lane. + - Stopped at Bound: Objects outside the target lane but close to its boundaries. +- **Target Lane Trailing**: Objects that overlap with the target lane or any lanes preceding the target lane. Only moving vehicles are included in this category. +- **Current Lane**: Objects in front of the ego vehicle in the ego vehicle's current lane. +- **Others**: Any objects not classified into the above categories. + +![object lanes](./images/lane_objects.drawio.svg) + +Furthermore, for **Target Lane Leading** and **Current Lane** objects, only those positioned within the lane boundary or before the goal position are considered. Objects exceeding the end of the lane or the goal position are classified as **Others**. + +Once objects are filtered into their respective categories, they are sorted by distance closest to the ego vehicle to farthest. + +The following diagram illustrates the filtering process, + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Filter Objects main flowchart + +start + +:Filter predicted objects by class; +note left: Remove objects whose classes are\nnot specified in the **target_object** parameter. + +:Filter oncoming predicted objects; +note left: Compare ego's current pose and object's current pose,\nand filter out any object whose yaw difference exceeds\n**collision_check.th_incoming_object_yaw**; + +:Transform predicted objects to extended predicted objects; + +group "Filter target lane objects" { +if (Object's lateral distance\nfrom the centerline\nis more than\nhalf of ego's width?) then (TRUE) + if (Object's current pose\nis before the goal or\nthe end of the lane) then (TRUE) + if (Object overlaps target lane?) then (TRUE) + #LightGreen:To SUBPROCESS "Separate object based on its behavior in target lane"; + if(can separate object in SUBPROCESS) then (TRUE) + stop + endif + else (FALSE) + if (Object is moving\nAND\nis a vehicle class\nAND\npath overlaps target lane?) then (TRUE) + #LightGreen:To SUBPROCESS "Separate object based on its behavior in target lane"; + if(can separate object in SUBPROCESS) then (TRUE) + stop + endif + endif + endif + endif + #LightPink:From SUBPROCESS "Separate object based on its behavior in target lane"; + if (Object is in expanded target lane\nAND\nis stopped\nAND\nin front of ego?) then (TRUE) + :Add object to **filtered_object.leading_objects.stopped_at_bound** list; + stop + endif + else (FALSE) + if (Object overlaps preceding target lanes?) then (TRUE) + :Add object to **filtered_object.trailing_objects** list; + stop + endif +endif +} +if (Object is in front of ego\nAND\nis before terminal\nAND\noverlaps current lane?) then (TRUE) + :Add object to **filtered_object.current_lane** list; + stop +else (FALSE) + :Add object to **filtered_object.others** list; +endif + +stop + +#LightGreen:Start separating object based on its behavior in target lane; +if (Object is behind ego AND moving?) then (TRUE) + :Add object to **filtered_object.trailing_objects** list; + :Can separate object; + stop +else (FALSE) + if (Object is in front of ego?) then (TRUE) + if (Object is moving?) then (TRUE) + :Add object to **filtered_object.leading_objects.moving** list; + :Can separate object; + stop + else (FALSE) + :Add object to **filtered_object.leading_objects.stopped** list; + :Can separate object; + stop + endif + endif +endif +#LightPink:Cant separate proceed with other checks; + +@enduml +``` + +!!! note + + As shown in the flowchart, oncoming objects are also filtered out. The filtering process considers the difference between the current orientation of the ego vehicle and that of the object. However, depending on the map's geometry, a certain threshold may need to be allowed. This threshold can be configured using the parameter collision_check.th_incoming_object_yaw. + +!!! note + + The **Target Lane Leading's Stopped at Boundary** objects are detected using the expanded area of the target lane beyond its original boundaries. The parameters `lane_expansion.left_offset` and `lane_expansion.right_offset` can be configured to adjust the expanded width. + +
+ + + + + +
+
+
Without Lane Expansion
+ Without lane expansion +
+ 		+
+
With Lane Expansion
+ With lane expansion +
+
+
+ #### Candidate Path's validity check A candidate path is considered valid if it meets the following criteria: @@ -424,133 +551,8 @@ See [safety check utils explanation](../autoware_behavior_path_planner_common/do First, we divide the target objects into obstacles in the target lane, obstacles in the current lane, and obstacles in other lanes. Target lane indicates the lane that the ego vehicle is going to reach after the lane change and current lane mean the current lane where the ego vehicle is following before the lane change. Other lanes are lanes that do not belong to the target and current lanes. The following picture describes objects on each lane. Note that users can remove objects either on current and other lanes from safety check by changing the flag, which are `check_objects_on_current_lanes` and `check_objects_on_other_lanes`. -![object lanes](./images/lane_objects.drawio.svg) - Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md). -The detection area for the target lane can be expanded beyond its original boundaries to enable detection of objects that are outside the target lane's limits. - -
- - - - - -
-
-
Without Lane Expansion
- Without lane expansion -
- 		-
-
With Lane Expansion
- With lane expansion -
-
-
- -##### Object filtering - -Before performing safety checks, predicted objects are categorized based on their current pose and behavior at the time. These categories help determine how each object impacts the lane change process and guide the safety evaluation. - -The predicted objects are divided into four main categories: - -- **Target Lane Leading**: Objects that overlap with the target lane and are in front of the ego vehicle. This category is further divided into three subcategories: - - Moving: Objects with a velocity above a certain threshold. - - Stopped: Stationary objects within the target lane. - - Stopping at Bound: Objects outside the target lane but close to its boundaries. -- **Target Lane Trailing**: Objects that overlap with the target lane or any lanes preceding the target lane. Only moving vehicles are included in this category. -- **Current Lane**: Objects in front of the ego vehicle in the ego vehicle's current lane. -- **Others**: Any objects not classified into the above categories. - -Furthermore, for **Target Lane Leading** and **Current Lane** objects, only those positioned within the lane boundary or before the goal position are considered. Objects exceeding the end of the lane or the goal position are classified as **Others**. - -Once objects are filtered into their respective categories, they are sorted by distance closest to the ego vehicle to farthest. - -The following diagram illustrates the filtering process, - -```plantuml -@startuml -skinparam defaultTextAlignment center -skinparam backgroundColor #WHITE - -title Filter Objects main flowchart - -start - -:Filter predicted objects by class; -note left: Remove objects whose classes are\nnot specified in the **target_object** parameter. - -:Filter oncoming predicted objects; -note left: Compare ego's current pose and object's current pose,\nand filter out any object whose yaw difference exceeds\n**collision_check.th_incoming_object_yaw**; - -:Transform predicted objects to extended predicted objects; - -group "Filter target lane objects" { -if (Object's lateral distance\nfrom the centerline\nis more than\nhalf of ego's width?) then (TRUE) - if (Object's current pose\nis before the goal or\nthe end of the lane) then (TRUE) - if (Object overlaps target lane?) then (TRUE) - #LightGreen:To SUBPROCESS "Separate object based on its behavior in target lane"; - if(can separate object in SUBPROCESS) then (TRUE) - stop - endif - else (FALSE) - if (Object is moving\nAND\nis a vehicle class\nAND\npath overlaps target lane?) then (TRUE) - #LightGreen:To SUBPROCESS "Separate object based on its behavior in target lane"; - if(can separate object in SUBPROCESS) then (TRUE) - stop - endif - endif - endif - endif - #LightPink:From SUBPROCESS "Separate object based on its behavior in target lane"; - if (Object is in expanded target lane\nAND\nis stopped\nAND\nin front of ego?) then (TRUE) - :Add object to **filtered_object.leading_objects.stopped_at_bound** list; - stop - endif - else (FALSE) - if (Object overlaps preceding target lanes?) then (TRUE) - :Add object to **filtered_object.trailing_objects** list; - stop - endif -endif -} -if (Object is in front of ego\nAND\nis before terminal\nAND\noverlaps current lane?) then (TRUE) - :Add object to **filtered_object.current_lane** list; - stop -else (FALSE) - :Add object to **filtered_object.others** list; -endif - -stop - -#LightGreen:Start separating object based on its behavior in target lane; -if (Object is behind ego AND moving?) then (TRUE) - :Add object to **filtered_object.trailing_objects** list; - :Can separate object; - stop -else (FALSE) - if (Object is in front of ego?) then (TRUE) - if (Object is moving?) then (TRUE) - :Add object to **filtered_object.leading_objects.moving** list; - :Can separate object; - stop - else (FALSE) - :Add object to **filtered_object.leading_objects.stopped** list; - :Can separate object; - stop - endif - endif -endif -#LightPink:Cant separate proceed with other checks; - -@enduml -``` - -!!! note - - As shown in the flowchart, oncoming objects are also filtered out. The filtering process considers the difference between the current orientation of the ego vehicle and that of the object. However, depending on the map's geometry, a certain threshold may need to be allowed. This threshold can be configured using the parameter collision_check.th_incoming_object_yaw. - ##### Collision check in prepare phase 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. From 83834674291115ab59c3850309b07a9e73da4a2b Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 20 Jan 2025 16:13:55 +0900 Subject: [PATCH 3/4] Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../autoware_behavior_path_lane_change_module/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 0982ce5532e2e..f75dcbd29cf2f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -279,7 +279,7 @@ Within this range, we sample the lateral acceleration for the ego vehicle. Simil lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_acceleration) / lateral_acceleration_sampling_num ``` -##### Object filtering +#### Object filtering Before performing safety checks, predicted objects are categorized based on their current pose and behavior at the time. These categories help determine how each object impacts the lane change process and guide the safety evaluation. From 9d0a78e02831329f5624422c70a61c5a3c95cf0d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 20 Jan 2025 16:14:03 +0900 Subject: [PATCH 4/4] Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../README.md | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index f75dcbd29cf2f..40ddce904e435 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -356,25 +356,26 @@ endif stop -#LightGreen:Start separating object based on its behavior in target lane; -if (Object is behind ego AND moving?) then (TRUE) - :Add object to **filtered_object.trailing_objects** list; - :Can separate object; - stop -else (FALSE) - if (Object is in front of ego?) then (TRUE) - if (Object is moving?) then (TRUE) - :Add object to **filtered_object.leading_objects.moving** list; - :Can separate object; +group Target Lane Objects Subprocess #LightYellow +start +if (Object is behind ego?) then (TRUE) + if (Object is moving?) then (TRUE) + :Add object to **filtered_object.trailing_objects** list; + #LightGreen:Can separate object; stop else (FALSE) - :Add object to **filtered_object.leading_objects.stopped** list; - :Can separate object; - stop + #LightPink:Cant separate proceed with other checks; + stop endif +else (FALSE) + if (Object is moving?) then (TRUE) + :Add object to **filtered_object.leading_objects.moving** list; + else (FALSE) + :Add object to **filtered_object.leading_objects.stopped** list; endif + #LightGreen:Can separate object; + stop endif -#LightPink:Cant separate proceed with other checks; @enduml ```