diff --git a/planning/behavior_path_goal_planner_module/README.md b/planning/behavior_path_goal_planner_module/README.md index 6bd2f8b9c79e4..30183d8aea9b8 100644 --- a/planning/behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -4,7 +4,7 @@ Plan path around the goal. -- Park at the designated goal. +- Arrive at the designated goal. - Modify the goal to avoid obstacles or to pull over at the side of tha lane. ## Design @@ -29,13 +29,13 @@ package goal_planner{ class FreeSpacePullOver {} } - class GoalSeacher {} + class GoalSearcher {} struct GoalCandidates {} struct PullOverPath{} abstract class PullOverPlannerBase {} - abstract class GoalSeacherBase {} + abstract class GoalsearcherBase {} } @@ -62,7 +62,7 @@ package freespace_planning_algorithms ShiftPullOver --|> PullOverPlannerBase GeometricPullOver --|> PullOverPlannerBase FreeSpacePullOver --|> PullOverPlannerBase -GoalSeacher --|> GoalSeacherBase +GoalSearcher --|> GoalSearcherBase DefaultFixedPlanner --|> FixedGoalPlannerBase PathShifter --o ShiftPullOver @@ -71,23 +71,27 @@ AstarSearch --o FreeSpacePullOver RRTStar --o FreeSpacePullOver PullOverPlannerBase --o GoalPlannerModule -GoalSeacherBase --o GoalPlannerModule +GoalSearcherBase --o GoalPlannerModule FixedGoalPlannerBase --o GoalPlannerModule PullOverPath --o PullOverPlannerBase -GoalCandidates --o GoalSeacherBase +GoalCandidates --o GoalSearcherBase @enduml ``` ## start condition -Either one is activated when all conditions are met. - ### fixed_goal_planner -- Route is set with `allow_goal_modification=false` by default. -- ego-vehicle is in the same lane as the goal. +This is a very simple function that plans a smooth path to a specified goal. This function does not require approval and always runs with the other modules. +_NOTE: this planner does not perform the several features described below, such as "goal search", "collision check", "safety check", etc._ + +Executed when both conditions are met. + +- Route is set with `allow_goal_modification=false`. This is the default. +- The goal is set in the normal lane. In other words, it is NOT `road_shoulder`. +- Ego-vehicle exists in the same lane sequence as the goal. If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. @@ -129,61 +133,78 @@ If the target path contains a goal, modify the points of the path so that the pa | th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | | center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | -## **collision check** +## **Goal Search** -### **occupancy grid based collision check** +To realize pull over even when an obstacle exists near the original goal, a collision free area is searched within a certain range around the original goal. The goal found will be published as `/planning/scenario_planning/modified_goal`. -Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. +[goal search video](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) -#### Parameters for occupancy grid based collision check +1. The original goal is set, and the refined goal pose is obtained by moving in the direction normal to the lane center line and keeping `margin_from_boundary` from the edge of the lane. + ![refined_goal](./images/goal_planner-refined_goal.drawio.svg) -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | -| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | -| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | -| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | -| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | +2. Using `refined_goal` as the base goal, search for candidate goals in the range of `-forward_goal_search_length` to `backward_goal_search_length` in the longitudinal direction and `longitudinal_margin` to `longitudinal_margin+max_lateral_offset` in th lateral direction based on refined_goal. + ![goal_candidates](./images/goal_planner-goal_candidates.drawio.svg) -### **object recognition based collision check** +3. Each candidate goal is prioritized and a path is generated for each planner for each goal. The priority of a candidate goal is determined by its distance from the base goal. The ego vehicle tries to park for the highest possible goal. The distance is determined by the selected policy. In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance. This means the distance is calculated by `longitudinal_distance + lateral_cost*lateral_distance` + ![goal_distance](./images/goal_planner-goal_distance.drawio.svg) + The following figure is an example of minimum_weighted_distance.​ The white number indicates the goal candidate priority, and the smaller the number, the higher the priority. the 0 goal indicates the base goal. + ![goal_priority_rviz_with_goal](./images/goal_priority_with_goal.png) + ![goal_priority_rviz](./images/goal_priority_rviz.png) -#### Parameters for object recognition based collision check +4. If the footprint in each goal candidate is within `object_recognition_collision_check_margin` of that of the object, it is determined to be unsafe. These goals are not selected. If `use_occupancy_grid_for_goal_search` is enabled, collision detection on the grid is also performed with `occupancy_grid_collision_check_margin`. -| Name | Unit | Type | Description | Default value | -| :----------------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | -| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | -| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | +Red goals candidates in the image indicate unsafe ones. -## **Goal Search** +![is_safe](./images/goal_planner-is_safe.drawio.svg) -If it is not possible to park safely at a given goal, `/planning/scenario_planning/modified_goal` is -searched for in certain range of the shoulder lane. +It is possible to keep `longitudinal_margin` in the longitudinal direction apart from the collision margin for obstacles from the goal candidate. This is intended to provide natural spacing for parking and efficient departure. -[goal search video](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) +![longitudinal_margin](./images/goal_planner-longitudinal_margin.drawio.svg) + +Also, if `prioritize_goals_before_objects` is enabled, To arrive at each goal, the number of objects that need to be avoided in the target range is counted, and those with the lowest number are given priority. + +The images represent a count of objects to be avoided at each range, with priority given to those with the lowest number, regardless of the aforementioned distances. + +![object_to_avoid](./images/goal_planner-object_to_avoid.drawio.svg) + +The gray numbers represent objects to avoid, and you can see that the goal in front has a higher priority in this case. + +![goal_priority_object_to_avoid_rviz.png](./images/goal_priority_object_to_avoid_rviz.png) ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | -| goal_priority | [-] | string | In case `minimum_weighted_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_longitudinal_distance`, sort with weighted lateral distance against longitudinal distance. | `minimum_weighted_distance` | -| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | +| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | ## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. +The path is generated with a certain margin (default: `0.75 m`) from the boundary of shoulder lane. + +The process is time consuming because multiple planners are used to generate path for each candidate goal. Therefore, in this module, the path generation is performed in a thread different from the main thread. +Path generation is performed at the timing when the shape of the output path of the previous module changes. If a new module launches, it is expected to go to the previous stage before the goal planner, in which case the goal planner re-generates the path. The goal planner is expected to run at the end of multiple modules, which is achieved by `keep_last` in the planner manager. + +Threads in the goal planner are shown below. + +![threads.png](./images/goal_planner-threads.drawio.svg) + +The main thread will be the one called from the planner manager flow. + +- The goal candidate generation and path candidate generation are done in a separate thread(lane path generation thread). +- The path candidates generated there are referred to by the main thread, and the one judged to be valid for the current planner data (e.g. ego and object information) is selected from among them. valid means no sudden deceleration, no collision with obstacles, etc. The selected path will be the output of this module. +- If there is no path selected, or if the selected path is collision and ego is stuck, a separate thread(freespace path generation thread) will generate a path using freespace planning algorithm. If a valid free space path is found, it will be the output of the module. If the object moves and the pull over path generated along the lane is collision-free, the path is used as output again. See also the section on freespace parking for more information on the flow of generating freespace paths. | Name | Unit | Type | Description | Default value | | :------------------------------- | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | @@ -276,12 +297,6 @@ Simultaneous execution with `avoidance_module` in the flowchart is under develop -#### Unimplemented parts / limitations for freespace parking - -- When a short path is generated, the ego does can not drive with it. -- Complex cases take longer to generate or fail. -- The drivable area is not guaranteed to fit in the parking_lot. - #### Parameters freespace parking | Name | Unit | Type | Description | Default value | @@ -289,3 +304,125 @@ Simultaneous execution with `avoidance_module` in the flowchart is under develop | enable_freespace_parking | [-] | bool | This flag enables freespace parking, which runs when the vehicle is stuck due to e.g. obstacles in the parking area. | true | See [freespace_planner](../freespace_planner/README.md) for other parameters. + +## **collision check for path generation** + +To select a safe one from the path candidates, a collision check with obstacles is performed. + +### **occupancy grid based collision check** + +Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. + +#### Parameters for occupancy grid based collision check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | +| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | + +### **object recognition based collision check** + +A collision decision is made for each of the path candidates, and a collision-free path is selected. +There are three main margins at this point. + +- `object_recognition_collision_check_margin` is margin in all directions of ego. +- In the forward direction, a margin is added by the braking distance calculated from the current speed and maximum deceleration. The maximum distance is The maximum value of the distance is suppressed by the `object_recognition_collision_check_max_extra_stopping_margin` +- In curves, the lateral margin is larger than in straight lines.This is because curves are more prone to control errors or to fear when close to objects (The maximum value is limited by `object_recognition_collision_check_max_extra_stopping_margin`, although it has no basis.) + +![collision_check_margin](./images/goal_planner-collision_check_margin.drawio.svg) + +Then there is the concept of soft and hard margins. Although not currently parameterized, if a collision-free path can be generated by a margin several times larger than `object_recognition_collision_check_margin`, then the priority is higher. + +#### Parameters for object recognition based collision check + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | +| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | + +## **safety check** + +Perform safety checks on moving objects. If the object is determined to be dangerous, no path decision is made and no approval is given, + +- path decision is not made and approval is not granted. +- After approval, the ego vehicle stops under deceleration and jerk constraints. + +This module has two methods of safety check, `RSS` and `integral_predicted_polygon`. + +`RSS` method is a method commonly used by other behavior path planner modules, see [RSS based safety check utils explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md). + +`integral_predicted_polygon` is a more safety-oriented method. This method is implemented because speeds during pull over are lower than during driving, and fewer objects travel along the edge of the lane. (It is sometimes too reactive and may be less available.) +This method integrates the footprints of egos and objects at a given time and checks for collisions between them. + +![safety_check](./images/goal_planner-safety_check.drawio.svg) + +In addition, the safety check has a time hysteresis, and if the path is judged "safe" for a certain period of time(`keep_unsafe_time`), it is finally treated as "safe". + +```txt + 　　 ==== is_safe + 　　 ---- current_is_safe + 　　 is_safe + 　　 ^ + 　　 | + 　　 | time + 　　 1 +--+ +---+ +---========= +--+ + 　　 | | | | | | | | + 　　 | | | | | | | | + 　　 | | | | | | | | + 　　 | | | | | | | | + 　　 0 =========================-------==========--> t +``` + +### Parameters for safety check + +| Name | Unit | Type | Description | Default value | +| :----------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | +| enable_safety_check | [-] | bool | flag whether to use safety check | true | +| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | +| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | + +#### Parameters for RSS safety check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------- | :--- | :----- | :-------------------------------------- | :------------ | +| rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | + +#### Parameters for integral_predicted_polygon safety check + +| Name | Unit | Type | Description | Default value | +| :-------------- | :--- | :----- | :------------------------------------- | :------------ | +| forward_margin | [m] | double | forward margin for ego footprint | 1.0 | +| backward_margin | [m] | double | backward margin for ego footprint | 1.0 | +| lat_margin | [m] | double | lateral margin for ego footprint | 1.0 | +| time_horizon | [s] | double | Time width to integrate each footprint | 10.0 | + +## **path deciding** + +When `decide_path_distance` closer to the start of the pull over, if it is collision-free at that time and safe for the predicted path of the objects, it transitions to DECIDING. If it is safe for a certain period of time, it moves to DECIDED. + +![path_deciding](./images/goal_planner-deciding_path.drawio.svg) + +## Unimplemented parts / limitations + +- Only shift pull over can be executed concurrently with other modules +- Parking in tight spots and securing margins are traded off. A mode is needed to reduce the margin by using a slower speed depending on the situation, but there is no mechanism for dynamic switching of speeds. +- Parking space available depends on visibility of objects, and sometimes parking decisions cannot be made properly. +- Margin to unrecognized objects(Not even unknown objects) depends on the occupancy grid. May get too close to unrecognized ground objects because the objects that are allowed to approach (e.g., grass, leaves) are indistinguishable. + +Unimplemented parts / limitations for freespace parking + +- When a short path is generated, the ego does can not drive with it. +- Complex cases take longer to generate or fail. +- The drivable area is not guaranteed to fit in the parking_lot. diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg new file mode 100644 index 0000000000000..04b53eadaec04 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg @@ -0,0 +1,122 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ collision_check_margin +
+
+
+ + + + + + + + +
+
+
+ extra_stopping_margin(v, max_decel) +
+
+
+ + + + + + + + +
+
+
+ extra_lateral_margin(v, κ) +
+
+
+ + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg new file mode 100644 index 0000000000000..38f3c4917c41c --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg @@ -0,0 +1,121 @@ + + + + + + + + + + + + + +
+
+
+ NOT DECIDED +
+
+
+ + NOT DECIDED + + + + + + + + + + +
+
+
+ DECIDING +
+
+
+ + DECIDING + + + + + + +
+
+
+ DECIDED +
+
+
+ + DECIDED + + + + + + +
+
+
not safe
+
+
+ + not safe + + + + + + +
+
+
+ safe for a certain time +
+
+
+ + safe for a certain ti... + + + + + + +
+
+
safe & close to pull over start point
+
+
+ + safe & close to pull over start poi... + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg new file mode 100644 index 0000000000000..67b2f89a99bb0 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg @@ -0,0 +1,585 @@ + + + + + + + + + + + + + + +
+
+
+ margin_from_boundary +
+
+
+ + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + +
+
+
+ + max_lateral_offset + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ forward_goal_search_length +
+
+
+ + + + + + + + + + + +
+
+
+ backward_goal_search_length +
+
+
+ + + + + + + + +
+
+
+ lateral_search_interval +
+
+
+ + + + + + + + +
+
+
+ + goal_search_interval + +
+
+
+ + + + + + + + + + + + + + +
+
+
+
refined goal
+
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg new file mode 100644 index 0000000000000..8bca6f4c7e18c --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg @@ -0,0 +1,466 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + +
+
+
+
refined goal
+
+
+
+ + + + + + + + + + + + + + +
+
+
+
longidudinal distance
+
+
+
+ + + + + + + + +
+
+
+
lateral distance
+
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg new file mode 100644 index 0000000000000..d7dab102a7890 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg @@ -0,0 +1,400 @@ + + + + + + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg new file mode 100644 index 0000000000000..66f021bdf887d --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ longitudinal_margin +
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg new file mode 100644 index 0000000000000..b990f3c75dcdf --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg @@ -0,0 +1,472 @@ + + + + + + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + +
+
+
+ 1 +
+
+
+ + + + + + + + +
+
+
+ 2 +
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg new file mode 100644 index 0000000000000..5c3f7ddcb2a03 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg @@ -0,0 +1,427 @@ + + + + + + + + + + + + + + +
+
+
+ margin_from_boundary +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
original goal
+
+
+
+ + + + + + + + +
+
+
+
refined goal
+
+
+
+ + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg new file mode 100644 index 0000000000000..4fe6313a8da45 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg new file mode 100644 index 0000000000000..aff3293e5a279 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg @@ -0,0 +1,651 @@ + + + + + + + + + + + + + + + +
+
+
+ pull over path candidates +
+
+
+ + + + + + + + + + +
+
+
+ sort paths by priority +
+
+
+ + + + + + + + + + +
+
+
+ + generate goal candidates + +
+
+
+ + + + + + + + + + + + +
+
+
+ select highest priority collision free path +
+
+
+ + + + + + + + +
+
+
run()
+
+
+ + + + + + + + + + +
+
+
+ main thread +
+
+
+ + + + + + + + +
+
+
get
+
+
+ + + + + + + + +
+
+
get
+
+
+ + + + + + + + +
+
+
+ collsion-free path +
+
+
+ + + + + + + + +
+
+
+ collision-free paths +
+
+
+ + + + + + + + +
+
+
+ + collision-free +
+ path is found + +
+
+
+ + + + + + + + +
+
+
+ get +
+
+
+ + + + + + + + + + + + +
+
+
Yes
+
+
+ + + + + + + + + + +
+
+
+ lane path generation thread +
+
+
+ + + + + + + + +
+
+
+ + generate path candidates + +
+
+
+ + + + + + + + + + +
+
+
+ onTimer() +
+
+
+ + + + + + + + +
+
+
+ Trigger: previous module output path  is changed. +
+
+
+ + + + + + + + +
+
+
goal candidates
+
+
+ + + + + + + + + + +
+
+
previous module path
+
+
+ + + + + + + + + + +
+
+
+ freespace path generation thead +
+
+
+ + + + + + + + +
+
+
+ Trigger: ego vehicle is stuck +
+
+
+ + + + + + + + +
+
+
+ + generate freespace paths + +
+
+
+ + + + + + + + + + +
+
+
+ onTimer() +
+
+
+ + + + + + + + +
+
+
freespace path
+
+
+ + + + + + + + + + +
+
+
+ if collision-fee path is found... +
+
+
+ + + + + + + + + + + + +
+
+
No
+
+
+ + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png b/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png new file mode 100644 index 0000000000000..5067e0efc90a8 Binary files /dev/null and b/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png differ diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png b/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png new file mode 100644 index 0000000000000..dbcd3e1055497 Binary files /dev/null and b/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png differ diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png b/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png new file mode 100644 index 0000000000000..4094734cb8996 Binary files /dev/null and b/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png differ