<commit_before>
/******************************************************************************
 * Copyright 2018 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 "modules/planning/scenarios/side_pass/side_pass_scenario.h"

#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/time/time.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/obstacle_blocking_analyzer.h"
#include "modules/planning/scenarios/side_pass/stage_approach_obstacle.h"
#include "modules/planning/scenarios/side_pass/stage_backup.h"
#include "modules/planning/scenarios/side_pass/stage_detect_safety.h"
#include "modules/planning/scenarios/side_pass/stage_generate_path.h"
#include "modules/planning/scenarios/side_pass/stage_pass_obstacle.h"
#include "modules/planning/scenarios/side_pass/stage_stop_on_wait_point.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

apollo::common::util::Factory<
    ScenarioConfig::StageType, Stage,
    Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
    SidePassScenario::s_stage_factory_;

// The clearance distance from intersection/destination.
// If ADC is too close, then do not enter SIDE_PASS.
constexpr double kClearDistance = 15.0;

void SidePassScenario::RegisterStages() {
  s_stage_factory_.Clear();
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_APPROACH_OBSTACLE,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageApproachObstacle(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_DETECT_SAFETY,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageDetectSafety(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_GENERATE_PATH,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageGeneratePath(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageStopOnWaitPoint(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_PASS_OBSTACLE,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StagePassObstacle(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_BACKUP,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageBackup(config);
      });
}

SidePassScenario::SidePassScenario(const ScenarioConfig& config,
                                   const ScenarioContext* scenario_context)
    : Scenario(config, scenario_context) {
  side_pass_context_.scenario_config_.CopyFrom(config.side_pass_config());

  // TODO(all): to be removed when SidePass obstacle decision impl is ready
  side_pass_context_.front_blocking_obstacle_id_ =
      PlanningContext::GetScenarioInfo()->side_pass_front_blocking_obstacle_id;
}

std::unique_ptr<Stage> SidePassScenario::CreateStage(
    const ScenarioConfig::StageConfig& stage_config) {
  if (s_stage_factory_.Empty()) {
    RegisterStages();
  }
  auto ptr = s_stage_factory_.CreateObjectOrNull(stage_config.stage_type(),
                                                 stage_config);
  if (ptr == nullptr) {
    AERROR << "Failed to create stage for config: "
           << stage_config.DebugString();
    return nullptr;
  }
  ptr->SetContext(&side_pass_context_);
  return ptr;
}

bool SidePassScenario::IsTransferable(const Scenario& current_scenario,
                                      const Frame& frame) {
  // Sanity checks.
  if (frame.reference_line_info().size() > 1) {
    return false;
  }

  std::string front_blocking_obstacle_id =
      PlanningContext::GetScenarioInfo()->side_pass_front_blocking_obstacle_id;
  front_blocking_obstacle_id += "_0";  // obstacle id for static obstacle

  if (current_scenario.scenario_type() == ScenarioConfig::SIDE_PASS) {
    // Check if the blocking obstacle is still static.
    // If not, then switch to LANE_FOLLOW.
    const auto front_blocking_obstacle =
        const_cast<Frame&>(frame).Find(front_blocking_obstacle_id);
    if (!front_blocking_obstacle) {
      ADEBUG << "Obstacle " << front_blocking_obstacle_id
            << " not exist any more. Change scenario to default scenario.";
      return false;
    }
    const auto& reference_line_info = frame.reference_line_info().front();
    const double adc_front_edge_s =
        reference_line_info.AdcSlBoundary().end_s();
    const double distance =
        front_blocking_obstacle->PerceptionSLBoundary().start_s() -
        adc_front_edge_s;

    constexpr double kSidePassMaxDistance = 10.0;<commit_msg>planning: fix InTransferable in sidepass.<commit_after>
/******************************************************************************
 * Copyright 2018 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 "modules/planning/scenarios/side_pass/side_pass_scenario.h"

#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/time/time.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/obstacle_blocking_analyzer.h"
#include "modules/planning/scenarios/side_pass/stage_approach_obstacle.h"
#include "modules/planning/scenarios/side_pass/stage_backup.h"
#include "modules/planning/scenarios/side_pass/stage_detect_safety.h"
#include "modules/planning/scenarios/side_pass/stage_generate_path.h"
#include "modules/planning/scenarios/side_pass/stage_pass_obstacle.h"
#include "modules/planning/scenarios/side_pass/stage_stop_on_wait_point.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

apollo::common::util::Factory<
    ScenarioConfig::StageType, Stage,
    Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
    SidePassScenario::s_stage_factory_;

// The clearance distance from intersection/destination.
// If ADC is too close, then do not enter SIDE_PASS.
constexpr double kClearDistance = 15.0;

void SidePassScenario::RegisterStages() {
  s_stage_factory_.Clear();
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_APPROACH_OBSTACLE,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageApproachObstacle(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_DETECT_SAFETY,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageDetectSafety(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_GENERATE_PATH,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageGeneratePath(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageStopOnWaitPoint(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_PASS_OBSTACLE,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StagePassObstacle(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_BACKUP,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageBackup(config);
      });
}

SidePassScenario::SidePassScenario(const ScenarioConfig& config,
                                   const ScenarioContext* scenario_context)
    : Scenario(config, scenario_context) {
  side_pass_context_.scenario_config_.CopyFrom(config.side_pass_config());

  // TODO(all): to be removed when SidePass obstacle decision impl is ready
  side_pass_context_.front_blocking_obstacle_id_ =
      PlanningContext::GetScenarioInfo()->side_pass_front_blocking_obstacle_id;
}

std::unique_ptr<Stage> SidePassScenario::CreateStage(
    const ScenarioConfig::StageConfig& stage_config) {
  if (s_stage_factory_.Empty()) {
    RegisterStages();
  }
  auto ptr = s_stage_factory_.CreateObjectOrNull(stage_config.stage_type(),
                                                 stage_config);
  if (ptr == nullptr) {
    AERROR << "Failed to create stage for config: "
           << stage_config.DebugString();
    return nullptr;
  }
  ptr->SetContext(&side_pass_context_);
  return ptr;
}

bool SidePassScenario::IsTransferable(const Scenario& current_scenario,
                                      const Frame& frame) {
  // Sanity checks.
  if (frame.reference_line_info().size() > 1) {
    return false;
  }

  std::string front_blocking_obstacle_id =
      PlanningContext::GetScenarioInfo()->side_pass_front_blocking_obstacle_id;

  if (current_scenario.scenario_type() == ScenarioConfig::SIDE_PASS) {
    // Check if the blocking obstacle is still static.
    // If not, then switch to LANE_FOLLOW.
    const auto front_blocking_obstacle =
        const_cast<Frame&>(frame).Find(front_blocking_obstacle_id);
    if (!front_blocking_obstacle) {
      ADEBUG << "Obstacle " << front_blocking_obstacle_id
            << " not exist any more. Change scenario to default scenario.";
      return false;
    }
    const auto& reference_line_info = frame.reference_line_info().front();
    const double adc_front_edge_s =
        reference_line_info.AdcSlBoundary().end_s();
    const double distance =
        front_blocking_obstacle->PerceptionSLBoundary().start_s() -
        adc_front_edge_s;

    constexpr double kSidePassMaxDistance = 10.0;
{'fraction_non_alphanumeric': 0.1273492035081439, 'fraction_numerical': 0.0027742974762842315, 'mean_word_length': 4.2025139664804465, 'pattern_counts': {'":': 0, '<': 29, '<?xml version=': 0, '>': 31, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3137074', 'n_tokens_mistral': 3389, 'n_tokens_neox': 3294, 'n_words': 676}