obstacle_stop_planner slow down planner does not work?

[VRichardJP]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

I have modified launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml like so:

enable_slow_down: True
slow_down_planner:
  detection_area:
    lateral_margin: 10.0  

If I am not wrong, with these parameters, autoware is supposed to slow down when an obstacle is found within 10 meters of the path. But both IRL and in simulation, the slow down planner seems to have no effect.

Expected behavior

The vehicle slows down when it is close to an obstacle?

Actual behavior

No slow down

Steps to reproduce

1. update obstacle_stop_planner parameters as indicated
2. Run the planning simulator and add a dummy pedestrian close to the vehicle path.

Versions

No response

Possible causes

No response

Additional context

No response

[takayuki5168]

@satoshi-ota

[VRichardJP]

I have found that motion_planning.launch.py overrides the enable_slow_down value to false (or at least in my work tree). So changing the value in the yaml file has no effect.

On top of that, it seems that even when consider_constraints is false in the yaml, the values min_slow_down_velocity/max_slow_down_velocity seems not to be used: I had them both set to 20.0 (=72km/h) but during slow down sections my vehicle speed was limited to 5km/h (so the slow_down_velocity parameter. I changed the latter to 20.0 as well and then the speed was not limited anymore.

[satoshi-ota]

@VRichardJP Thanks for bug report. Could you paste whole parameters which you modified ?

it seems that even when consider_constraints is false in the yaml, the values min_slow_down_velocity/max_slow_down_velocity seems not to be used

These params are used only when consider_constraints is false. These parameters represent the upper and lower limits of the target slow down speed, and if you set the same values the vehicle will always decelerate to the same target speed. (See docs) The vehicle doesn't slow down even if you set 5km/h both of min_slow_down_velocity/max_slow_down_velocity and set false consider_constraints, something is wrong.

[VRichardJP]

I have reproduced the second bug with the planning simulator:

As you can see the slow down planner is running and speed is limited to 5km/h

However I did the following changes:

• I commented out in motion_planning.launch.py:
  

  autoware.universe/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

  Line 212 in ef7dcda

  {"enable_slow_down": False},

• Modified the default parameters of obstacle_stop_planner.param.yaml to:

    # ...
    enable_slow_down: True                  # whether to use slow down planner [-]

    slow_down_planner:
      # ...

      # params for detection area
      detection_area:
        lateral_margin: 10.0                  # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]

      # params for velocity
      target_velocity:
        max_slow_down_velocity: 20.0         # max slow down velocity (use this param if consider_constraints is False)[m/s]
        min_slow_down_velocity: 20.0         # min slow down velocity (use this param if consider_constraints is False)[m/s]
        slow_down_velocity: 1.38             # target slow down velocity (use this param if consider_constraints is True)[m/s]

      # ...

      # others
      consider_constraints: False                 # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-]
      
      # ...

So the slow down planner should basically not slow down. The speed limit is no more limited to 5km/h when slow_down_velocity is changed to 20.0

[satoshi-ota]

There is possibility the value of slow_down_velocity is always used as the target deceleration speed 🤔
I'll check the code in obstacle_stop_planner.

[satoshi-ota]

@VRichardJP I could reproduce the improper behavior in my local env. And I fix that in #2276, so could you check that PR ?

I have confirmed that the vehicle don't slow down with following parameters.

simplescreenrecorder-2022-11-13_11.54.49.mp4

/**:
  ros__parameters:
    hunting_threshold: 0.5                   # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]
    lowpass_gain: 0.9                        # gain parameter for low pass filter [-]
    max_velocity: 20.0                       # max velocity [m/s]
    enable_slow_down: False                  # whether to use slow down planner [-]

    stop_planner:
      # params for stop position
      stop_position:
        max_longitudinal_margin: 5.0         # stop margin distance from obstacle on the path [m]
        min_longitudinal_margin: 5.0         # stop margin distance when any other stop point is inserted in stop margin [m]
        hold_stop_margin_distance: 0.0       # the ego keeps stopping if the ego is in this margin [m]

      # params for detection area
      detection_area:
        lateral_margin: 0.0                  # margin of vehicle footprint [m]
        step_length: 1.0                     # step length for pointcloud search range [m]
        extend_distance: 0.0                 # extend trajectory to consider after goal obstacle in the extend_distance [m]


    slow_down_planner:
      # params for slow down section
      slow_down_section:
        longitudinal_forward_margin: 5.0     # margin distance from slow down point to vehicle front [m]
        longitudinal_backward_margin: 0.0    # margin distance from slow down point to vehicle rear [m]
        longitudinal_margin_span: -0.1       # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s]
        min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s]

      # params for detection area
      detection_area:
        lateral_margin: 10.0                  # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]

      # params for velocity
      target_velocity:
        max_slow_down_velocity: 20.0         # max slow down velocity (use this param if consider_constraints is False)[m/s]
        min_slow_down_velocity: 20.0         # min slow down velocity (use this param if consider_constraints is False)[m/s]
        slow_down_velocity: 1.38             # target slow down velocity (use this param if consider_constraints is True)[m/s]

      # params for deceleration constraints (use this param if consider_constraints is True)
      constraints:
        jerk_min_slow_down: -0.6             # min slow down jerk constraint [m/sss]
        jerk_span: -0.01                     # fineness param for planning deceleration jerk [m/sss]
        jerk_start: -0.1                     # init jerk used for deceleration planning [m/sss]

      # others
      consider_constraints: False                 # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-]
      velocity_threshold_decel_complete: 0.2      # use for judge whether the ego velocity converges the target slow down velocity [m/s]
      acceleration_threshold_decel_complete: 0.1  # use for judge whether the ego velocity converges the target slow down velocity [m/ss]