non-consistent static obstacle projection on hdmap::path across two consective frames causes planning node crash

[jilinzhou]

Describe the bug

1. To reproduce the issue is a bit tricky. But once it is reproduced, it can be consistently reproduced.
2. In our setup, there is only one DESTINATION static obstacle. It is represented as a 2d box internally. The 4 corners of the box is projected onto the reference line in each frame to verify whether it is within the range. Here is the calling sequences: ReferenceLine::GetSLBoundary() --> ReferenceLine::XYToSL() -> mappath.GetProjection()

I added some printout at the end of mappath.GetProjection() as the following:

    AWARN << "path::getProjection s: " << *accumulate_s << " l:" << *lateral
           << " min_index: " << min_index << " num_segments: " << num_segments_
           << " total length: " << accumulated_s_.back();

Here is the output for the last two frames before the planning crashes for this DESTINATION static obstacle:

The frame in which projections are still fine:

W1016 13:02:57.962399    15 reference_line.cc:478] corner: 440752.7832 , 5018675.203
W1016 13:02:57.963398    15 path.cc:615] path::getProjection s: -35.353 l:171.136 min_index: 0 num_segments: 629 total length: 179.883
W1016 13:02:57.963398    15 reference_line.cc:478] corner: 440748.4133 , 5018672.773
W1016 13:02:57.963398    15 path.cc:615] path::getProjection s: -31.8085 l:167.61 min_index: 0 num_segments: 629 total length: 179.883
W1016 13:02:57.963398    15 reference_line.cc:478] corner: 440748.4619 , 5018672.686
W1016 13:02:57.964398    15 path.cc:615] path::getProjection s: -31.7379 l:167.681 min_index: 0 num_segments: 629 total length: 179.883
W1016 13:02:57.964398    15 reference_line.cc:478] corner: 440752.8318 , 5018675.116
W1016 13:02:57.965399    15 path.cc:615] path::getProjection s: -35.2825 l:171.207 min_index: 0 num_segments: 629 total length: 179.883

The 's' projection for 4 corners are (-35.353 , -31.8085, -31.7379, -35.2825).
....

The last frame in which projections are unexpected:

W1016 13:02:58.063395    15 reference_line.cc:478] corner: 440752.7832 , 5018675.203
W1016 13:02:58.063395    15 path.cc:615] path::getProjection s: -35.6943 l:171.135 min_index: 0 num_segments: 629 total length: 179.662
W1016 13:02:58.063395    15 reference_line.cc:478] corner: 440748.4133 , 5018672.773
W1016 13:02:58.064394    15 path.cc:615] path::getProjection s: -32.1497 l:167.609 min_index: 0 num_segments: 629 total length: 179.662
W1016 13:02:58.064394    15 reference_line.cc:478] corner: 440748.4619 , 5018672.686
W1016 13:02:58.065394    15 path.cc:615] path::getProjection s: 173.734 l:170.708 min_index: 574 num_segments: 629 total length: 179.662
W1016 13:02:58.065394    15 reference_line.cc:478] corner: 440752.8318 , 5018675.116
W1016 13:02:58.065394    15 path.cc:615] path::getProjection s: -35.6238 l:171.206 min_index: 0 num_segments: 629 total length: 179.662

The 's' projection for 4 corners are (-35.6943 , -32.1497, 173.734,  -35.6238).
Specially, the 's' projection of the third corner ( 440748.4619 , 5018672.686) is now 173.734 which has a big diff from other three corner. In turn, it causes the following crash at FrenetFramePath::EvaluateByS().
...
W1016 13:02:58.073395    15 path_decider.cc:126] obstacle start s: -35.6943 Id: DEST
W1016 13:02:58.073395    15 frenet_frame_path.cc:64] s: -35.6943 back_s: 67.7514 front_s: 29.7514
F1016 13:02:58.073395    15 frenet_frame_path.cc:67] Check failed: s < points_.back().s() + 1.0e-6 && s > points_.front().s() - 1.0e-6

I think the extra lane segments appended in the last frame's reference line causes the issue. It just happens that one of those appended segments is the closest to the DESTINATION obstacle's third corner.

I have not figured a nice way to fix the issue. Intuitively, it may be better to check collision in between the 2d box as a whole against each LaneSegment instead of checking each corner independently.

[quning18]

@lianglia-apollo

[lianglia-apollo]

The l value are very large (~170). Are you trying to project obstacles near to the reference line?

[jilinzhou]

The l value are very large (~170). Are you trying to project obstacles near to the reference line?

@lianglia-apollo Thanks for the quick reply. I did not modify any logic of the code. You are right. The 'l' value are very large which mean the DEST obstacle is far away from the reference line.

In the following function,

