<commit_before>/******************************************************************************
 * Copyright 2019 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
 **/
#include <limits>
#include <queue>
#include <utility>
#include <vector>

#include "modules/planning/tasks/optimizers/open_space_trajectory_partition/open_space_trajectory_partition.h"

namespace apollo {
namespace planning {

using common::PathPoint;
using common::Status;
using common::TrajectoryPoint;
using common::math::NormalizeAngle;
using common::time::Clock;

OpenSpaceTrajectoryPartition::OpenSpaceTrajectoryPartition(
    const TaskConfig& config)
    : TrajectoryOptimizer(config) {
  open_space_trajectory_partition_config_ =
      config_.open_space_trajectory_partition_config();
}

void OpenSpaceTrajectoryPartition::Restart() {
  auto* current_gear_status =
      frame_->mutable_open_space_info()->mutable_gear_switch_states();
  current_gear_status->gear_switching_flag = false;
  current_gear_status->gear_shift_period_finished = true;
  current_gear_status->gear_shift_period_started = true;
  current_gear_status->gear_shift_period_time = 0.0;
  current_gear_status->gear_shift_start_time = 0.0;
  current_gear_status->gear_shift_position = canbus::Chassis::GEAR_DRIVE;
}

Status OpenSpaceTrajectoryPartition::Process() {
  const auto& open_space_info = frame_->open_space_info();
  auto open_space_info_ptr = frame_->mutable_open_space_info();
  const auto& trajectory = open_space_info.stitched_trajectory_result();

  auto interpolated_trajectory_result_ptr =
      open_space_info_ptr->mutable_interpolated_trajectory_result();

  InterpolateTrajectory(trajectory, interpolated_trajectory_result_ptr);

  auto paritioned_trajectories_ptr =
      open_space_info_ptr->mutable_paritioned_trajectories();
  double distance_s = 0.0;

  paritioned_trajectories_ptr->emplace_back();
  TrajGearPair* current_trajectory = &(paritioned_trajectories_ptr->back());
  // Set initial gear position for first ADCTrajectory depending on v
  // and check potential edge cases
  constexpr double kepsilon = 1e-8;
  size_t horizon = interpolated_trajectory_result_ptr->size();
  size_t initial_horizon = std::min(
      horizon, static_cast<size_t>(open_space_trajectory_partition_config_
                                       .initial_gear_check_horizon()));
  int direction_flag = 0;
  int init_direction = 0;
  for (size_t i = 0; i < initial_horizon; ++i) {
    if (interpolated_trajectory_result_ptr->at(i).v() > kepsilon) {
      direction_flag++;
      if (init_direction == 0) {
        init_direction++;
      }
    } else if (interpolated_trajectory_result_ptr->at(i).v() < -kepsilon) {
      direction_flag--;
      if (init_direction == 0) {
        init_direction--;
      }
    }
  }

  if (direction_flag > 1) {
    current_trajectory->second = canbus::Chassis::GEAR_DRIVE;
  } else if (direction_flag < -1) {
    current_trajectory->second = canbus::Chassis::GEAR_REVERSE;
  } else {
    if (init_direction > 0) {
      ADEBUG << "initial speed oscillate too frequent around zero";
      current_trajectory->second = canbus::Chassis::GEAR_DRIVE;
    } else if (init_direction < 0) {
      ADEBUG << "initial speed oscillate too "
                "frequent around zero";
      current_trajectory->second = canbus::Chassis::GEAR_REVERSE;
    } else {
      ADEBUG << "Invalid trajectory start! Speed values of initial points are "
                "too small to decide gear";
      current_trajectory->second = canbus::Chassis::GEAR_DRIVE;
    }
  }

  // Partition trajectory points into each trajectory
  for (size_t i = 0; i < horizon; ++i) {
    // shift from GEAR_DRIVE to GEAR_REVERSE if v < 0<commit_msg>Planning: Simple code clean.<commit_after>/******************************************************************************
 * Copyright 2019 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
 **/
#include <limits>
#include <queue>
#include <utility>
#include <vector>

#include "modules/planning/tasks/optimizers/open_space_trajectory_partition/open_space_trajectory_partition.h"

namespace apollo {
namespace planning {

using common::PathPoint;
using common::Status;
using common::TrajectoryPoint;
using common::math::NormalizeAngle;
using common::time::Clock;

OpenSpaceTrajectoryPartition::OpenSpaceTrajectoryPartition(
    const TaskConfig& config)
    : TrajectoryOptimizer(config) {
  open_space_trajectory_partition_config_ =
      config_.open_space_trajectory_partition_config();
}

void OpenSpaceTrajectoryPartition::Restart() {
  auto* current_gear_status =
      frame_->mutable_open_space_info()->mutable_gear_switch_states();
  current_gear_status->gear_switching_flag = false;
  current_gear_status->gear_shift_period_finished = true;
  current_gear_status->gear_shift_period_started = true;
  current_gear_status->gear_shift_period_time = 0.0;
  current_gear_status->gear_shift_start_time = 0.0;
  current_gear_status->gear_shift_position = canbus::Chassis::GEAR_DRIVE;
}

Status OpenSpaceTrajectoryPartition::Process() {
  const auto& open_space_info = frame_->open_space_info();
  auto open_space_info_ptr = frame_->mutable_open_space_info();
  const auto& trajectory = open_space_info.stitched_trajectory_result();

  auto interpolated_trajectory_result_ptr =
      open_space_info_ptr->mutable_interpolated_trajectory_result();

  InterpolateTrajectory(trajectory, interpolated_trajectory_result_ptr);

  auto paritioned_trajectories_ptr =
      open_space_info_ptr->mutable_paritioned_trajectories();
  double distance_s = 0.0;

  paritioned_trajectories_ptr->emplace_back();
  TrajGearPair* current_trajectory = &(paritioned_trajectories_ptr->back());
  // Set initial gear position for first ADCTrajectory depending on v
  // and check potential edge cases
  constexpr double kepsilon = 1e-8;
  size_t horizon = interpolated_trajectory_result_ptr->size();
  size_t initial_horizon = std::min(
      horizon, static_cast<size_t>(open_space_trajectory_partition_config_
                                       .initial_gear_check_horizon()));
  int direction_flag = 0;
  int init_direction = 0;
  for (size_t i = 0; i < initial_horizon; ++i) {
    if (interpolated_trajectory_result_ptr->at(i).v() > kepsilon) {
      direction_flag++;
      if (init_direction == 0) {
        init_direction++;
      }
    } else if (interpolated_trajectory_result_ptr->at(i).v() < -kepsilon) {
      direction_flag--;
      if (init_direction == 0) {
        init_direction--;
      }
    }
  }

  if (direction_flag > 1) {
    current_trajectory->second = canbus::Chassis::GEAR_DRIVE;
  } else if (direction_flag < -1) {
    current_trajectory->second = canbus::Chassis::GEAR_REVERSE;
  } else {
    if (init_direction > 0) {
      ADEBUG << "initial speed oscillate too frequent around zero";
      current_trajectory->second = canbus::Chassis::GEAR_DRIVE;
    } else if (init_direction < 0) {
      ADEBUG << "initial speed oscillate too frequent around zero";
      current_trajectory->second = canbus::Chassis::GEAR_REVERSE;
    } else {
      ADEBUG << "Invalid trajectory start! Speed values of initial points are "
                "too small to decide gear";
      current_trajectory->second = canbus::Chassis::GEAR_DRIVE;
    }
  }

  // Partition trajectory points into each trajectory
  for (size_t i = 0; i < horizon; ++i) {
    // shift from GEAR_DRIVE to GEAR_REVERSE if v < 0
{'fraction_non_alphanumeric': 0.128690036900369, 'fraction_numerical': 0.0062269372693726935, 'mean_word_length': 4.510165184243965, 'pattern_counts': {'":': 0, '<': 37, '<?xml version=': 0, '>': 63, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5665352', 'n_tokens_mistral': 2713, 'n_tokens_neox': 2622, 'n_words': 602}