Vehicle get stuck in multi-level parking lot

[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 created the following lanelet2 map representing a multi-level parking lot:

dual_spiral_parking.osm.txt

In the planning simulator, I set vehicle pose and goal so that the vehicle has to drive through the whole "building". However, Autoware always stop right before the first curve:

Video demo:

2026-05-01.154613.mp4

The logs does not tell much about what is going on:

[component_container_mt-33] [INFO 1777618057.671877034] [control.autoware_operation_mode_transition_manager]: Engage unavailable: enable_engage_on_driving is false, and the vehicle is not stationary. (isModeChangeAvailable() at /workspace/src/universe/autoware_universe/control/autoware_operation_mode_transition_manager/src/state.cpp:230)
[component_container_mt-33] [INFO 1777618060.771716487] [control.autoware_operation_mode_transition_manager]: Engage unavailable: enable_engage_on_driving is false, and the vehicle is not stationary. (isModeChangeAvailable() at /workspace/src/universe/autoware_universe/control/autoware_operation_mode_transition_manager/src/state.cpp:230)
[component_container_mt-33] [INFO 1777618063.771958951] [control.autoware_operation_mode_transition_manager]: Engage unavailable: enable_engage_on_driving is false, and the vehicle is not stationary. (isModeChangeAvailable() at /workspace/src/universe/autoware_universe/control/autoware_operation_mode_transition_manager/src/state.cpp:230)
^[[B[component_container_mt-33] [INFO 1777618066.871803203] [control.autoware_operation_mode_transition_manager]: Engage unavailable: enable_engage_on_driving is false, and the vehicle is not stationary. (isModeChangeAvailable() at /workspace/src/universe/autoware_universe/control/autoware_operation_mode_transition_manager/src/state.cpp:230)
[autoware_mrm_handler_node-13] [INFO 1777618068.684115830] [system.mrm_handler]: MRM State changed: NORMAL -> MRM_OPERATING (transitionTo() at /workspace/src/universe/autoware_universe/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp:410)
[autoware_mrm_handler_node-13] [WARN 1777618068.684449952] [system.mrm_handler]: EMERGENCY_STOP is operated. (requestMrmBehavior() at /workspace/src/universe/autoware_universe/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp:300)
[component_container_mt-33] [WARN 1777618068.696096218] [control.vehicle_cmd_gate]: Emergency! (publishControlCommands() at /workspace/src/universe/autoware_universe/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp:594)
[logging_node-15] [WARN 1777618068.706610164] [logging_diag_graph]: The target mode is not available for the following reasons:
[logging_node-15] - /autoware/modes/autonomous ERROR
[logging_node-15]     - /autoware/localization ERROR
[logging_node-15]         - /autoware/localization/state STALE
[logging_node-15]  (on_timer() at /workspace/src/universe/autoware_universe/system/autoware_diagnostic_graph_utils/src/node/logging.cpp:70)
[autoware_mrm_handler_node-13] [INFO 1777618068.784370253] [system.mrm_handler]: MRM State changed: MRM_OPERATING -> NORMAL (transitionTo() at /workspace/src/universe/autoware_universe/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp:410)
[autoware_mrm_handler_node-13] [WARN 1777618068.784871629] [system.mrm_handler]: EMERGENCY_STOP is canceled. (requestMrmBehavior() at /workspace/src/universe/autoware_universe/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp:300)
[logging_node-15] [INFO 1777618068.806539200] [logging_diag_graph]: The target mode is available now. (on_timer() at /workspace/src/universe/autoware_universe/system/autoware_diagnostic_graph_utils/src/node/logging.cpp:94)

However the blue lines supposed to indicate the left and right of the drivable area are suspicious:

You can also clearly see in the video the vehicle stops immediately after a new lanelet from the bottom floor is added to the local path planning.

Expected behavior

Can drive through the map

Actual behavior

Stops before the first curve

Steps to reproduce

dual_spiral_parking.osm.txt

See video

Versions

• Ubuntu 22.04 (docker)
• ros2 humble
• release/1.7.1

Possible causes

having multiple lanelets overlapping vertically may cause some trouble.

Additional context

No response

[VRichardJP]

This looks very similar to: #3368

$ ros2 topic echo --once /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound
markers:
- header:
    stamp:
      sec: 1778462375
      nanosec: 589727384
    frame_id: map
  ns: left_bound
  id: 0
  type: 4
  action: 0
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  scale:
    x: 0.2
    y: 0.2
    z: 0.2
  color:
    r: 0.0
    g: 0.578125
    b: 0.80078125
    a: 0.9990000128746033
  lifetime:
    sec: 0
    nanosec: 500000000
  frame_locked: true
  points:
  - x: 113.37426110715043
    y: 6.0
    z: 5.115088667141948
  - x: 110.7698637001119
    y: 6.118208612821675
    z: 5.447703233584211
  colors: []
  texture_resource: ''
  texture:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ''
    format: ''
    data: []
  uv_coordinates: []
  text: ''
  mesh_resource: ''
  mesh_file:
    filename: ''
    data: []
  mesh_use_embedded_materials: false
- header:
    stamp:
      sec: 1778462375
      nanosec: 589727384
    frame_id: map
  ns: right_bound
  id: 0
  type: 4
  action: 0
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  scale:
    x: 0.2
    y: 0.2
    z: 0.2
  color:
    r: 0.0
    g: 0.578125
    b: 0.80078125
    a: 0.9990000128746033
  lifetime:
    sec: 0
    nanosec: 500000000
  frame_locked: true
  points:
  - x: 113.37426110715043
    y: -1.2134
    z: 4.279636667141948
  - x: 112.33220823640053
    y: -1.2134
    z: 4.279636667141948
  - x: 110.47661122273905
    y: -1.2134
    z: 4.279636667141948
  - x: 108.61431151647452
    y: -1.2134
    z: 4.279636667141948
  - x: 107.51555853349069
    y: -1.2134
    z: 4.982680975981117
  colors: []
  texture_resource: ''
  texture:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ''
    format: ''
    data: []
  uv_coordinates: []
  text: ''
  mesh_resource: ''
  mesh_file:
    filename: ''
    data: []
  mesh_use_embedded_materials: false
---

I added a trap in BehaviorPathPlannerNode::run() when I get path->left_bound.size() == 2 and rebuilt autoware_behavior_path_planner and autoware_behavior_path_planner_common in debug mode. Catching the trap in gdb, I can call planner_manager_->run(planner_data_) manually again and investigate what is going on.

The issue root cause is in:

• BehaviorPathPlannerNode::run
  • PlannerManager::run
    • PlannerManager::generateCombinedDrivableArea
      • utils::generateDrivableArea
        • utils::calcBound
          • utils::postProcess

The problem is that goal_idx is not calculated properly here. Here are the detailed intermediate values for the right bound:

(gdb) p start_idx
$7 = 7
(gdb) p path.points
$8 = std::vector of length 69, capacity 69 = {{point = {pose = {position = {x = 109.00411723023237, y = 2.3933, z = 8.2217799323721152}, orientation = {x = 0, y = -0.059678980860424856, z = 0, w = 0.99821762118460977}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 111.00411723023237, y = 2.3933, z = 7.9817799323721159}, orientation = {x = 0, y = -0.059678980860424856, z = 0, w = 0.99821762118460977}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 113.00411723023237, y = 2.3933, z = 7.7417799323721166}, orientation = {x = 0, y = -0.059678980860425078, z = 0, w = 0.99821762118460977}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 115.00411723023237, y = 2.3933, z = 7.5017799323721164}, orientation = {x = 0, y = -0.059678980860425078, z = 0, w = 0.99821762118460977}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 117.00411723023237, y = 2.3933, z = 7.2617799323721162}, orientation = {x = 0, y = -0.059678980860425078, z = 0, w = 0.99821762118460977}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 119.00411723023237, y = 2.3933, z = 7.021779932372116}, orientation = {x = 0, y = -0.059678980860425078, z = 0, w = 0.99821762118460977}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 121.00411723023237, y = 2.3933, z = 6.7817799323721157}, orientation = {x = 0, y = -0.059678980860425078, z = 0, w = 0.99821762118460977}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 123.00411723023237, y = 2.3933, z = 6.5417799323721155}, orientation = {x = 0, y = -0.059678980860425078, z = 0, w = 0.99821762118460977}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 125.00411723023237, y = 2.3933, z = 6.3017799323721153}, orientation = {x = 0, y = -0.059678980860425078, z = 0, w = 0.99821762118460977}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 127.00411723023237, y = 2.3933, z = 6.0617799323721151}, orientation = {x = 0, y = -0.059678980860425432, z = 0, w = 0.99821762118460966}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {26}}, {point = {pose = {position = {x = 127.51894999999999, y = 2.3933, z = 6}, orientation = {x = 0, y = 0, z = 0, w = 1}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 2, capacity 2 = {26, 27}}, {point = {pose = {position = {x = 129.00411723023237, y = 2.3933, z = 6}, orientation = {x = 0, y = 0, z = 0, w = 1}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {27}}, {point = {pose = {position = {x = 131.00411723023237, y = 2.3933, z = 6}, orientation = {x = 0, y = 0, z = 0, w = 1}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {27}}, {point = {pose = {position = {x = 133.00411723023237, y = 2.3933, z = 6}, orientation = {x = 0, y = 0, z = 0, w = 1}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {27}}, {point = {pose = {position = {x = 135.00411723023237, y = 2.3933, z = 6}, orientation = {x = 0, y = 0, z = 0, w = 1}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {27}}, {point = {pose = {position = {x = 137.00411723023237, y = 2.3933, z = 6}, orientation = {x = 0, y = 0, z = 0, w = 1}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {27}}, {point = {pose = {position = {x = 139.00411723023237, y = 2.3933, z = 6}, orientation = {x = 0, y = 0, z = 0.02100540846764571, w = 0.99977936206700491}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {27}}, {point = {pose = {position = {x = 140.99598317731909, y = 2.4770353453204597, z = 6}, orientation = {x = 0, y = 0, z = 0.11177320289204934, w = 0.99373374256651503}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {27}}, {point = {pose = {position = {x = 142.95659014806353, y = 2.9237370818027948, z = 6}, orientation = {x = 0, y = 0, z = 0.16290603663162248, w = 0.9866415880292988}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {27}}, {point = {pose = {position = {x = 144.8335500670712, y = 3.5609239418476117, z = 6}, orientation = {x = 0, y = 0, z = 0.2686386250346256, w = 0.96324103377062686}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {27}}, {point = {pose = {position = {x = 145.84025, y = 4.1698000000000004, z = 6}, orientation = {x = 0, y = 0, z = 0.35551253439351427, w = 0.93467151336129872}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 2, capacity 2 = {27, 28}}, {point = {pose = {position = {x = 146.51196766139796, y = 4.7672220190973045, z = 6}, orientation = {x = 0, y = 0, z = 0.48505972596083607, w = 0.87448102452300169}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 147.56921850712254, y = 6.4613324398825007, z = 6}, orientation = {x = 0, y = 0, z = 0.60554157809792419, w = 0.79581366989683933}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 148.10892677898337, y = 8.4121699442571298, z = 6}, orientation = {x = 0, y = 0, z = 0.67582904776202191, w = 0.7370584089480825}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 148.28109999999998, y = 10.394915849077154, z = 6}, orientation = {x = 0, y = 0, z = 0.70710678118654746, w = 0.70710678118654757}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 148.28109999999998, y = 12.394915849077151, z = 6}, orientation = {x = 0, y = 0, z = 0.70710678118654746, w = 0.70710678118654757}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 148.28109999999998, y = 14.394915849077153, z = 6}, orientation = {x = 0, y = 0, z = 0.70710678118654746, w = 0.70710678118654757}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 148.28109999999998, y = 16.394915849077154, z = 6}, orientation = {x = 0, y = 0, z = 0.70710678118654746, w = 0.70710678118654757}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 148.28109999999998, y = 18.394915849077158, z = 6}, orientation = {x = 0, y = 0, z = 0.70805246064216842, w = 0.7061598352912537}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 148.27574841811145, y = 20.394323537155365, z = 6}, orientation = {x = 0, y = 0, z = 0.76296002079590497, w = 0.64644567186045288}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 147.94827682482156, y = 22.361403470191725, z = 6}, orientation = {x = 0, y = 0, z = 0.80587556173189723, w = 0.59208494238858922}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 147.34682233738175, y = 24.281845581015578, z = 6}, orientation = {x = 0, y = 0, z = 0.87844162950793359, w = 0.47784966626277603}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 146.25796565899157, y = 25.964324781502071, z = 6}, orientation = {x = 0, y = 0, z = 0.92689814576111207, w = 0.37531297257703777}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {28}}, {point = {pose = {position = {x = 145.9984, y = 26.21575, z = 6}, orientation = {x = 0, y = 0, z = 0.95480346697080942, w = 0.29723784997964575}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 2, capacity 2 = {28, 29}}, {point = {pose = {position = {x = 144.55451571469027, y = 27.211207508342753, z = 6}, orientation = {x = 0, y = 0, z = 0.98948122774082126, w = 0.14466132844861171}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 142.66354070410523, y = 27.776201737503847, z = 6}, orientation = {x = 0, y = 0, z = 0.99920272837859458, w = 0.039923772376523302}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 140.66829280899887, y = 27.935899452060809, z = 6}, orientation = {x = 0, y = 0, z = 0.99985377304854839, w = 0.017100658483867347}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 138.6704371727887, y = 28.004258735301278, z = 6}, orientation = {x = 0, y = 0, z = 0.99999999885679891, w = 4.7816338391285436e-05}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 136.67044402714544, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 134.67044402714544, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 132.67044402714544, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 130.67044402714544, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 128.67044402714544, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 126.67044402714542, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 124.67044402714542, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 122.67044402714542, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 120.67044402714542, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 118.67044402714542, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 116.67044402714542, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 114.67044402714542, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = 1, w = 6.123233995736766e-17}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 112.67044402714542, y = 28.004449999999999, z = 6}, orientation = {x = 0, y = 0, z = -0.99999956552714497, w = 0.00093217247398963681}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 110.67057850547157, y = 28.000721555958304, z = 6}, orientation = {x = 0, y = 0, z = -0.99982534735816275, w = 0.018688894566803897}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 108.67266310609976, y = 27.926004744708848, z = 6}, orientation = {x = 0, y = 0, z = -0.99848185199482287, w = 0.055081677870129381}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 106.69314745262547, y = 27.706936417479277, z = 6}, orientation = {x = 0, y = 0, z = -0.97458228617274023, w = 0.22402983613419672}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 104.90588080868466, y = 26.839407404677058, z = 6}, orientation = {x = 0, y = 0, z = -0.94416244133258753, w = 0.32948032471892486}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {29}}, {point = {pose = {position = {x = 103.90475000000001, y = 26.043799999999997, z = 6}, orientation = {x = 0, y = 0, z = -0.92746685728381373, w = 0.37390537391175033}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 2, capacity 2 = {29, 30}}, {point = {pose = {position = {x = 103.29362507766459, y = 25.455427051461637, z = 6}, orientation = {x = 0, y = 0, z = -0.88178083446532174, w = 0.47165936858037799}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 102.19140620595205, y = 23.803711016711624, z = 6}, orientation = {x = 0, y = 0, z = -0.7918116560160352, w = 0.61076534069734512}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 101.67976319917639, y = 21.854867761022284, z = 6}, orientation = {x = 0, y = 0, z = -0.7357117092863158, w = 0.67729482562544907}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 101.5153, y = 19.869222790930017, z = 6}, orientation = {x = 0, y = 0, z = -0.70710678118654746, w = 0.70710678118654757}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 101.5153, y = 17.869222790930017, z = 6}, orientation = {x = 0, y = 0, z = -0.70710678118654746, w = 0.70710678118654757}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 101.5153, y = 15.869222790930017, z = 6}, orientation = {x = 0, y = 0, z = -0.70710678118654746, w = 0.70710678118654757}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 101.5153, y = 13.869222790930017, z = 6}, orientation = {x = 0, y = 0, z = -0.70710678118654746, w = 0.70710678118654757}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 101.5153, y = 11.869222790930017, z = 6}, orientation = {x = 0, y = 0, z = -0.70710678118654746, w = 0.70710678118654757}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 101.5153, y = 9.8692227909300172, z = 6}, orientation = {x = 0, y = 0, z = -0.63706700741571409, w = 0.77080842500739866}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 101.88896371324383, y = 7.9202205637213519, z = 6}, orientation = {x = 0, y = 0, z = -0.57472035163497825, w = 0.8183498746969825}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 102.57807741036173, y = 6.0103077202259385, z = 6}, orientation = {x = 0, y = 0, z = -0.51349596753059834, w = 0.85809200633138094}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 103.51350588953746, y = 4.2661830389393263, z = 6}, orientation = {x = 0, y = 0, z = -0.37553414755833131, w = 0.92680855845079324}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 1, capacity 1 = {30}}, {point = {pose = {position = {x = 104.0527, y = 3.7433999999999981, z = 6}, orientation = {x = -0.019503309311052531, y = -0.068467588099924356, z = -0.27326155538937863, w = 0.95930179435455754}}, longitudinal_velocity_mps = 2.77777767, lateral_velocity_mps = 0, heading_rate_rps = 0, is_final = false}, lane_ids = std::vector of length 2, capacity 2 = {30, 31}}}
(gdb) p tmp_bound
$9 = std::vector of length 33, capacity 33 = {{x = 100, y = 30, z = 9}, {x = 97.030600000000007, y = 25.582100000000001, z = 9}, {x = 97.030600000000007, y = 20.582100000000001, z = 9}, {x = 97.030600000000007, y = 15.582100000000001, z = 9}, {x = 97.030600000000007, y = 10.582100000000001, z = 9}, {x = 97.030600000000007, y = 5.5820999999999996, z = 9}, {x = 100, y = 0, z = 9}, {x = 104.03789999999999, y = -1.2134, z = 8.4000000000000004}, {x = 109.03789999999999, y = -1.2134, z = 7.7999999999999998}, {x = 114.03789999999999, y = -1.2134, z = 7.2000000000000002}, {x = 119.03789999999999, y = -1.2134, z = 6.5999999999999996}, {x = 124.03789999999999, y = -1.2134, z = 6}, {x = 129.03790000000001, y = -1.2134, z = 6}, {x = 134.03790000000001, y = -1.2134, z = 6}, {x = 139.03790000000001, y = -1.2134, z = 6}, {x = 144.03790000000001, y = -1.2134, z = 6}, {x = 149.33959999999999, y = 0.67679999999999996, z = 6}, {x = 152.56219999999999, y = 5.0235000000000003, z = 6}, {x = 152.56219999999999, y = 10.0235, z = 6}, {x = 152.56219999999999, y = 15.0235, z = 6}, {x = 152.56219999999999, y = 20.023499999999999, z = 6}, {x = 152.56219999999999, y = 25.023499999999999, z = 6}, {x = 149.60820000000001, y = 29.837900000000001, z = 6}, {x = 144.691, y = 32.008899999999997, z = 6}, {x = 139.691, y = 32.008899999999997, z = 6}, {x = 134.691, y = 32.008899999999997, z = 6}, {x = 129.691, y = 32.008899999999997, z = 6}, {x = 124.691, y = 32.008899999999997, z = 6}, {x = 119.691, y = 32.008899999999997, z = 6}, {x = 114.691, y = 32.008899999999997, z = 6}, {x = 109.691, y = 32.008899999999997, z = 6}, {x = 104.691, y = 32.008899999999997, z = 6}, {x = 100, y = 30, z = 6}}
(gdb) p goal_pose
$10 = {position = {x = 104.0527, y = 3.7433999999999981, z = 6}, orientation = {x = -0.019503309311052531, y = -0.068467588099924356, z = -0.27326155538937863, w = 0.95930179435455754}}
(gdb) p goal_start_idx
$11 = 6
(gdb) p goal_point
$12 = {x = 107.51555853349069, y = -1.2134, z = 7.9826809759811166}
(gdb) p p_tmp
$13 = {position = {x = 107.51555853349069, y = -1.2134, z = 7.9826809759811166}, orientation = {x = -0.019503309311052531, y = -0.068467588099924356, z = -0.27326155538937863, w = 0.95930179435455754}}
(gdb) p goal_idx
$14 = 7

As you can see, both path and tmp_bound are long enough, but start_idx and goal_idx are both 7. Another weird thing is that goal_pose is {x = 104.0527, y = 3.7433999999999981, z = 6} but goal_point is {x = 107.51555853349069, y = -1.2134, z = 7.9826809759811166}

[VRichardJP]

A simple but hacky fix is to reduce forward_path_length so that the planner never see any loop.