Goal Planner design

Purpose / Role

goal_planner generates a smooth path toward the goal and additionally searches for safe path and goal to execute dynamic pull_over on the road shoulders lanes following the traffic rules.

Design

If goal modification is not allowed, just park at the designated fixed goal using fixed_goal_planner.

If allowed, rough_goal_planner works to park around the vacant spots in the shoulder lanes around the goal by executing pull_over toward left or right side of the road lanes.

@startuml
package goal_planner{

    class GoalPlannerModule {}

    package rough_goal_planner <<Rectangle>>{

        package lane_parking <<Rectangle>>{
            class ShiftPullOver {}
            class GeometricPullOver {}
        }

        package freespace_parking <<Rectangle>>{
            class FreeSpacePullOver {}
        }

        class GoalSearcher {}

        struct GoalCandidates {}
        struct PullOverPath{}

        abstract class PullOverPlannerBase {}
    }

    package fixed_goal_planner <<Rectangle>>{
        abstract class FixedGoalPlannerBase {}
        class DefaultFixedPlanner{}
    }
}


package utils{
    class PathShifter {}

    class GeometricParallelParking {}
}

package freespace_planning_algorithms
 {
    class AstarSearch{}
    class RRTStar{}
}

' goal planner
ShiftPullOver --|> PullOverPlannerBase
GeometricPullOver --|> PullOverPlannerBase
FreeSpacePullOver --|> PullOverPlannerBase
DefaultFixedPlanner --|> FixedGoalPlannerBase

PathShifter --o ShiftPullOver
GeometricParallelParking --o GeometricPullOver
AstarSearch --o FreeSpacePullOver
RRTStar --o FreeSpacePullOver

PullOverPlannerBase --o GoalPlannerModule
FixedGoalPlannerBase --o GoalPlannerModule

PullOverPath --o PullOverPlannerBase

@enduml

trigger condition

fixed_goal_planner

fixed_goal_planner just plans a smooth path to the designated goal. NOTE: this planner does not perform the several features described below, such as “goal search”, “collision check”, “safety check”, etc.

fixed_goal_planner is used when both conditions are met.

• Route is set with allow_goal_modification=false. This is the default.
• The goal is set on road lanes.

If the path given to goal_planner covers the goal, fixed_goal_planner smoothly connects the goal and the path points around the goal within the radius of refine_goal_search_radius_range using spline interpolation.

rough_goal_planner

pull over on road lane

rough_goal_planner is triggered following the behavior_path_planner scene module interface namely through isExecutionRequested function and it returns true when following two conditions are met.

• The distance between the goal and ego get shorter than $\max$(pull_over_minimum_request_length, stop distance with decel and jerk constraints).
• Route is set with allow_goal_modification=true or is on a road_shoulder type lane.
  • We can set this option with SetRoute api service.
  • We support 2D Rough Goal Pose with the key bind r in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz.

finish condition

• The distance to the goal from ego is lower than threshold (default: < 1m).
• Ego is stopped.
  • The speed is lower than threshold (default: < 0.01m/s).

General parameters for goal_planner

Name	Unit	Type	Description	Default value
th_arrived_distance	[m]	double	distance threshold for arrival of path termination	1.0
th_stopped_velocity	[m/s]	double	velocity threshold for arrival of path termination	0.01
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

Goal Search

To execute safe pull over in the presence of parked vehicles and other obstacles, collision free areas are searched within a certain range around the original goal. The selected best goal pose will be published as /planning/scenario_planning/modified_goal.

goal search video

First, the original(designated) goal is provided, and a refined goal pose is obtained so that it is at least margin_from_boundary offset from the edge of the lane.

Second, goal candidates are searched in the interval of [-forward_goal_search_length, backward_goal_search_length] in the longitudinal direction and in the interval of [longitudinal_margin,longitudinal_margin+max_lateral_offset] in the lateral direction centered around the refined goal.

Each goal candidate is prioritized and pull over paths are generated by each planner for each goal candidate. The priority of a goal candidate is determined by a sort policy using several distance metrics from the refined goal.

The minimum_longitudinal_distance policy sorts the goal candidates to assign higher priority to goal with smaller longitudinal distance and then auxiliary to goal with smaller lateral distance, to prioritize goal candidates that are close to the original goal.

The minimum_weighted_distance policy sorts the goal candidates by the weighted sum of lateral distance and longitudinal distance longitudinal_distance + lateral_cost*lateral_distance.

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 original refined goal.

To achieve a goal pose which is easy to start the maneuvering after arrival, the goal candidate pose is aligned so that ego center becomes parallel to the shoulder lane boundary at that pose.

If the footprint in each goal candidate is within object_recognition_collision_check_margin from one of the parked object, or the longitudinal distance to one of the parked objects from that goal candidate is less than longitudinal_margin, 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.

Red goal candidates in the below figure indicate unsafe ones.

Also, if prioritize_goals_before_objects is enabled, the number of objects that need to be avoided before reaching the goal is counted, and the goal candidate with the number are prioritized.

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.

The gray numbers represent objects to avoid, and you can see that the goal in front has a higher priority in this case.

BusStopArea

If the flag use_bus_stop_area is true, the goal search is limited inside the BusStopArea regulatory element polygon. The goal candidates are searched more densely compared to road shoulder parking method, and the goal candidate that keeps the ego footprint inside the BusStopArea is accepted. Refer to BusStopArea spec for more detail.

Parameters for goal search

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	This parameter ensures that the distance between the start of the shoulder lane and the goal is not less than the specified value. It’s used to prevent setting goals too close to the beginning of the shoulder lane, which might lead to unsafe or impractical pull-over maneuvers. Increasing this value will force the system to ignore potential goal positions near the start of the shoulder lane, potentially leading to safer and more comfortable pull-over locations.	0.0
margin_from_boundary	[m]	double	distance margin from edge of the shoulder lane	0.5

Pull Over

Since the path candidates generation is time consuming, goal_planner employs two separate threads to generate path candidates in the background and get latest candidates asynchronously. One is LaneParkingThread which plans path candidates on road shoulder lanes and the other is FreespaceParkingThread which plans on freespace area. The normal process of goal_planner is executed on the main thread.

Although the two threads are running periodically, the primary background process is performed only when following conditions are met in order not to consume computational resource.

• ego has approached the goal within the threshold of pull_over_prepare_length
• upstream module path shape has changed from the one which was sent by the main thread in previous process
• upstream module path shape has changed from the one which was used for path candidates generation in the previous process

LaneParkingThread executes either

• shift based path planning
• arc forward, arc backward path planning
• bezier based path planning

depending on the situation and configuration. If use_bus_stop_area is true and the goal is on a BusStopArea regulatory element and the estimated pull over angle(the difference of pose between the shift start and shift end) is larger than bezier_parking.pull_over_angle_threshold, bezier based path planner works to generate path candidates. Otherwise shift based path planner works. bezier based path planner tends to generate more natural paths on a curved lane than shift based path planner, so it is used if the shift requires a certain amount of pose rotation.

The overall flow is as follows.

The main thread and the each thread communicate by sending request and response respectively. The main thread sends latest main thread data as LaneParkingRequest/FreespaceParkingRequest and each thread sets LaneParkingResponse/FreespaceParkingResponse as the output when it’s finished. The bluish blocks on the flow diagram are the critical section.

While

• there are no path candidates, or
• the threads fail to generate candidates, or
• the main thread cannot nail down that the selected candidate is SAFE against dynamic objects(which means the DecisionState is not still DECIDED)

the main thread inserts a stop pose either at closest_start_pose which is the closest shift start pose among the path candidates, or at the position which is certain distance before the closest goal candidate.

Once the main thread finally selected the best pull over path, goal_planner transits to DECIDED state and it sets SAFE as the RTC status(NOTE: this SAFE means that “a safe pull over path has been finally selected”.)

If there are no path candidates or the selected path is not SAFE and thus the LaneParkingThread causes ego to get stuck, the FreespaceParkingThread is triggered by the stuck detection and it starts generating path candidates using freespace parking algorithms. If a valid freespace path is found and ego is still stuck, the freespace path is used instead. If the selected lane parking pull over path becomes collision-free again in case the blocking parked objects moved, and the path is continuous from current freespace path, lane parking pull over path is selected again.

Name	Unit	Type	Description	Default value
pull_over_minimum_request_length	[m]	double	when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated.	100.0
pull_over_velocity	[m/s]	double	decelerate to this speed by the goal search area	3.0
pull_over_minimum_velocity	[m/s]	double	speed of pull_over after stopping once. this prevents excessive acceleration.	1.38
decide_path_distance	[m]	double	decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed	10.0
maximum_deceleration	[m/s2]	double	maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly	1.0
path_priority	[-]	string	In case efficient_path use a goal that can generate an efficient path which is set in efficient_path_order. In case close_goal use the closest goal to the original one.	efficient_path
efficient_path_order	[-]	string	efficient order of pull over planner along lanes excluding freespace pull over	[“SHIFT”, “ARC_FORWARD”, “ARC_BACKWARD”]
lane_departure_check_expansion_margin	[m]	double	margin to expand the ego vehicle footprint when doing lane departure checks	0.0

shift parking

Pull over distance is calculated by the speed, lateral deviation, and the lateral jerk. The lateral jerk is searched for among the predetermined minimum and maximum values.

