lane_escape_stage.cc

浏览该文件的文档.

/******************************************************************************
 * Copyright 2024 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/scenarios/lane_escape/lane_escape_stage.h"
 
#include <string>
#include <vector>
#include "modules/planning/planning_base/common/frame.h"
#include "modules/planning/planning_base/common/util/common.h"
 
namespace apollo {
namespace planning {
 
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
 
bool LaneEscapeParkStage::Init(
        const StagePipeline& config,
        const std::shared_ptr<DependencyInjector>& injector,
        const std::string& config_dir,
        void* context) {
    CHECK_NOTNULL(context);
    bool ret = Stage::Init(config, injector, config_dir, context);
    if (!ret) {
        AERROR << Name() << "init failed!";
        return false;
    }
    scenario_config_.CopyFrom(GetContextAs<LaneEscapeContext>()->scenario_config);
    return ret;
}
 
StageResult LaneEscapeParkStage::Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) {
    apollo::common::VehicleState vehicle_state = injector_->vehicle_state()->vehicle_state();
    if (vehicle_state.gear() != canbus::Chassis::GEAR_REVERSE) {
        frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);
        auto* traj_pair = frame->mutable_open_space_info()->mutable_publishable_trajectory_data();
        static constexpr double gear_shift_max_t = 5.0;
        static constexpr double gear_shift_unit_t = 0.1;
        for (double t = 0.0; t < gear_shift_max_t; t += gear_shift_unit_t) {
            TrajectoryPoint point;
            point.mutable_path_point()->set_x(frame->vehicle_state().x());
            point.mutable_path_point()->set_y(frame->vehicle_state().y());
            point.mutable_path_point()->set_theta(frame->vehicle_state().heading());
            point.mutable_path_point()->set_s(0.0);
            point.mutable_path_point()->set_kappa(0);
            point.set_relative_time(t);
            point.set_v(0.0);
            point.set_a(0.0);
            traj_pair->first.emplace_back(point);
        }
        traj_pair->second = apollo::canbus::Chassis::GEAR_REVERSE;
        AINFO << "GENERATE REVERSE TRAJECTORY";
        need_to_stop_ = false;
        return StageResult(StageStatusType::RUNNING);
    }
    auto& reference_line_info = frame->mutable_reference_line_info()->back();
    const auto& reference_line = reference_line_info.reference_line();
    double adc_start_s = reference_line_info.AdcSlBoundary().start_s();
 
    if (stop_point_ == nullptr) {
        apollo::common::math::Vec2d stop_point;
        auto ref_point = reference_line.GetReferencePoint(adc_start_s - scenario_config_.escape_reverse_distance());
        stop_point_ = std::make_shared<common::PathPoint>(ref_point.ToPathPoint(0.0));
    }
    AINFO << "need to stop";
    common::SLPoint sl_point;
    reference_line.XYToSL(*stop_point_, &sl_point);
    std::vector<std::string> wait_for_obstacles;
    planning::util::BuildStopDecision(
            "reverse_stop",
            sl_point.s(),
            0.1,
            StopReasonCode::STOP_REASON_STOP_SIGN,
            wait_for_obstacles,
            "reverse stop",
            frame,
            &(frame->mutable_reference_line_info()->front()),
            10.0);
    StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
    const auto& candidate_paths = reference_line_info.GetCandidatePathData();
    bool has_regular_path = false;
    for (const auto& path_data : candidate_paths) {
        if (path_data.path_label().find("regular") != std::string::npos) {
            AINFO << "path_label: true";
        }
        double distance_to_block_obs = std::numeric_limits<double>::max();
        if (!path_data.blocking_obstacle_id().empty()) {
            auto obstacle = reference_line_info.path_decision()->Find(path_data.blocking_obstacle_id());
            distance_to_block_obs
                    = obstacle->PerceptionSLBoundary().start_s() - reference_line_info.AdcSlBoundary().end_s();
            AINFO << "blocking_obstacle_id: " << path_data.blocking_obstacle_id()
                  << " distance_to_block_obs: " << distance_to_block_obs;
        }
        if (path_data.discretized_path().Length() > scenario_config_.forward_path_length()) {
            AINFO << "discretized_path().Length(): true";
        }
        if (path_data.path_label().find("regular") != std::string::npos
            && path_data.discretized_path().Length() > scenario_config_.forward_path_length()
            && distance_to_block_obs > scenario_config_.forward_path_length()) {
            AINFO << "find regular path: " << path_data.path_label();
            AINFO << "distance_to_block_obs: " << distance_to_block_obs;
            has_regular_path = true;
            break;
        }
    }
    if (has_regular_path) {
        success_path_times_++;
    } else {
        success_path_times_ = 0;
        AINFO << "reset";
        need_to_stop_ = false;
    }
    AINFO << "success_path_times_: " << success_path_times_;
    if (success_path_times_ > scenario_config_.success_path_count()
        && !reference_line_info.path_data().discretized_path().empty()
        && !need_to_stop_) {
        AINFO << "find square path change stage";
        need_to_stop_ = true;
        double ego_speed = frame->vehicle_state().linear_velocity();
        double deceleration_distance = ego_speed * ego_speed / (2 * 0.5);
        double stop_distance = std::min(
                std::max(deceleration_distance, scenario_config_.escape_stop_distance()),
                reference_line_info.mutable_path_data()->discretized_path().Length());
        *stop_point_ = reference_line_info.mutable_path_data()->discretized_path().Evaluate(stop_distance);
        AINFO << "length: " << reference_line_info.mutable_path_data()->discretized_path().Length()
              << " stop_distance: " << stop_distance << " deceleration_distance: " << deceleration_distance
              << std::setprecision(9) << "stop_point_:" << stop_point_->x() << "," << stop_point_->y();
    }
    stop_check_count_ = std::fabs(vehicle_state.linear_velocity()) < 0.05 ? stop_check_count_ + 1 : 0;
    if (success_path_times_ > scenario_config_.success_path_count()
        && stop_check_count_ > scenario_config_.stop_check_window()) {
        AINFO << "already stopped change stage" << vehicle_state.linear_velocity() << " " << vehicle_state.x() << ", "
              << vehicle_state.y();
        return FinishStage();
    }
    return result.SetStageStatus(StageStatusType::RUNNING);
}
 
StageResult LaneEscapeParkStage::FinishStage() {
    next_stage_.clear();
    ParkDataCenter::Instance()->set_need_escape(false);
    return StageResult(StageStatusType::FINISHED);
}
 
}  // namespace planning
}  // namespace apollo