feat(obstacle_cruise_planner): yield function for ocp (#837)

* add params for yield

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* param name change

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* add params

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* refactoring

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* fix typo, tuning

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* update parameters

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* delete unused param

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* set cruise planner as default for testing

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add param for stopped obj speed threshold

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* change back param

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* set default false

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

Author: danielsanchezaran

autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml

@@ -95,7 +95,12 @@
           obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
           ego_obstacle_overlap_time_threshold : 2.0 #  time threshold to decide cut-in obstacle for cruise or stop [s]
           max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
-
+        yield:
+          enable_yield: false
+          lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding
+          max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it
+          max_obstacles_collision_time: 10.0 # how far the blocking obstacle
+          stopped_obstacle_velocity_threshold: 0.5
       slow_down:
         max_lat_margin: 1.1  # lateral margin between obstacle and trajectory band with ego's width
         lat_hysteresis_margin: 0.2