1. Apply uniform offset to centerline of shoulder lane for ensuring margin
2. The interval of shift start and end is shifted by the shift based path planner
3. Combine this path with center line of road lane and the remaining shoulder lane centerline

shift_parking video

Parameters for shift parking

Name	Unit	Type	Description	Default value
enable_shift_parking	[-]	bool	flag whether to enable shift parking	true
shift_sampling_num	[-]	int	Number of samplings in the minimum to maximum range of lateral_jerk	4
maximum_lateral_jerk	[m/s3]	double	maximum lateral jerk	2.0
minimum_lateral_jerk	[m/s3]	double	minimum lateral jerk	0.5
deceleration_interval	[m]	double	distance of deceleration section	15.0
after_shift_straight_distance	[m]	double	straight line distance after pull over end point	1.0

geometric parallel parking

This method generate two arc paths with discontinuous curvature. It stops twice in the middle of the path to do dry steering. There are two path generation methods: forward and backward.

See also [1] for details of the algorithm. There is also a simple python implementation.

Parameters geometric parallel parking

Name	Unit	Type	Description	Default value
arc_path_interval	[m]	double	interval between arc path points	1.0
max_steer_angle_margin_scale	[-]	double	scaling factor applied to the maximum steering angle (max_steer_angle) defined in vehicle_info parameter	0.72

arc forward parking

Generate two forward arc paths.

arc_forward_parking video

Parameters arc forward parking

Name	Unit	Type	Description	Default value
enable_arc_forward_parking	[-]	bool	flag whether to enable arc forward parking	true
after_forward_parking_straight_distance	[m]	double	straight line distance after pull over end point	2.0
forward_parking_velocity	[m/s]	double	velocity when forward parking	1.38
forward_parking_lane_departure_margin	[m/s]	double	lane departure margin for front left corner of ego-vehicle when forward parking	0.0

arc backward parking

Generate two backward arc paths.

.

arc_backward_parking video

Parameters arc backward parking

Name	Unit	Type	Description	Default value
enable_arc_backward_parking	[-]	bool	flag whether to enable arc backward parking	true
after_backward_parking_straight_distance	[m]	double	straight line distance after pull over end point	2.0
backward_parking_velocity	[m/s]	double	velocity when backward parking	-1.38
backward_parking_lane_departure_margin	[m/s]	double	lane departure margin for front right corner of ego-vehicle when backward	0.0

freespace parking

If the vehicle gets stuck with LaneParkingPlanning, FreespaceParkingPlanner is triggered.

To run this feature, you need to set parking_lot to the map, activate_by_scenario of costmap_generator to false and enable_freespace_parking to true

Simultaneous execution with avoidance_module in the flowchart is under development.

Parameters freespace parking

Name	Unit	Type	Description	Default value
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 for other parameters.

bezier parking

shift based path planner tends to generate unnatural path when the shift lane is curved as illustrated below.

bezier based path planner interpolates the shift path start and end pose using tbe bezier curve for a several combination of parameters, to obtain a better result through the later selection process. In the below screenshot the goal is on a BusStopArea and use_bus_stop_area is set to true, so bezier planner is triggered instead. Internally, goalplanner first tries to use _shift planner, and if it turns out that the shift start and end is not parallel, it switches to bezier planner from the next process.

collision check for path generation

To select a safe one from the path candidates, collision is checked against parked objects for each path.

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

collision is checked for each of the path candidates. There are three margins for this purpose.