PathObstacle* ReferenceLineInfo::AddObstacle(const Obstacle* obstacle) {
...
  SLBoundary perception_sl;

  if (!reference_line_.GetSLBoundary(obstacle->PerceptionBoundingBox(),
                                     &perception_sl)) {
    AERROR << "Failed to get sl boundary for obstacle: " << obstacle->Id();
    return path_obstacle;
  }
  path_obstacle->SetPerceptionSlBoundary(perception_sl);
...
}

The obstacle's SL boundary is computed by calling ReferenceLine::GetSLBoundary(). The 'l' value is not checked for lateralDecision at this stage. Then later on, in the following function:

bool PathDecider::MakeStaticObstacleDecision(const PathData &path_data, PathDecision *const path_decision) { 
  DCHECK_NOTNULL(path_decision);
  const auto &frenet_path = path_data.frenet_frame_path();
  const auto &frenet_points = frenet_path.points();
  if (frenet_points.empty()) {
    AERROR << "Path is empty.";
    return false;
  }

  const double half_width =
      common::VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;

  const double lateral_radius = half_width + FLAGS_lateral_ignore_buffer;

  const double lateral_stop_radius =
      half_width + FLAGS_static_decision_nudge_l_buffer;

  for (const auto *path_obstacle : path_decision->path_obstacles().Items()) {
    const auto &obstacle = *path_obstacle->obstacle();
    if (!obstacle.IsStatic()) {
      continue;
    }
    if (path_obstacle->HasLongitudinalDecision() &&
        path_obstacle->LongitudinalDecision().has_ignore() &&
        path_obstacle->HasLateralDecision() &&
        path_obstacle->LateralDecision().has_ignore()) {
      continue;
    }
    if (path_obstacle->HasLongitudinalDecision() &&
        path_obstacle->LongitudinalDecision().has_stop()) {
      continue;
    }

    if (path_obstacle->st_boundary().boundary_type() ==
        StBoundary::BoundaryType::KEEP_CLEAR) {
      continue;
    }

    // IGNORE by default
    ObjectDecisionType object_decision;
    object_decision.mutable_ignore();

    const auto &sl_boundary = path_obstacle->PerceptionSLBoundary();

//************************No lateral distance check yet*********************************************
    if (sl_boundary.end_s() < frenet_points.front().s() ||
        sl_boundary.start_s() > frenet_points.back().s()) {
      path_decision->AddLongitudinalDecision("PathDecider/not-in-s",
                                             obstacle.Id(), object_decision);
      path_decision->AddLateralDecision("PathDecider/not-in-s", obstacle.Id(),
                                        object_decision);
      continue;
    }

    // if (sl_boundary.start_s() < 0.0) {
      AWARN << "obstacle start s: " <<sl_boundary.start_s() << " Id: " << path_obstacle->Id();
    // }

//****************************Failed in the following call***********************************************
    const auto frenet_point = frenet_path.EvaluateByS(sl_boundary.start_s());

....

}

The problem is the discontinuity of projection of 2d box onto reference line. I have attached a picture which might explain when this occurs. One corner of DEST box is projected to other side of the reference line (s = 173.743 in our debugging output) while other three corners are projected to the other side of ref line ((-35.6943 , -32.1497, -35.6238).

[lianglia-apollo]

We also noticed this issue. We are fixing it by cutting the reference line if the curvature is too large. The new code will be released soon. We made some modifications on pnc_map to cut a segment of routing lane segment if we found the curvature is greater than some threshold.

[lianglia-apollo]

I am thinking that maybe we don't need to check for DEST obstacle when it is very far away, which will cause some mapping issues. We are able to find the destination from routing, and extract the distance from ego vehicle to the destination point.

[jilinzhou]

@lianglia-apollo : Yes, for DEST obstacle, it can be ruled out using routing info. For general static objects, projecting each corner of 2d box onto reference line might be computationally too expensive. How about build a few layers (coarse to fine) of axis aligned 2d boxes (lane width can be used here) for the reference line and then does the collision detection in between the bounding box of obstacle and the ref line? At the top layer, the reference line is represented a big 2d AABox. If there is no collision in between them, we are done. If there is a collision, check the collision to the 2nd layer AABox of ref line. If there is no collision, we are done again, otherwise go to 3rd layer. Repeat the process till the final decision can be made.

[ImCabbage]

@lianglia-apollo @jilinzhou Could you please tell me what exactly you do to solve this problem since I still have the same issue. My situation is a dynamic moving obstacle. I found the start_s of the obstacle just changed suddenly since the projection of one corner of the obstacle just changes. Logically there is nothing wrong with the projection since the nearest point on the reference line changes. But in reality, it seems a little bit weird that something changed while the obstacle remains.