`obstacle_stop_planner` inserts stop point even when the ego don't have to stop

satoshi-ota commented 1 year ago

Checklist

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

Description

When the ego and other vehicle drive paralelly, obstacle_stop_planner inserts stop point suddenly behind the ego. I guess that this is unexpected behavior.

I used following default config and the stop_lateral_margin is 0.0, so the NPC vehicle seems to be NOT inside detection area for stop judgement in this movie.

/**:
  ros__parameters:
    chattering_threshold: 0.5                # even if the obstacle disappears, the stop judgment continues for chattering_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 [-]
    enable_z_axis_obstacle_filtering: True   # filter obstacles in z axis (height) [-]
    z_axis_filtering_buffer: 0.0             # additional buffer for z axis filtering [m]
    voxel_grid_x: 0.05                       # voxel grid x parameter for filtering pointcloud [m]
    voxel_grid_y: 0.05                       # voxel grid y parameter for filtering pointcloud [m]
    voxel_grid_z: 100000.0                   # voxel grid z parameter for filtering pointcloud [m]
    use_predicted_objects: False            # whether to use predicted objects [-]
    publish_obstacle_polygon: False          # whether to publish obstacle polygon [-]
    predicted_object_filtering_threshold: 1.5 # threshold for filtering predicted objects (valid only publish_obstacle_polygon true) [m]

    stop_planner:
      # params for stop position
      stop_position:
        max_longitudinal_margin: 5.0             # stop margin distance from obstacle on the path [m]
        max_longitudinal_margin_behind_goal: 3.0 # stop margin distance from obstacle behind goal 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 [m]
        vehicle_lateral_margin: 0.0          # margin of vehicle footprint [m]
        pedestrian_lateral_margin: 0.0       # margin of pedestrian footprint [m]
        unknown_lateral_margin: 0.0          # margin of unknown footprint [m]
        step_length: 1.0                     # step length for pointcloud search range [m]
        enable_stop_behind_goal_for_obstacle: True # enable extend trajectory after goal lane for obstacle detection

https://github.com/autowarefoundation/autoware.universe/assets/44889564/86084aec-6289-40fb-bb86-ecbfaa6d3709

Expected behavior

Don't insert stop point in above situation.

Actual behavior

Ego stops suddenly.

https://github.com/autowarefoundation/autoware.universe/assets/44889564/75016bb4-95d2-4d78-992e-ecafcb55bacf

Steps to reproduce

build behavior path planner with new architecture (set COMPILE_WITH_OLD_ARCHITECTURE FALSE)
set ego init pos & goal at straight lane
engage the vehicle
set NPC on adjacent lane

Versions

ubuntu22.04
universe:main (hash:c41ba0638f668bb25053f7f4e61ce0602835e582)
launch:main (hash:be7f530c2ffe43ddb1d156e5b74586c47bc3a173)

Possible causes

When I revert https://github.com/autowarefoundation/autoware.universe/pull/3505, this unexpected behavior doesn't happed. (But I don't know why.)

https://github.com/autowarefoundation/autoware.universe/assets/44889564/6cbab9dc-7c1b-4336-81a7-d2d999de0efa

Additional context

No response

brkay54 commented 1 year ago

@satoshi-ota Thank you for your detailed report 🙏 . I will work on this. Also, if it is not a problem, could you share the map and scenario with me?

satoshi-ota commented 1 year ago

@brkay54 cc @mitsudome-r Thanks for your quick response. I can't share the maps on public place like this issue because the map is TIER IV private data. But we TIER IV & Leodrive have an NDA contract, so I can send you the map & scenario by email.

I would like to send you the data to berkay@leodrive.ai. Is it OK?

brkay54 commented 1 year ago

@satoshi-ota Yes, it is okay, thank you!

satoshi-ota commented 1 year ago

@brkay54 Thanks! I'll email you.

And, let me share another info. This strange behavior seems to be happen only on new architecture. (Unfortunately, I don't know why...)

So, I updated the Steps to reproduce.

NOTE: This is my evaluating branch, and please use it if you need.

satoshi-ota commented 1 year ago

@brkay54 I sent the data now. Please check it :pray:

brkay54 commented 1 year ago

@satoshi-ota I created PR to solve the problem. The problem is that if the enable_stop_behind_goal_for_obstacle param is true, the extendTrajectory function causes NaN positions. So, createOneStepPolygon creates bad polygons and causes false positive collisions.

satoshi-ota commented 1 year ago

@brkay54 Thanks! I'll review your PR!

autowarefoundation / autoware.universe