fix(static_obstacle_avoidance): stop position is unstable

[satoshi-ota]

Description

This module compensates object even after the system lost them until the time has passed more than threshold.

However, since the longitudinal distance of compensated object was not updated, the avoidance stop point was not calculated expectedly. 0:30~ in following movie, the stop point moved forward even though the target object kept stopping.

simplescreenrecorder-2024-07-05_10.14.07.mp4

In this PR, I fixed to update longitudinal for compensated objects in following function.

void compensateLostTargetObjects(
  ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now,
  const std::shared_ptr<const PlannerData> & planner_data,
  const std::shared_ptr<AvoidanceParameters> & parameters)
{
  const auto include = [](const auto & objects, const auto & search_id) {
    return std::all_of(objects.begin(), objects.end(), [&search_id](const auto & o) {
      return o.object.object_id != search_id;
    });
  };

  // STEP.1 UPDATE STORED OBJECTS.
  const auto match = [&data](auto & object) {
    const auto & search_id = object.object.object_id;
    const auto same_id_object = std::find_if(
      data.target_objects.begin(), data.target_objects.end(),
      [&search_id](const auto & o) { return o.object.object_id == search_id; });

    // same id object is detected. update registered.
    if (same_id_object != data.target_objects.end()) {
      object = *same_id_object;
      return true;
    }

    const auto similar_pos_obj = std::find_if(
      data.target_objects.begin(), data.target_objects.end(), [&object](const auto & o) {
        constexpr auto POS_THR = 1.5;
        return calcDistance2d(object.getPose(), o.getPose()) < POS_THR;
      });

    // same id object is not detected, but object is found around registered. update registered.
    if (similar_pos_obj != data.target_objects.end()) {
      object = *similar_pos_obj;
      return true;
    }

    // Same ID nor similar position object does not found.
    return false;
  };

  // STEP1-1: REMOVE EXPIRED OBJECTS.
  const auto itr = std::remove_if(
    stored_objects.begin(), stored_objects.end(), [&now, &match, &parameters](auto & o) {
      if (!match(o)) {
        o.lost_time = (now - o.last_seen).seconds();
      } else {
        o.last_seen = now;
        o.lost_time = 0.0;
      }

      return o.lost_time > parameters->object_last_seen_threshold;
    });

  stored_objects.erase(itr, stored_objects.end());

  // STEP1-2: UPDATE STORED OBJECTS IF THERE ARE NEW OBJECTS.
  for (const auto & current_object : data.target_objects) {
    if (!include(stored_objects, current_object.object.object_id)) {
      stored_objects.push_back(current_object);
    }
  }

  // STEP2: COMPENSATE CURRENT TARGET OBJECTS
  const auto is_detected = [&](const auto & object_id) {
    return std::any_of(
      data.target_objects.begin(), data.target_objects.end(),
      [&object_id](const auto & o) { return o.object.object_id == object_id; });
  };

  const auto is_ignored = [&](const auto & object_id) {
    return std::any_of(
      data.other_objects.begin(), data.other_objects.end(),
      [&object_id](const auto & o) { return o.object.object_id == object_id; });
  };

  for (auto & stored_object : stored_objects) {
    if (is_detected(stored_object.object.object_id)) {
      continue;
    }
    if (is_ignored(stored_object.object.object_id)) {
      continue;
    }

    const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
    fillLongitudinalAndLengthByClosestEnvelopeFootprint(
      data.reference_path_rough, ego_pos, stored_object);

    data.target_objects.push_back(stored_object);
  }
}

Related links

Parent Issue:

How was this PR tested?

• Psim + reproducer

simplescreenrecorder-2024-07-08_08.23.27.mp4

• PASS TIER IV INTERNAL SCENARIOS

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[rej55]

LGTM