<commit_before>/******************************************************************************
 * Copyright 2017 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.
 *****************************************************************************/

/**
 * @file speed_optimizer.cc
 **/

#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/speed_limit.h"
#include "modules/planning/tasks/speed_optimizer.h"

namespace apollo {
namespace planning {

using apollo::planning_internal::StGraphBoundaryDebug;
using apollo::planning_internal::STGraphDebug;

SpeedOptimizer::SpeedOptimizer(const std::string& name) : Task(name) {}

apollo::common::Status SpeedOptimizer::Execute(
    Frame* frame, ReferenceLineInfo* reference_line_info) {
  Task::Execute(frame, reference_line_info);

  auto ret = Process(
      reference_line_info->AdcSlBoundary(), reference_line_info->path_data(),
      frame->PlanningStartPoint(), reference_line_info->reference_line(),
      reference_line_info->path_decision(),
      reference_line_info->mutable_speed_data());

  if (!ret.ok() && FLAGS_enable_slowdown_profile_generator &&
      frame->PlanningStartPoint().v() < FLAGS_slowdown_speed_threshold) {
    *reference_line_info->mutable_speed_data() =
        GenerateStopProfile(frame->PlanningStartPoint().v());
  }
  RecordDebugInfo(reference_line_info->speed_data());
  return ret;
}

SpeedData SpeedOptimizer::GenerateStopProfile(const double init_speed) const {
  AERROR << "Slowing down the car.";
  SpeedData speed_data;

  const double min_acc = FLAGS_slowdown_profile_deceleration;
  const size_t max_t = 3.0;
  const double unit_t = 0.02;

  double pre_s = 0.0;
  for (double t = 0.0; t < max_t; t += unit_t) {
    const double s = std::fmax(pre_s, init_speed * t + 0.5 * min_acc * t * t);
    const double v = std::fmax(0.0, init_speed + min_acc * t);
    speed_data.AppendSpeedPoint(s, t, v, min_acc, 0.0);
    pre_s = s;
  }
  return speed_data;
}

void SpeedOptimizer::RecordDebugInfo(const SpeedData& speed_data) {
  auto debug = frame_->MutableADCTrajectory()->mutable_debug();
  auto ptr_speed_plan = debug->mutable_planning_data()->add_speed_plan();<commit_msg>Planning: slowdown for all failed speed optimization cases.<commit_after>/******************************************************************************
 * Copyright 2017 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.
 *****************************************************************************/

/**
 * @file speed_optimizer.cc
 **/

#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/speed_limit.h"
#include "modules/planning/tasks/speed_optimizer.h"

namespace apollo {
namespace planning {

using apollo::planning_internal::StGraphBoundaryDebug;
using apollo::planning_internal::STGraphDebug;

SpeedOptimizer::SpeedOptimizer(const std::string& name) : Task(name) {}

apollo::common::Status SpeedOptimizer::Execute(
    Frame* frame, ReferenceLineInfo* reference_line_info) {
  Task::Execute(frame, reference_line_info);

  auto ret = Process(
      reference_line_info->AdcSlBoundary(), reference_line_info->path_data(),
      frame->PlanningStartPoint(), reference_line_info->reference_line(),
      reference_line_info->path_decision(),
      reference_line_info->mutable_speed_data());

  if (!ret.ok() && FLAGS_enable_slowdown_profile_generator) {
    *reference_line_info->mutable_speed_data() =
        GenerateStopProfile(frame->PlanningStartPoint().v());
  }
  RecordDebugInfo(reference_line_info->speed_data());
  return ret;
}

SpeedData SpeedOptimizer::GenerateStopProfile(const double init_speed) const {
  AERROR << "Slowing down the car.";
  SpeedData speed_data;

  double slowdown_decel = FLAGS_slowdown_profile_deceleration;
  if (frame_->PlanningStartPoint().v() > FLAGS_slowdown_speed_threshold) {
    // TODO(all): select the best deceleration for slow down.
    slowdown_decel = FLAGS_slowdown_speed_threshold / 2.0;
  }

  const size_t max_t = 3.0;
  const double unit_t = 0.02;

  double pre_s = 0.0;
  for (double t = 0.0; t < max_t; t += unit_t) {
    const double s =
        std::fmax(pre_s, init_speed * t + 0.5 * slowdown_decel * t * t);
    const double v = std::fmax(0.0, init_speed + slowdown_decel * t);
    speed_data.AppendSpeedPoint(s, t, v, slowdown_decel, 0.0);
    pre_s = s;
  }
  return speed_data;
}

void SpeedOptimizer::RecordDebugInfo(const SpeedData& speed_data) {
  auto debug = frame_->MutableADCTrajectory()->mutable_debug();
  auto ptr_speed_plan = debug->mutable_planning_data()->add_speed_plan();
{'fraction_non_alphanumeric': 0.1593513132381456, 'fraction_numerical': 0.008461131676361713, 'mean_word_length': 5.140692640692641, 'pattern_counts': {'":': 0, '<': 10, '<?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': '5155360', 'n_tokens_mistral': 1749, 'n_tokens_neox': 1677, 'n_words': 427}