feat(goal_planner): prioritize goals before objects to avoid (#594)

* feat(goal_planner): extend goal search are

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* feat(goal_planner): prioritize goals before objects to avoid

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml

@@ -12,7 +12,7 @@
         goal_priority: "minimum_weighted_distance" # "minimum_weighted_distance" or "minimum_longitudinal_distance"
         minimum_weighted_distance:
           lateral_weight: 40.0
-        path_priority: "efficient_path" # "efficient_path" or "close_goal"
+        prioritize_goals_before_objects: true
         parking_policy: "left_side" # "left_side" or "right_side"
         forward_goal_search_length: 40.0
         backward_goal_search_length: 20.0
@@ -47,6 +47,8 @@
         decide_path_distance: 10.0
         maximum_deceleration: 1.0
         maximum_jerk: 1.0
+        path_priority: "efficient_path" # "efficient_path" or "close_goal"
+        efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exculde freespace parking)
 
         # shift parking
         shift_parking: