<commit_before>/******************************************************************************
 * Copyright 2020 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/planning/common/trajectory_evaluator.h"

#include <limits>

#include "absl/strings/str_cat.h"
#include "cyber/common/file.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/pipeline/evaluator_logger.h"

namespace apollo {
namespace planning {

void TrajectoryEvaluator::WriteLog(const std::string& msg) {
  AERROR << msg;
  if (FLAGS_planning_offline_learning) {
    EvaluatorLogger::GetStream() << msg << std::endl;
  }
}

void TrajectoryEvaluator::EvaluateTrajectoryByTime(
    const int frame_num, const std::string& obstacle_id,
    const std::vector<std::pair<double, CommonTrajectoryPointFeature>>&
        trajectory,
    const double start_point_timestamp_sec, const double delta_time,
    std::vector<TrajectoryPointFeature>* evaluated_trajectory) {
  if (trajectory.empty() ||
      fabs(trajectory.front().first - trajectory.back().first) < delta_time) {
    return;
  }

  std::vector<apollo::common::TrajectoryPoint> updated_trajectory;
  for (const auto& tp : trajectory) {
    // CommonTrajectoryPointFeature => common::TrajectoryPoint
    double relative_time = tp.first - start_point_timestamp_sec;
    apollo::common::TrajectoryPoint trajectory_point;
    Convert(tp.second, relative_time, &trajectory_point);
    updated_trajectory.push_back(trajectory_point);
  }

  if (trajectory.front().first > trajectory.back().first) {
    std::reverse(updated_trajectory.begin(), updated_trajectory.end());
  }

  DiscretizedTrajectory discretized_trajectory;
  double last_relative_time = std::numeric_limits<double>::lowest();
  for (const auto& tp : updated_trajectory) {
    // filter out abnormal perception data
    if (tp.relative_time() > last_relative_time) {
      discretized_trajectory.AppendTrajectoryPoint(tp);
    } else {
      const std::string msg = absl::StrCat(
          "SKIP trajectory point: frame_num[", frame_num, "] obstacle_id[",
          obstacle_id, "] last_relative_time[", last_relative_time,
          "] relatice_time[", tp.relative_time(), "] relative_time_diff[",
          tp.relative_time() - last_relative_time, "]");
      WriteLog(msg);
    }
    last_relative_time = tp.relative_time();
  }

  const int low_bound =
      ceil(updated_trajectory.front().relative_time() / delta_time);
  const int high_bound =
      floor(updated_trajectory.back().relative_time() / delta_time);
  ADEBUG << "frame_num[" << frame_num << "] obstacle_id[" << obstacle_id
         << "] low[" << low_bound << "] high[" << high_bound << "]";
  for (int i = low_bound; i <= high_bound; ++i) {
    double timestamp_sec = start_point_timestamp_sec + i * delta_time;
    double relative_time = i * delta_time;
    auto tp = discretized_trajectory.Evaluate(relative_time);

    // common::TrajectoryPoint => TrajectoryPointFeature
    TrajectoryPointFeature trajectory_point;
    Convert(tp, timestamp_sec, &trajectory_point);

    evaluated_trajectory->push_back(trajectory_point);
  }
}

void TrajectoryEvaluator::EvaluateADCTrajectory(
    const double start_point_timestamp_sec, const double delta_time,
    LearningDataFrame* learning_data_frame) {
  std::vector<std::pair<double, CommonTrajectoryPointFeature>> trajectory;
  for (const auto& adc_tp : learning_data_frame->adc_trajectory_point()) {
    trajectory.push_back(
        std::make_pair(adc_tp.timestamp_sec(), adc_tp.trajectory_point()));
  }

  if (trajectory.size() <= 1) {<commit_msg>planning: fixed an issue when evaluate obstacle history trajectory with only single point in history<commit_after>/******************************************************************************
 * Copyright 2020 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/planning/common/trajectory_evaluator.h"

#include <limits>

#include "absl/strings/str_cat.h"
#include "cyber/common/file.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/pipeline/evaluator_logger.h"

namespace apollo {
namespace planning {

void TrajectoryEvaluator::WriteLog(const std::string& msg) {
  AERROR << msg;
  if (FLAGS_planning_offline_learning) {
    EvaluatorLogger::GetStream() << msg << std::endl;
  }
}

void TrajectoryEvaluator::EvaluateTrajectoryByTime(
    const int frame_num, const std::string& obstacle_id,
    const std::vector<std::pair<double, CommonTrajectoryPointFeature>>&
        trajectory,
    const double start_point_timestamp_sec, const double delta_time,
    std::vector<TrajectoryPointFeature>* evaluated_trajectory) {
  if (trajectory.empty() ||
      (fabs(trajectory.front().first - start_point_timestamp_sec) <
          delta_time &&
      fabs(trajectory.back().first - start_point_timestamp_sec) <
          delta_time)) {
    return;
  }

  std::vector<apollo::common::TrajectoryPoint> updated_trajectory;
  for (const auto& tp : trajectory) {
    // CommonTrajectoryPointFeature => common::TrajectoryPoint
    double relative_time = tp.first - start_point_timestamp_sec;
    apollo::common::TrajectoryPoint trajectory_point;
    Convert(tp.second, relative_time, &trajectory_point);
    updated_trajectory.push_back(trajectory_point);
  }

  if (trajectory.front().first > trajectory.back().first) {
    std::reverse(updated_trajectory.begin(), updated_trajectory.end());
  }

  DiscretizedTrajectory discretized_trajectory;
  double last_relative_time = std::numeric_limits<double>::lowest();
  for (const auto& tp : updated_trajectory) {
    // filter out abnormal perception data
    if (tp.relative_time() > last_relative_time) {
      discretized_trajectory.AppendTrajectoryPoint(tp);
    } else {
      const std::string msg = absl::StrCat(
          "SKIP trajectory point: frame_num[", frame_num, "] obstacle_id[",
          obstacle_id, "] last_relative_time[", last_relative_time,
          "] relatice_time[", tp.relative_time(), "] relative_time_diff[",
          tp.relative_time() - last_relative_time, "]");
      WriteLog(msg);
    }
    last_relative_time = tp.relative_time();
  }

  int low_bound = 0;
  int high_bound = 0;
  if (updated_trajectory.size() == 1) {
    const double single_point_relative_time =
        updated_trajectory.front().relative_time();
    if (single_point_relative_time > 0) {
      high_bound = floor(single_point_relative_time / delta_time);
    } else {
      low_bound = ceil(single_point_relative_time / delta_time);
    }
  } else {
    low_bound = ceil(updated_trajectory.front().relative_time() / delta_time);
    high_bound = floor(updated_trajectory.back().relative_time() / delta_time);
  }
  ADEBUG << "frame_num[" << frame_num << "] obstacle_id[" << obstacle_id
         << "] low[" << low_bound << "] high[" << high_bound << "]";
  for (int i = low_bound; i <= high_bound; ++i) {
    double timestamp_sec = start_point_timestamp_sec + i * delta_time;
    double relative_time = i * delta_time;
    auto tp = discretized_trajectory.Evaluate(relative_time);

    // common::TrajectoryPoint => TrajectoryPointFeature
    TrajectoryPointFeature trajectory_point;
    Convert(tp, timestamp_sec, &trajectory_point);

    evaluated_trajectory->push_back(trajectory_point);
  }
}

void TrajectoryEvaluator::EvaluateADCTrajectory(
    const double start_point_timestamp_sec, const double delta_time,
    LearningDataFrame* learning_data_frame) {
  std::vector<std::pair<double, CommonTrajectoryPointFeature>> trajectory;
  for (const auto& adc_tp : learning_data_frame->adc_trajectory_point()) {
    trajectory.push_back(
        std::make_pair(adc_tp.timestamp_sec(), adc_tp.trajectory_point()));
  }

  if (trajectory.size() <= 1) {
{'fraction_non_alphanumeric': 0.14309174512809283, 'fraction_numerical': 0.00240858331508649, 'mean_word_length': 4.738065326633166, 'pattern_counts': {'":': 0, '<': 74, '<?xml version=': 0, '>': 32, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4547363', 'n_tokens_mistral': 2831, 'n_tokens_neox': 2770, 'n_words': 591}