• 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.)

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_soft_margins	[m]	vector[double]	soft margins for collision check when path generation. It is not strictly the distance between footprints, but the maximum distance when ego and objects are oriented.	[5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
object_recognition_collision_check_hard_margins	[m]	vector[double]	hard margins for collision check when path generation	[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
collision_check_outer_margin_factor	[-]	double	factor to extend the collision check margin from the inside margin to the outside in the curved path	2.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.

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.

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”.

                         ==== is_safe
                         ---- current_is_safe
       is_safe
        ^
        |
        |                   time
      1 +--+    +---+       +---=========   +--+
        |  |    |   |       |           |   |  |
        |  |    |   |       |           |   |  |
        |  |    |   |       |           |   |  |
        |  |    |   |       |           |   |  |
      0 =========================-------==========--> t

Parameters for safety check

Name	Unit	Type	Description	Default value
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
collision_check_yaw_diff_threshold	[rad]	double	Maximum yaw difference between ego and object when executing rss-based collision checking	3.1416

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 ego approached the start of the temporarily selected pull over path within the distance of decide_path_distance, if it is collision-free at that time and safe against dynamic objects, it transitions to DECIDING. And if those conditions hold for a certain period of time, it transitions to DECIDED and the selected path is fixed.

Open

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.

Changelog for package autoware_behavior_path_goal_planner_module

0.44.0 (2025-04-18)

• Merge remote-tracking branch 'origin/main' into humble
• fix(behavior_path_planner): apply THROTTLE to frequent log (#10417)
• feat(start/goal_planner): use common max steer angle parameter from vehicle_info (#10321)
  • fix(autoware_behavior_path_start_planner_module): update parameter name for geometric pull out max steer angle
  • fix(docs): update unit for max_steer_angle_margin_scale in README

  * fix dead link ---------

• feat(goal_planner): expand outer collision check margin (#10294) feat(goal_planner): expand outer collision check margin margin comment update svg
• fix(goal_planner): use precise distance to objects for sorting candidate paths (#10296)
  • fix(goal_planner): use precise distance to objects for sorting candidate paths
  • shortest_distance_from_ego_footprint_to_objects_on_path

  * improve doxygen update docs * fix build ---------

• Contributors: Kosuke Takeuchi, Kyoichi Sugahara, Ryohsuke Mitsudome, Takayuki Murooka

0.43.0 (2025-03-21)

• Merge remote-tracking branch 'origin/main' into chore/bump-version-0.43
• chore: rename from [autoware.universe]{.title-ref} to [autoware_universe]{.title-ref} (#10306)
• fix(goal_planner): ignore use bus_stop_area flag if there are no BusStopArea on the pull over lanes (#10274)
• docs(goal_planner): update README (#10263)
• fix(goal_planner): invert lane boundary of neighbor opposite lanelets when generating departure check lane (#10207)
  • fix(goal_planner): invert lane boundary of neighbor opposite lanelets when generating departure check lane

  * remove unnecessary loop ---------

• Contributors: Hayato Mizushima, Mamoru Sobue, Mert Çolak, Yutaka Kondo

0.42.0 (2025-03-03)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat(autoware_utils): replace autoware_universe_utils with autoware_utils (#10191)
• refactor(goal_planner, QC): remove unused function (#10195) refactor(goal_planner): remove unused function
• refactor(goal_planner): refactor planPullOverAsCandidate (#10114)
• feat(goal_planner): align vehicle center to be parallel to lane boundary (#10118)
• fix(autoware_behavior_path_goal_planner_module): check optional before accessing (#10182) Update shift_pull_over.cpp
• feat(autoware_vehicle_info_utils): replace autoware_universe_utils with autoware_utils (#10167)
• feat(goal_planner): resample path interval for lane departure check accuracy (#10058)
• feat(goal_planner): ensure stop while path candidates are empty (#10101)
• feat!: replace tier4_planning_msgs/PathWithLaneId with autoware_internal_planning_msgs/PathWithLaneId (#10023)
• refactor(goal_planner): fix updateData continuation condition (#10079) refactor(goal_planner): fix updateData contiuation condition
• feat(goal_planner): replace LastApprovalData with the time changed to DECIDED (#10066)
• feat(goal_planner): do not use isActivated() in deciding state transition (#10056)
• refactor(goal_planner): remove enable_safety_check because it is default (#10052)
• refactor(goal_planner): remove use_object_recognition because it is default (#10050)
• fix(goal_planner): fix goal_searcher assert (#10055)
• refactor(goal_planner): refactor goal_searcher and goal_candidates (#10049)
• fix(goal_planner): check usage of bus_stop_area by goal_pose (#10041)
• refactor(goal_planner): make parameters const (#10043)
• Contributors: Fumiya Watanabe, Kosuke Takeuchi, Mamoru Sobue, Ryohsuke Mitsudome, Yukinari Hisaki, 心刚

0.41.2 (2025-02-19)

• chore: bump version to 0.41.1 (#10088)
• Contributors: Ryohsuke Mitsudome

0.41.1 (2025-02-10)

0.41.0 (2025-01-29)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat(goal_planner): introduce bezier based pullover for bus stop area (#10027)
• fix(goal_planner): fix waiting approval path of backward parking (#10015)
• refactor(goal_planner): divide extract_dynamic_object/is_goal_in_lanes util function (#10019)
• fix(start_planner, goal_planner): refactor lane departure checker initialization (#9944)
• feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (#9927) Co-authored-by: Satoshi OTA <<44889564+satoshi-ota@users.noreply.github.com>> Co-authored-by: Ryohsuke Mitsudome <<43976834+mitsudome-r@users.noreply.github.com>> Co-authored-by: satoshi-ota <<satoshi.ota928@gmail.com>>
• fix(goal_planner): fix geometric pull over (#9932)
• test(autoware_behavior_path_start_planner_module): add test helper and implement unit tests for FreespacePullOut (#9832)
  • refactor(autoware_behavior_path_start_planner_module): remove unnecessary time_keeper parameter from pull-out planners
  • refactor(autoware_behavior_path_start_planner_module): remove TimeKeeper parameter from pull-out planners
  • refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling
  • refactor(planner): add planner_data parameter to plan methods in pull out planners

  * refactor(autoware_behavior_path_start_planner_module): remove LaneDepartureChecker dependency from pull-out planners ---------

• feat(goal_planner): update lateral_deviation_thresh from [0.3]{.title-ref} to [0.1]{.title-ref} (#9850)
  • fix(goal_planner): Update lateral_deviation_thresh from 0.3 to 0.1
  • unified hasDeviatedFrom{Last|Current}PreviousModulePath
  • style(pre-commit): autofix
  • hasDeviatedFromPath argument modification

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com>

• feat(goal_planner): cut stop path to goal (#9829)

• refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling (#9791) * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling ---------

• fix(goal_planner): fix usage of last_previous_module_output (#9811)
• fix(behavior_path_planner): add freespace_planning_algorithms dependency (#9800)
• chore(autoware_test_utils): update test map (#9664)
• refactor(goal_planner): divide sort function (#9650)
• perf(goal_planner): remove unnecessary call to setMap on freespace planning (#9644)
• feat(goal_planner): add bezier based pull over planner (#9642)
• feat(goal_planner): divide Planners to isolated threads (#9514)
• Contributors: Fumiya Watanabe, Kazunori-Nakajima, Kosuke Takeuchi, Kyoichi Sugahara, Mamoru Sobue, Takayuki Murooka

0.40.0 (2024-12-12)

• Merge branch 'main' into release-0.40.0
• Revert "chore(package.xml): bump version to 0.39.0 (#9587)" This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5.
• feat(behavior_path_planner): add detail text to virutal wall (#9600)
  • feat(behavior_path_planner): add detail text to virutal wall
  • goal is far
  • pull over start pose is far
  • fix lc build
  • fix build

  * Update planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp ---------

• fix: fix ticket links in CHANGELOG.rst (#9588)
• fix(goal_planner): fix isStopped judgement (#9585)
  • fix(goal_planner): fix isStopped judgement

  * fix typo ---------

• chore(package.xml): bump version to 0.39.0 (#9587)
  • chore(package.xml): bump version to 0.39.0
  • fix: fix ticket links in CHANGELOG.rst

  * fix: remove unnecessary diff ---------Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>

• fix: fix ticket links in CHANGELOG.rst (#9588)
• feat(goal_planner): check opposite lane for lane departure_check (#9460)
  • feat(goal_planner): check opposite lane for lane departure_check

  * refactor getMostInnerLane ---------

• refactor(goal_planner): improve log message and change level (#9562) Co-authored-by: Yukinari Hisaki <<42021302+yhisaki@users.noreply.github.com>>
• fix(cpplint): include what you use - planning (#9570)
• refactor(goal_planner): move PathDecisionController implementation to a different file (#9523) refactor(goal_planner): move decision_state implementation
• refactor(goal_planner): move unnecessary member functions (#9522)
• fix(autoware_freespace_planner, autoware_freespace_planning_algorithms): modify freespace planner to use node clock instead of system clock (#9152)
  • Modified the autoware_freespace_planner and autoware_freespace_planning_algorithms packages to use the node clock instead of rclcpp detached clock. This allows the module to make use of sim time. Previously during simulation the parking trajectory would have system time in trajectory header messages causing downstream issues like non-clearance of trajectory buffers in motion planning based on elapsed time.
  • style(pre-commit): autofix
  • Updated the freespace planner instantiation call in the path planning modules
  • style(pre-commit): autofix
  • Updated tests for the utility functions

  * style(pre-commit): autofix ---------Co-authored-by: Steven Brills <<sbrills@oshkoshcorp.com>> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com>

• fix(goal_planner): fix multiple lane ids of shift pull over (#9360) fix vel
• fix(goal_planner): remove stop reason (#9365)
• 0.39.0
• update changelog
• Merge commit '6a1ddbd08bd' into release-0.39.0
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• fix(goal_planner): use departure_check_lane for path (#9423)
• refactor(goal_planner): rename shoulder_lane to pull_over_lane (#9422)
• feat(goal_planner): do not insert shift end pose on pull over lane to path (#9361)
• feat(goal_planner): remove unnecessary member from ThreadSafeData (#9393)
• feat(goal_planner): move goal_candidates from ThreadSafeData to GoalPlannerData (#9292)
• feat(goal_planner): output velocity factor (#9348)
• refactor(bpp): rework steering factor interface (#9325)
  • refactor(bpp): rework steering factor interface
  • refactor(soa): rework steering factor interface
  • refactor(AbLC): rework steering factor interface
  • refactor(doa): rework steering factor interface
  • refactor(lc): rework steering factor interface
  • refactor(gp): rework steering factor interface
  • refactor(sp): rework steering factor interface
  • refactor(sbp): rework steering factor interface

  * refactor(ss): rework steering factor interface ---------

• refactor(goal_planner): remove reference_goal_pose getter/setter (#9270)
• feat(goal_planner): safety check with only parking path (#9293)
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
  • unify package.xml version to 0.37.0
  • remove system_monitor/CHANGELOG.rst
  • add changelog

  * 0.38.0

• feat(goal_planner): sort candidate path only when num to avoid is different (#9271)
• fix(autoware_behavior_path_goal_planner_module): fix cppcheck unreadVariable (#9192)
• Contributors: Esteve Fernandez, Fumiya Watanabe, Kosuke Takeuchi, M. Fatih Cırıt, Mamoru Sobue, Ryohsuke Mitsudome, Ryuta Kambe, Satoshi OTA, Yutaka Kondo, stevenbrills

0.39.0 (2024-11-25)

• Merge commit '6a1ddbd08bd' into release-0.39.0
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• refactor(goal_planner): remove reference_goal_pose getter/setter (#9270)
• feat(goal_planner): safety check with only parking path (#9293)
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
  • unify package.xml version to 0.37.0
  • remove system_monitor/CHANGELOG.rst
  • add changelog

  * 0.38.0

• feat(goal_planner): sort candidate path only when num to avoid is different (#9271)
• fix(autoware_behavior_path_goal_planner_module): fix cppcheck unreadVariable (#9192)
• Contributors: Esteve Fernandez, Kosuke Takeuchi, Mamoru Sobue, Ryuta Kambe, Yutaka Kondo

0.38.0 (2024-11-08)

• unify package.xml version to 0.37.0
• feat(goal_planner): align vehicle footprint heading parallel to parking side lane boundary (#9159)
• chore(goal_planner): compare sampled/filtered candidate paths on plot (#9140) Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>>
• feat(goal_planner): use vehicle side edge to check isCrossingPossible for pull over execution (#9102)
• feat(autoware_test_utils): move test_map, add launcher for test_map (#9045)
• refactor(goal_planner): move last_previous_module_output_path out of ThreadSafeData (#9075)
• refactor(bpp_common, motion_utils): move path shifter util functions to autoware::motion_utils (#9081)
  • remove unused function
  • mover path shifter utils function to autoware motion utils
  • minor change in license header
  • fix warning message

  * remove header file ---------

• refactor(goal_planner): remove prev_data / last_path_idx_time from ThreadSafeData (#9064) refactor(goal_planner): remove prev_data and last_path_idx_update_time
• refactor(goal_planner): remove lane parking pull over path (#9063)
• refactor(goal_planner): remove modified_goal in ThreadDafeData (#9010)
• refactor(goal planner): hold modified_goal in PullOverPath ,copy modified goal once from background thread (#9006) refactor(goal_planner): save modified_goal_pose in PullOverPlannerBase
• fix(behavior_path_planner_common): swap boolean for filterObjectsByVelocity (#9036) fix filter object by velocity
• fix(goal_planner): fix parking_path curvature and DecidingState transition (#9022)
• refactor(goal_planner): use the PullOverPath, PullOverPathCandidates copied from ThreadData to reduce access (#8994)
• refactor(goal_planner): remove unused header and divide ThreadSafeData to another file (#8990)
• refactor(goal_planner): refactor PullOverPlannseBase to instantiate only valid path (#8983)
• fix(goal_planner): fix freespace planning chattering (#8981)
• feat(goal_planner): use neighboring lane of pull over lane to check goal footprint (#8716) move to utils and add tests
• refactor(goal_planner): remove unnecessary GoalPlannerData member (#8920)
• feat(goal_planner): move PathDecidingStatus to other controller class (#8872)
• chore(planning): consistent parameters with autoware_launch (#8915)
  • chore(planning): consistent parameters with autoware_launch
  • update

  * fix json schema ---------

• fix(goal_planner): fix typo (#8910)
• fix(autoware_behavior_path_goal_planner_module): fix unusedFunction (#8786) fix:unusedFunction
• refactor(goal_planner): reduce call to isSafePath (#8812)
• feat(goal_planner): execute goal planner if previous module path terminal is pull over neighboring lane (#8715)
• feat(goal_planner): dense goal candidate sampling in BusStopArea (#8795)
• fix(autoware_behavior_path_planner): align the parameters with launcher (#8790) parameters in behavior_path_planner aligned
• feat(goal_planner): add getBusStopAreaPolygons (#8794)
• fix(autoware_behavior_path_goal_planner_module): fix unusedFunction (#8775) fix:unusedFunction
• feat(behavior_path_goal planner): add example plot for development (#8772)
• fix(goal_planner): fix time_keeper race (#8780)
• fix(goal_planner): fix object extraction area (#8764)
• fix(goal_planner): fix typo (#8763)
• feat(goal_planner): extend pull over lanes inward to extract objects (#8714)
  • feat(goal_planner): extend pull over lanes inward to extract objects
  • update from review
  • use optionale
  • rename lamda
  • return nullopt

  * Update planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp Co-authored-by: Mamoru Sobue <<mamoru.sobue@tier4.jp>> * pre-commit ---------Co-authored-by: Mamoru Sobue <<mamoru.sobue@tier4.jp>>

• refactor(goal_planner): initialize parameter with free function (#8712)
• fix(bpp): use common steering factor interface for same scene modules (#8675)
• refactor(goal_planner): remove unnecessary member from PreviousPullOverData (#8698)
• refactor(goal_planner): remove unnecessary member from pull_over_planner (#8697)
• refactor(goal_planner): move pull_over_planner directory (#8696)
• fix(goal_planner): fix zero velocity in middle of path (#8563)
  • fix(goal_planner): fix zero velocity in middle of path

  * add comment ---------

• fix(goal_planner): remove time keeper in non main thread (#8610)
• feat(freespace_planning_algorithms): implement option for backward search from goal to start (#8091)
  • refactor freespace planning algorithms
  • fix error
  • use vector instead of map for a-star node graph
  • remove unnecessary parameters
  • precompute average turning radius
  • add threshold for minimum distance between direction changes
  • apply curvature weight and change in curvature weight
  • store total cost instead of heuristic cost
  • fix reverse weight application
  • fix parameter description in README
  • implement edt map to store distance to nearest obstacle for each grid cell
  • use obstacle edt in collision check
  • add cost for distance to obstacle
  • fix formats
  • add missing include
  • refactor functions
  • add missing include
  • implement backward search option
  • precompute number of margin cells to reduce out of range vertices check necessity
  • add reset data function
  • remove unnecessary code
  • add member function set() to AstarNode struct
  • implement adaptive expansion distance
  • remove unnecessary code
  • interpolate nodes with large expansion distance
  • minor refactor
  • fix interpolation for backward search
  • ensure expansion distance is larger than grid cell diagonal
  • compute collision free distance to goal map
  • use obstacle edt when computing collision free distance map
  • minor refactor
  • fix expansion cost function
  • set distance map before setting start node
  • refactor detect collision function
  • use flag instead of enum
  • add missing variable initialization
  • remove declared but undefined function
  • refactor makePlan() function
  • remove bool return statement for void function
  • remove unnecessary checks
  • minor fix
  • refactor computeEDTMap function
  • remove unnecessary code
  • set min and max expansion distance after setting costmap
  • refactor detectCollision function
  • remove unused function
  • change default parameter values
  • add missing last waypoint
  • fix computeEDTMap function
  • rename parameter
  • use linear function for obstacle distance cost
  • fix rrtstar obstacle check
  • add public access function to get distance to nearest obstacle
  • remove redundant return statements
  • check goal pose validity before setting collision free distance map
  • declare variables as const where necessary
  • compare front and back lengths when setting min and max dimension
  • add docstring and citation for computeEDTMap function
  • transform pose to local frame in getDistanceToObstacle funcion
  • update freespace planner parameter schema
  • refactor setPath function
  • fix function setPath

  * minor refactor ---------Co-authored-by: Maxime CLEMENT <<78338830+maxime-clem@users.noreply.github.com>>

• perf(goal_planner): faster path sorting and selection (#8457)
  • perf(goal_planner): faster path sorting and selection

  * path_id_to_rough_margin_map ---------

• refactor(behavior_path_planner): apply clang-tidy check (#7549)
  • goal_planner

  * lane_change ---------Co-authored-by: Shumpei Wakabayashi <<42209144+shmpwk@users.noreply.github.com>>

• perf(goal_planner): reduce unnecessary recursive lock guard (#8465)
  • perf(goal_planner): reduce unnecessary recursive lock guard

  * make set_no_lock private ---------

• fix(turn_signal, lane_change, goal_planner): add optional to tackle lane change turn signal and pull over turn signal (#8463)
  • add optional to tackle LC turn signal and pull over turn signal

  * CPP file should not re-define default value; typo in copying from internal repos ---------

• fix(goal_planner): fix lane departure check not working correctly due to uninitialized variable (#8449)
• fix(autoware_behavior_path_goal_planner_module): fix unreadVariable (#8365) fix:unreadVariable
• feat(behavior_path _planner): divide planner manager modules into dependent slots (#8117)
• perf(goal_planner): reduce processing time (#8195)
  • perf(goal_palnner): reduce processing time
  • add const& return
  • use copy getter

  * pre commit ---------

• fix(start/goal_planner): fix freespace planning error handling (#8246)
• feat(goal_planner): add time keeper (#8194) time keeper
• refactor(freespace_planning_algorithm): refactor and improve astar search (#8068)
  • refactor freespace planning algorithms
  • fix error
  • use vector instead of map for a-star node graph
  • remove unnecessary parameters
  • precompute average turning radius
  • add threshold for minimum distance between direction changes
  • apply curvature weight and change in curvature weight
  • store total cost instead of heuristic cost
  • fix reverse weight application
  • fix parameter description in README
  • fix formats
  • add missing include
  • refactor functions
  • precompute number of margin cells to reduce out of range vertices check necessity
  • add reset data function
  • add member function set() to AstarNode struct
  • remove unnecessary code
  • minor refactor
  • ensure expansion distance is larger than grid cell diagonal
  • compute collision free distance to goal map
  • minor refactor
  • fix expansion cost function
  • set distance map before setting start node
  • minor fix
  • remove unnecessary code
  • change default parameter values
  • rename parameter
  • fix rrtstar obstacle check
  • remove redundant return statements
  • check goal pose validity before setting collision free distance map

  * declare variables as const where necessary ---------

• fix(autoware_behavior_path_goal_planner_module): fix shadowVariable (#7962) fix:shadowVariable
• fix(start/goal_planner): fix addition of duplicate segments in calcBeforeShiftedArcLength (#7902)
  • fix(start/goal_planner): fix addition of duplicate segments in calcBeforeShiftedArcLength

  * Update trajectory.hpp Co-authored-by: Kyoichi Sugahara <<kyoichi.sugahara@tier4.jp>> * Update trajectory.hpp Co-authored-by: Kyoichi Sugahara <<kyoichi.sugahara@tier4.jp>> ---------Co-authored-by: Kyoichi Sugahara <<kyoichi.sugahara@tier4.jp>>

• docs(goal_planner): update parameter description (#7889)
  • docs(goal_planner): update parameter description

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com>

• feat(goal_planner): prioritize pull over path by curvature (#7791) * feat(goal_planner): prioritize pull over path by curvature fix
  • add comment

  * pre commit ---------Co-authored-by: Mamoru Sobue <<mamoru.sobue@tier4.jp>>

• feat(safety_check): filter safety check targe objects by yaw deviation between pose and lane (#7828)
  • fix(safety_check): filter by yaw deviation to check object belongs to lane

  * fix(static_obstacle_avoidance): check yaw only when the object is moving ---------

• feat: add [autoware_]{.title-ref} prefix to [lanelet2_extension]{.title-ref} (#7640)
• feat(start_planner): yaw threshold for rss check (#7657)
  • add param to customize yaw th
  • add param to other modules
  • docs
  • update READMEs with params
  • fix LC README

  * use normalized yaw diff ---------

• fix(autoware_behavior_path_goal_planner_module): fix lateral_offset related warnings (#7624)
• refactor(universe_utils/motion_utils)!: add autoware namespace (#7594)
• refactor(motion_utils)!: add autoware prefix and include dir (#7539) refactor(motion_utils): add autoware prefix and include dir
• feat(autoware_universe_utils)!: rename from tier4_autoware_utils (#7538) Co-authored-by: kosuke55 <<kosuke.tnp@gmail.com>>
• refactor(freespace_planner)!: rename to include/autoware/{package_name} (#7525) refactor(freespace_planner)!: rename to include/autoware/{package_name} refactor(start_planner): make autoware include dir refactor(goal_planner): make autoware include dir sampling planner module fix sampling planner build dynamic_avoidance lc side shift autoware_behavior_path_static_obstacle_avoidance_module autoware_behavior_path_planner_common make behavior_path dir pre-commit fix pre-commit fix build autoware_freespace_planner freespace_planning_algorithms
• refactor(control)!: refactor directory structures of the control checkers (#7524)
  • aeb
  • control_validator
  • lane_departure_checker
  • shift_decider

  * fix

• refactor(behaivor_path_planner)!: rename to include/autoware/{package_name} (#7522)
  • refactor(behavior_path_planner)!: make autoware dir in include
  • refactor(start_planner): make autoware include dir
  • refactor(goal_planner): make autoware include dir
  • sampling planner module
  • fix sampling planner build
  • dynamic_avoidance
  • lc
  • side shift
  • autoware_behavior_path_static_obstacle_avoidance_module
  • autoware_behavior_path_planner_common
  • make behavior_path dir
  • pre-commit
  • fix pre-commit

  * fix build ---------

• Contributors: Fumiya Watanabe, Go Sakayori, Keisuke Shima, Kosuke Takeuchi, Mamoru Sobue, Ryuta Kambe, Satoshi OTA, Takayuki Murooka, Yuki TAKAGI, Yutaka Kondo, Yuxuan Liu, Zhe Shen, danielsanchezaran, kobayu858, mkquda

0.26.0 (2024-04-03)