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

[satoshi-ota]

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

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

simplescreenrecorder-2023-05-11_08.49.08.mp4

Expected behavior

Don't insert stop point in above situation.

Actual behavior

Ego stops suddenly.

simplescreenrecorder-2023-05-11_09.12.01.mp4

Steps to reproduce

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

Versions

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

Possible causes

When I revert #3505, this unexpected behavior doesn't happed.
(But I don't know why.)

simplescreenrecorder-2023-05-11_09.19.46.mp4

Additional context

No response

[brkay54]

@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]

@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]

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

[satoshi-ota]

@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]

@brkay54 I sent the data now. Please check it 🙏

[brkay54]

@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]

@brkay54 Thanks! I'll review your PR!