Apollo_lane_change_path.cc代码注释版本

于 2024-06-21 23:33:41 发布
阅读量340 收藏 1
点赞数 1
文章标签： 自动驾驶
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/GOLDMINDER/article/details/139871943
版权 
/******************************************************************************
 * Copyright 2023 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/tasks/lane_change_path/lane_change_path.h"

#include <algorithm>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "cyber/time/clock.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/planning/planning_base/common/planning_context.h"
#include "modules/planning/planning_interface_base/task_base/common/path_generation.h"
#include "modules/planning/planning_interface_base/task_base/common/path_util/path_assessment_decider_util.h"
#include "modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.h"
#include "modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::VehicleConfigHelper;
using apollo::common::math::Box2d;
using apollo::common::math::Polygon2d;
using apollo::common::math::Vec2d;
using apollo::cyber::Clock;

constexpr double kIntersectionClearanceDist = 20.0;
constexpr double kJunctionClearanceDist = 15.0;

// bool LaneChangePath::Init(const std::string& config_dir,
//                           const std::string& name,
//                           const std::shared_ptr<DependencyInjector>& injector) {
//   if (!Task::Init(config_dir, name, injector)) {
//     return false;
//   }
//   // Load the config this task.
//   return Task::LoadConfig<LaneChangePathConfig>(&config_);
// }

// 初始化 LaneChangePath 类的方法
bool LaneChangePath::Init(
        const std::string& config_dir,                          // 配置文件目录
        const std::string& name,                                // 任务名称
        const std::shared_ptr<DependencyInjector>& injector) {  // 依赖注入器
    // 调用父类 Task 的 Init 方法进行初始化，如果初始化失败，则返回 false
    if (!Task::Init(config_dir, name, injector)) {
        return false;
    }
    // 加载此任务的配置
    return Task::LoadConfig<LaneChangePathConfig>(&config_);
}

// apollo::common::Status LaneChangePath::Process(
//     Frame* frame, ReferenceLineInfo* reference_line_info) {
//   UpdateLaneChangeStatus();
//   const auto& status = injector_->planning_context()
//                            ->mutable_planning_status()
//                            ->mutable_change_lane()
//                            ->status();
//   if (!reference_line_info->IsChangeLanePath() ||
//       reference_line_info->path_reusable()) {
//     ADEBUG << "Skip this time" << reference_line_info->IsChangeLanePath()
//            << "path reusable" << reference_line_info->path_reusable();
//     return Status::OK();
//   }
//   if (status != ChangeLaneStatus::IN_CHANGE_LANE) {
//     ADEBUG << injector_->planning_context()
//                   ->mutable_planning_status()
//                   ->mutable_change_lane()
//                   ->DebugString();
//     return Status(ErrorCode::PLANNING_ERROR,
//                   "Not satisfy lane change  conditions");
//   }
//   std::vector<PathBoundary> candidate_path_boundaries;
//   std::vector<PathData> candidate_path_data;

//   GetStartPointSLState();
//   if (!DecidePathBounds(&candidate_path_boundaries)) {
//     return Status(ErrorCode::PLANNING_ERROR, "lane change path bounds failed");
//   }
//   if (!OptimizePath(candidate_path_boundaries, &candidate_path_data)) {
//     return Status(ErrorCode::PLANNING_ERROR,
//                   "lane change path optimize failed");
//   }
//   if (!AssessPath(&candidate_path_data,
//                   reference_line_info->mutable_path_data())) {
//     return Status(ErrorCode::PLANNING_ERROR, "No valid lane change path");
//   }

//   return Status::OK();
// }

// LaneChangePath 类的处理方法
apollo::common::Status LaneChangePath::Process(
        Frame* frame,                              // 当前的帧信息
        ReferenceLineInfo* reference_line_info) {  // 参考线信息
    // 更新车道变更状态
    UpdateLaneChangeStatus();

    // 获取当前的车道变更状态
    const auto& status = injector_->planning_context()->mutable_planning_status()->mutable_change_lane()->status();

    // 如果当前不是在进行车道变更或者路径可以重复使用，则跳过此次处理
    if (!reference_line_info->IsChangeLanePath() || reference_line_info->path_reusable()) {
        ADEBUG << "Skip this time" << reference_line_info->IsChangeLanePath() << "path reusable"
               << reference_line_info->path_reusable();
        return Status::OK();
    }

    // 如果当前的状态不是正在进行车道变更，则返回错误
    if (status != ChangeLaneStatus::IN_CHANGE_LANE) {
        ADEBUG << injector_->planning_context()->mutable_planning_status()->mutable_change_lane()->DebugString();
        return Status(ErrorCode::PLANNING_ERROR, "Not satisfy lane change  conditions");
    }

    // 定义候选的路径边界和路径数据
    std::vector<PathBoundary> candidate_path_boundaries;
    std::vector<PathData> candidate_path_data;

    // 获取起始点的 SL 状态
    GetStartPointSLState();

    // 如果决定路径边界失败，则返回错误
    if (!DecidePathBounds(&candidate_path_boundaries)) {
        return Status(ErrorCode::PLANNING_ERROR, "lane change path bounds failed");
    }

    // 如果优化路径失败，则返回错误
    if (!OptimizePath(candidate_path_boundaries, &candidate_path_data)) {
        return Status(ErrorCode::PLANNING_ERROR, "lane change path optimize failed");
    }

    // 如果评估路径失败，则返回错误
    if (!AssessPath(&candidate_path_data, reference_line_info->mutable_path_data())) {
        return Status(ErrorCode::PLANNING_ERROR, "No valid lane change path");
    }

    // 如果以上所有步骤都成功，则返回成功状态
    return Status::OK();
}

// bool LaneChangePath::DecidePathBounds(std::vector<PathBoundary>* boundary) {
//   boundary->emplace_back();
//   auto& path_bound = boundary->back();
//   double path_narrowest_width = 0;
//   // 1. Initialize the path boundaries to be an indefinitely large area.
//   if (!PathBoundsDeciderUtil::InitPathBoundary(*reference_line_info_,
//                                                &path_bound, init_sl_state_)) {
//     const std::string msg = "Failed to initialize path boundaries.";
//     AERROR << msg;
//     return false;
//   }
//   // 2. Decide a rough boundary based on lane info and ADC's position
//   if (!PathBoundsDeciderUtil::GetBoundaryFromSelfLane(
//           *reference_line_info_, init_sl_state_, &path_bound)) {
//     AERROR << "Failed to decide a rough boundary based on self lane.";
//     return false;
//   }
//   if (!PathBoundsDeciderUtil::ExtendBoundaryByADC(
//           *reference_line_info_, init_sl_state_, config_.extend_adc_buffer(),
//           &path_bound)) {
//     AERROR << "Failed to decide a rough boundary based on adc.";
//     return false;
//   }

//   // 3. Remove the S-length of target lane out of the path-bound.
//   GetBoundaryFromLaneChangeForbiddenZone(&path_bound);

//   PathBound temp_path_bound = path_bound;
//   std::string blocking_obstacle_id;
//   if (!PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles(
//           *reference_line_info_, init_sl_state_, &path_bound,
//           &blocking_obstacle_id, &path_narrowest_width)) {
//     AERROR << "Failed to decide fine tune the boundaries after "
//               "taking into consideration all static obstacles.";
//     return false;
//   }

//   // Append some extra path bound points to avoid zero-length path data.
//   int counter = 0;
//   while (!blocking_obstacle_id.empty() &&
//          path_bound.size() < temp_path_bound.size() &&
//          counter < FLAGS_num_extra_tail_bound_point) {
//     path_bound.push_back(temp_path_bound[path_bound.size()]);
//     counter++;
//   }
//   path_bound.set_label("regular/lane_change");
//   path_bound.set_blocking_obstacle_id(blocking_obstacle_id);
//   RecordDebugInfo(path_bound, path_bound.label(), reference_line_info_);
//   return true;
// }

// 决定路径边界的方法
bool LaneChangePath::DecidePathBounds(std::vector<PathBoundary>* boundary) {
    // 在边界向量中添加一个新的元素
    boundary->emplace_back();
    // 获取刚刚添加的边界元素的引用
    auto& path_bound = boundary->back();
    // 初始化路径最窄宽度为0
    double path_narrowest_width = 0;

    // 1. 初始化路径边界为无限大的区域
    if (!PathBoundsDeciderUtil::InitPathBoundary(*reference_line_info_, &path_bound, init_sl_state_)) {
        const std::string msg = "Failed to initialize path boundaries.";
        AERROR << msg;
        return false;
    }

    // 2. 基于车道信息和自动驾驶车辆的位置决定一个粗略的边界
    if (!PathBoundsDeciderUtil::GetBoundaryFromSelfLane(*reference_line_info_, init_sl_state_, &path_bound)) {
        AERROR << "Failed to decide a rough boundary based on self lane.";
        return false;
    }
    // 基于自动驾驶车辆扩展边界
    if (!PathBoundsDeciderUtil::ExtendBoundaryByADC(
                *reference_line_info_, init_sl_state_, config_.extend_adc_buffer(), &path_bound)) {
        AERROR << "Failed to decide a rough boundary based on adc.";
        return false;
    }

    // 3. 从路径边界中移除目标车道的S长度
    GetBoundaryFromLaneChangeForbiddenZone(&path_bound);

    // 创建一个临时的路径边界
    PathBound temp_path_bound = path_bound;
    // 初始化阻塞障碍物的ID
    std::string blocking_obstacle_id;
    // 基于静态障碍物获取边界
    if (!PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles(
                *reference_line_info_, init_sl_state_, &path_bound, &blocking_obstacle_id, &path_narrowest_width)) {
        AERROR << "Failed to decide fine tune the boundaries after "
                  "taking into consideration all static obstacles.";
        return false;
    }

    // 添加一些额外的路径边界点以避免路径数据长度为零
    int counter = 0;
    while (!blocking_obstacle_id.empty() && path_bound.size() < temp_path_bound.size()
           && counter < FLAGS_num_extra_tail_bound_point) {
        path_bound.push_back(temp_path_bound[path_bound.size()]);
        counter++;
    }
    // 设置路径边界的标签
    path_bound.set_label("regular/lane_change");
    // 设置阻塞障碍物的ID
    path_bound.set_blocking_obstacle_id(blocking_obstacle_id);
    // 记录调试信息
    RecordDebugInfo(path_bound, path_bound.label(), reference_line_info_);
    // 如果所有步骤都成功，则返回 true
    return true;
}
// 优化路径的方法

// bool LaneChangePath::OptimizePath(
//     const std::vector<PathBoundary>& path_boundaries,
//     std::vector<PathData>* candidate_path_data) {
//   const auto& config = config_.path_optimizer_config();
//   const ReferenceLine& reference_line = reference_line_info_->reference_line();
//   std::array<double, 3> end_state = {0.0, 0.0, 0.0};
//   for (const auto& path_boundary : path_boundaries) {
//     size_t path_boundary_size = path_boundary.boundary().size();
//     if (path_boundary_size <= 1U) {
//       AERROR << "Get invalid path boundary with size: " << path_boundary_size;
//       return false;
//     }
//     std::vector<double> opt_l, opt_dl, opt_ddl;
//     std::vector<std::pair<double, double>> ddl_bounds;
//     PathOptimizerUtil::CalculateAccBound(path_boundary, reference_line,
//                                          &ddl_bounds);
//     const double jerk_bound = PathOptimizerUtil::EstimateJerkBoundary(
//         std::fmax(init_sl_state_.first[1], 1e-12));
//     std::vector<double> ref_l(path_boundary_size, 0);
//     std::vector<double> weight_ref_l(path_boundary_size, 0);

//     bool res_opt = PathOptimizerUtil::OptimizePath(
//         init_sl_state_, end_state, ref_l, weight_ref_l, path_boundary,
//         ddl_bounds, jerk_bound, config, &opt_l, &opt_dl, &opt_ddl);
//     if (res_opt) {
//       auto frenet_frame_path = PathOptimizerUtil::ToPiecewiseJerkPath(
//           opt_l, opt_dl, opt_ddl, path_boundary.delta_s(),
//           path_boundary.start_s());
//       PathData path_data;
//       path_data.SetReferenceLine(&reference_line);
//       path_data.SetFrenetPath(std::move(frenet_frame_path));
//       if (FLAGS_use_front_axe_center_in_path_planning) {
//         auto discretized_path = DiscretizedPath(
//             PathOptimizerUtil::ConvertPathPointRefFromFrontAxeToRearAxe(
//                 path_data));
//         path_data.SetDiscretizedPath(discretized_path);
//       }
//       path_data.set_path_label(path_boundary.label());
//       path_data.set_blocking_obstacle_id(path_boundary.blocking_obstacle_id());
//       candidate_path_data->push_back(std::move(path_data));
//     }
//   }
//   if (candidate_path_data->empty()) {
//     return false;
//   }
//   return true;
// }

// 优化路径的方法
bool LaneChangePath::OptimizePath(
        const std::vector<PathBoundary>& path_boundaries,
        std::vector<PathData>* candidate_path_data) {
    // 获取路径优化器配置
    const auto& config = config_.path_optimizer_config();
    // 获取参考线
    const ReferenceLine& reference_line = reference_line_info_->reference_line();
    // 初始化结束状态为 {0.0, 0.0, 0.0}
    std::array<double, 3> end_state = {0.0, 0.0, 0.0};

    // 遍历路径边界
    for (const auto& path_boundary : path_boundaries) {
        // 获取路径边界的大小
        size_t path_boundary_size = path_boundary.boundary().size();
        // 如果路径边界的大小小于等于1，返回错误
        if (path_boundary_size <= 1U) {
            AERROR << "Get invalid path boundary with size: " << path_boundary_size;
            return false;
        }

        // 初始化优化路径的各个参数
        std::vector<double> opt_l, opt_dl, opt_ddl;
        std::vector<std::pair<double, double>> ddl_bounds;
        // 计算加速度边界
        PathOptimizerUtil::CalculateAccBound(path_boundary, reference_line, &ddl_bounds);
        // 估计冲量边界
        const double jerk_bound = PathOptimizerUtil::EstimateJerkBoundary(std::fmax(init_sl_state_.first[1], 1e-12));
        std::vector<double> ref_l(path_boundary_size, 0);
        std::vector<double> weight_ref_l(path_boundary_size, 0);

        // 优化路径
        bool res_opt = PathOptimizerUtil::OptimizePath(
                init_sl_state_,
                end_state,
                ref_l,
                weight_ref_l,
                path_boundary,
                ddl_bounds,
                jerk_bound,
                config,
                &opt_l,
                &opt_dl,
                &opt_ddl);

        // 如果优化成功
        if (res_opt) {
            // 转换为分段冲量路径
            auto frenet_frame_path = PathOptimizerUtil::ToPiecewiseJerkPath(
                    opt_l, opt_dl, opt_ddl, path_boundary.delta_s(), path_boundary.start_s());
            PathData path_data;
            // 设置参考线
            path_data.SetReferenceLine(&reference_line);
            // 设置弗雷内特路径
            path_data.SetFrenetPath(std::move(frenet_frame_path));
            // 如果在路径规划中使用前轴中心
            if (FLAGS_use_front_axe_center_in_path_planning) {
                // 转换路径点的参考点从前轴到后轴
                auto discretized_path
                        = DiscretizedPath(PathOptimizerUtil::ConvertPathPointRefFromFrontAxeToRearAxe(path_data));
                // 设置离散化路径
                path_data.SetDiscretizedPath(discretized_path);
            }
            // 设置路径标签
            path_data.set_path_label(path_boundary.label());
            // 设置阻塞障碍物的ID
            path_data.set_blocking_obstacle_id(path_boundary.blocking_obstacle_id());
            // 将路径数据添加到候选路径数据中
            candidate_path_data->push_back(std::move(path_data));
        }
    }

    // 如果候选路径数据为空，返回 false
    if (candidate_path_data->empty()) {
        return false;
    }

    // 如果所有步骤都成功，返回 true
    return true;
}

// bool LaneChangePath::AssessPath(std::vector<PathData>* candidate_path_data,
//                                 PathData* final_path) {
//   std::vector<PathData> valid_path_data;
//   for (auto& curr_path_data : *candidate_path_data) {
//     if (PathAssessmentDeciderUtil::IsValidRegularPath(*reference_line_info_,
//                                                       curr_path_data)) {
//       SetPathInfo(&curr_path_data);
//       PathAssessmentDeciderUtil::TrimTailingOutLanePoints(&curr_path_data);
//       if (curr_path_data.Empty()) {
//         AINFO << "lane change path is empty after trimed";
//         continue;
//       }
//       valid_path_data.push_back(curr_path_data);
//     }
//   }
//   if (valid_path_data.empty()) {
//     AINFO << "All lane change path are not valid";
//     return false;
//   }

//   *final_path = valid_path_data[0];
//   RecordDebugInfo(*final_path, final_path->path_label(), reference_line_info_);
//   return true;
// }

// 评估路径的方法
bool LaneChangePath::AssessPath(std::vector<PathData>* candidate_path_data, PathData* final_path) {
    // 初始化有效路径数据
    std::vector<PathData> valid_path_data;
    // 遍历候选路径数据
    for (auto& curr_path_data : *candidate_path_data) {
        // 如果当前路径是有效的常规路径
        if (PathAssessmentDeciderUtil::IsValidRegularPath(*reference_line_info_, curr_path_data)) {
            // 设置路径信息
            SetPathInfo(&curr_path_data);
            // 剪裁尾部超出车道的点
            PathAssessmentDeciderUtil::TrimTailingOutLanePoints(&curr_path_data);
            // 如果剪裁后的路径为空，继续下一次循环
            if (curr_path_data.Empty()) {
                AINFO << "lane change path is empty after trimed";
                continue;
            }
            // 将当前路径数据添加到有效路径数据中
            valid_path_data.push_back(curr_path_data);
        }
    }
    // 如果有效路径数据为空，返回 false
    if (valid_path_data.empty()) {
        AINFO << "All lane change path are not valid";
        return false;
    }

    // 将第一个有效路径数据设置为最终路径
    *final_path = valid_path_data[0];
    // 记录调试信息
    RecordDebugInfo(*final_path, final_path->path_label(), reference_line_info_);
    // 如果所有步骤都成功，返回 true
    return true;
}

// void LaneChangePath::UpdateLaneChangeStatus() {
//   std::string change_lane_id;
//   auto* prev_status = injector_->planning_context()
//                           ->mutable_planning_status()
//                           ->mutable_change_lane();
//   double now = Clock::NowInSeconds();
//   // Init lane change status
//   if (!prev_status->has_status()) {
//     UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED, "");
//     return;
//   }
//   bool has_change_lane = frame_->reference_line_info().size() > 1;
//   if (!has_change_lane) {
//     if (prev_status->status() == ChangeLaneStatus::IN_CHANGE_LANE) {
//       UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED,
//                    prev_status->path_id());
//     }
//     return;
//   }
//   // has change lane
//   if (reference_line_info_->IsChangeLanePath()) {
//     const auto* history_frame = injector_->frame_history()->Latest();
//     if (!CheckLastFrameSucceed(history_frame)) {
//       UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FAILED, change_lane_id);
//       is_exist_lane_change_start_position_ = false;
//       return;
//     }
//     is_clear_to_change_lane_ = IsClearToChangeLane(reference_line_info_);
//     change_lane_id = reference_line_info_->Lanes().Id();
//     ADEBUG << "change_lane_id" << change_lane_id;
//     if (prev_status->status() == ChangeLaneStatus::CHANGE_LANE_FAILED) {
//       if (now - prev_status->timestamp() >
//           config_.change_lane_fail_freeze_time()) {
//         UpdateStatus(now, ChangeLaneStatus::IN_CHANGE_LANE, change_lane_id);
//         ADEBUG << "change lane again after failed";
//       }
//       return;
//     } else if (prev_status->status() ==
//                ChangeLaneStatus::CHANGE_LANE_FINISHED) {
//       if (now - prev_status->timestamp() >
//           config_.change_lane_success_freeze_time()) {
//         UpdateStatus(now, ChangeLaneStatus::IN_CHANGE_LANE, change_lane_id);
//         AINFO << "change lane again after success";
//       }
//     } else if (prev_status->status() == ChangeLaneStatus::IN_CHANGE_LANE) {
//       if (prev_status->path_id() != change_lane_id) {
//         AINFO << "change_lane_id" << change_lane_id << "prev"
//               << prev_status->path_id();
//         UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED,
//                      prev_status->path_id());
//       }
//     }
//   }
// }

// 更新车道变更状态的方法
void LaneChangePath::UpdateLaneChangeStatus() {
    // 初始化车道变更ID
    std::string change_lane_id;
    // 获取上一次的状态
    auto* prev_status = injector_->planning_context()->mutable_planning_status()->mutable_change_lane();
    // 获取当前时间
    double now = Clock::NowInSeconds();
    // 如果上一次的状态不存在，更新状态为车道变更完成，并返回
    if (!prev_status->has_status()) {
        UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED, "");
        return;
    }
    // 判断是否有车道变更，如果参考线信息的大小大于1，表示有车道变更
    bool has_change_lane = frame_->reference_line_info().size() > 1;
    // 如果没有车道变更
    if (!has_change_lane) {
        // 如果上一次的状态是正在变更车道，更新状态为车道变更完成，并返回
        if (prev_status->status() == ChangeLaneStatus::IN_CHANGE_LANE) {
            UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED, prev_status->path_id());
        }
        return;
    }
    // 如果有车道变更
    if (reference_line_info_->IsChangeLanePath()) {
        // 获取最新的历史帧
        const auto* history_frame = injector_->frame_history()->Latest();
        // 如果最后一帧没有成功
        if (!CheckLastFrameSucceed(history_frame)) {
            // 更新状态为车道变更失败，并返回
            UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FAILED, change_lane_id);
            is_exist_lane_change_start_position_ = false;
            return;
        }
        // 判断是否可以变更车道
        is_clear_to_change_lane_ = IsClearToChangeLane(reference_line_info_);
        // 获取车道ID
        change_lane_id = reference_line_info_->Lanes().Id();
        ADEBUG << "change_lane_id" << change_lane_id;
        // 如果上一次的状态是车道变更失败
        if (prev_status->status() == ChangeLaneStatus::CHANGE_LANE_FAILED) {
            // 如果当前时间减去上一次的时间戳大于车道变更失败冻结时间
            if (now - prev_status->timestamp() > config_.change_lane_fail_freeze_time()) {
                // 更新状态为正在变更车道，并返回
                UpdateStatus(now, ChangeLaneStatus::IN_CHANGE_LANE, change_lane_id);
                ADEBUG << "change lane again after failed";
            }
            return;
        }
        // 如果上一次的状态是车道变更完成
        else if (prev_status->status() == ChangeLaneStatus::CHANGE_LANE_FINISHED) {
            // 如果当前时间减去上一次的时间戳大于车道变更成功冻结时间
            if (now - prev_status->timestamp() > config_.change_lane_success_freeze_time()) {
                // 更新状态为正在变更车道
                UpdateStatus(now, ChangeLaneStatus::IN_CHANGE_LANE, change_lane_id);
                AINFO << "change lane again after success";
            }
        }
        // 如果上一次的状态是正在变更车道
        else if (prev_status->status() == ChangeLaneStatus::IN_CHANGE_LANE) {
            // 如果上一次的路径ID不等于当前的车道变更ID
            if (prev_status->path_id() != change_lane_id) {
                AINFO << "change_lane_id" << change_lane_id << "prev" << prev_status->path_id();
                // 更新状态为车道变更完成
                UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED, prev_status->path_id());
            }
        }
    }
}

// bool LaneChangePath::IsClearToChangeLane(
//     ReferenceLineInfo* reference_line_info) {
//   double ego_start_s = reference_line_info->AdcSlBoundary().start_s();
//   double ego_end_s = reference_line_info->AdcSlBoundary().end_s();
//   double ego_v =
//       std::abs(reference_line_info->vehicle_state().linear_velocity());

//   for (const auto* obstacle :
//        reference_line_info->path_decision()->obstacles().Items()) {
//     if (obstacle->IsVirtual() || obstacle->IsStatic()) {
//       ADEBUG << "skip one virtual or static obstacle";
//       continue;
//     }

//     double start_s = std::numeric_limits<double>::max();
//     double end_s = -std::numeric_limits<double>::max();
//     double start_l = std::numeric_limits<double>::max();
//     double end_l = -std::numeric_limits<double>::max();

//     for (const auto& p : obstacle->PerceptionPolygon().points()) {
//       apollo::common::SLPoint sl_point;
//       reference_line_info->reference_line().XYToSL(p, &sl_point);
//       start_s = std::fmin(start_s, sl_point.s());
//       end_s = std::fmax(end_s, sl_point.s());

//       start_l = std::fmin(start_l, sl_point.l());
//       end_l = std::fmax(end_l, sl_point.l());
//     }

//     if (reference_line_info->IsChangeLanePath()) {
//       double left_width(0), right_width(0);
//       reference_line_info->mutable_reference_line()->GetLaneWidth(
//           (start_s + end_s) * 0.5, &left_width, &right_width);
//       if (end_l < -right_width || start_l > left_width) {
//         continue;
//       }
//     }

//     // Raw estimation on whether same direction with ADC or not based on
//     // prediction trajectory
//     bool same_direction = true;
//     if (obstacle->HasTrajectory()) {
//       double obstacle_moving_direction =
//           obstacle->Trajectory().trajectory_point(0).path_point().theta();
//       const auto& vehicle_state = reference_line_info->vehicle_state();
//       double vehicle_moving_direction = vehicle_state.heading();
//       if (vehicle_state.gear() == canbus::Chassis::GEAR_REVERSE) {
//         vehicle_moving_direction =
//             common::math::NormalizeAngle(vehicle_moving_direction + M_PI);
//       }
//       double heading_difference = std::abs(common::math::NormalizeAngle(
//           obstacle_moving_direction - vehicle_moving_direction));
//       same_direction = heading_difference < (M_PI / 2.0);
//     }

//     // TODO(All) move to confs
//     static constexpr double kSafeTimeOnSameDirection = 3.0;
//     static constexpr double kSafeTimeOnOppositeDirection = 5.0;
//     static constexpr double kForwardMinSafeDistanceOnSameDirection = 10.0;
//     static constexpr double kBackwardMinSafeDistanceOnSameDirection = 10.0;
//     static constexpr double kForwardMinSafeDistanceOnOppositeDirection = 50.0;
//     static constexpr double kBackwardMinSafeDistanceOnOppositeDirection = 1.0;
//     static constexpr double kDistanceBuffer = 0.5;

//     double kForwardSafeDistance = 0.0;
//     double kBackwardSafeDistance = 0.0;
//     if (same_direction) {
//       kForwardSafeDistance =
//           std::fmax(kForwardMinSafeDistanceOnSameDirection,
//                     (ego_v - obstacle->speed()) * kSafeTimeOnSameDirection);
//       kBackwardSafeDistance =
//           std::fmax(kBackwardMinSafeDistanceOnSameDirection,
//                     (obstacle->speed() - ego_v) * kSafeTimeOnSameDirection);
//     } else {
//       kForwardSafeDistance =
//           std::fmax(kForwardMinSafeDistanceOnOppositeDirection,
//                     (ego_v + obstacle->speed()) * kSafeTimeOnOppositeDirection);
//       kBackwardSafeDistance = kBackwardMinSafeDistanceOnOppositeDirection;
//     }

//     if (HysteresisFilter(ego_start_s - end_s, kBackwardSafeDistance,
//                          kDistanceBuffer, obstacle->IsLaneChangeBlocking()) &&
//         HysteresisFilter(start_s - ego_end_s, kForwardSafeDistance,
//                          kDistanceBuffer, obstacle->IsLaneChangeBlocking())) {
//       reference_line_info->path_decision()
//           ->Find(obstacle->Id())
//           ->SetLaneChangeBlocking(true);
//       ADEBUG << "Lane Change is blocked by obstacle" << obstacle->Id();
//       return false;
//     } else {
//       reference_line_info->path_decision()
//           ->Find(obstacle->Id())
//           ->SetLaneChangeBlocking(false);
//     }
//   }
//   return true;
// }

// 判断是否可以更换车道
bool LaneChangePath::IsClearToChangeLane(ReferenceLineInfo* reference_line_info) {
    // 获取自车的起始和结束位置以及速度
    double ego_start_s = reference_line_info->AdcSlBoundary().start_s();
    double ego_end_s = reference_line_info->AdcSlBoundary().end_s();
    double ego_v = std::abs(reference_line_info->vehicle_state().linear_velocity());

    // 遍历所有障碍物
    for (const auto* obstacle : reference_line_info->path_decision()->obstacles().Items()) {
        // 如果障碍物是虚拟的或静态的，跳过
        if (obstacle->IsVirtual() || obstacle->IsStatic()) {
            ADEBUG << "skip one virtual or static obstacle";
            continue;
        }

        // 初始化障碍物的起始和结束位置
        double start_s = std::numeric_limits<double>::max();
        double end_s = -std::numeric_limits<double>::max();
        double start_l = std::numeric_limits<double>::max();
        double end_l = -std::numeric_limits<double>::max();

        // 遍历障碍物的所有点，获取障碍物的起始和结束位置
        for (const auto& p : obstacle->PerceptionPolygon().points()) {
            apollo::common::SLPoint sl_point;
            reference_line_info->reference_line().XYToSL(p, &sl_point);
            start_s = std::fmin(start_s, sl_point.s());
            end_s = std::fmax(end_s, sl_point.s());

            start_l = std::fmin(start_l, sl_point.l());
            end_l = std::fmax(end_l, sl_point.l());
        }

        // 如果是更换车道的路径，检查障碍物是否在车道宽度之外
        if (reference_line_info->IsChangeLanePath()) {
            double left_width(0), right_width(0);
            reference_line_info->mutable_reference_line()->GetLaneWidth(
                    (start_s + end_s) * 0.5, &left_width, &right_width);
            if (end_l < -right_width || start_l > left_width) {
                continue;
            }
        }

        // 基于预测轨迹，原始估计障碍物是否与ADC同向
        bool same_direction = true;
        if (obstacle->HasTrajectory()) {
            double obstacle_moving_direction = obstacle->Trajectory().trajectory_point(0).path_point().theta();
            const auto& vehicle_state = reference_line_info->vehicle_state();
            double vehicle_moving_direction = vehicle_state.heading();
            if (vehicle_state.gear() == canbus::Chassis::GEAR_REVERSE) {
                vehicle_moving_direction = common::math::NormalizeAngle(vehicle_moving_direction + M_PI);
            }
            double heading_difference
                    = std::abs(common::math::NormalizeAngle(obstacle_moving_direction - vehicle_moving_direction));
            same_direction = heading_difference < (M_PI / 2.0);
        }

        // 安全时间和安全距离的设定
        static constexpr double kSafeTimeOnSameDirection = 3.0;
        static constexpr double kSafeTimeOnOppositeDirection = 5.0;
        static constexpr double kForwardMinSafeDistanceOnSameDirection = 10.0;
        static constexpr double kBackwardMinSafeDistanceOnSameDirection = 10.0;
        static constexpr double kForwardMinSafeDistanceOnOppositeDirection = 50.0;
        static constexpr double kBackwardMinSafeDistanceOnOppositeDirection = 1.0;
        // static constexpr double kForwardMinSafeDistanceOnSameDirection = 10.0;
        // static constexpr double kBackwardMinSafeDistanceOnSameDirection = 10.0;
        // static constexpr double kForwardMinSafeDistanceOnOppositeDirection = 50.0;
        // static constexpr double kBackwardMinSafeDistanceOnOppositeDirection = 1.0;
        // 应对问题3扣除20分做出的测试修改,important
        static constexpr double kDistanceBuffer = 0.5;
        /*kSafeTimeOnSameDirection 和 kSafeTimeOnOppositeDirection
        可能是指车辆在同向行驶和相向行驶时，预计需要多长时间才能完全停止车辆。
        kForwardMinSafeDistanceOnSameDirection、
        kBackwardMinSafeDistanceOnSameDirection、
        kForwardMinSafeDistanceOnOppositeDirection
        和 kBackwardMinSafeDistanceOnOppositeDirection
        可能是指车辆在不同方向和行驶状态下的最小安全距离。
        kDistanceBuffer 可能是一个额外的缓冲距离，
        用于考虑测量误差或车辆动态性能的不确定性。
        这些都是静态常量，用于定义车辆在进行车道变更时的安全距离和时间。具体含义如下：
        - `kSafeTimeOnSameDirection`：当车辆与前方车辆同向行驶时，保持的安全时间间隔，单位为秒。
        - `kSafeTimeOnOppositeDirection`：当车辆与前方车辆反向行驶时，保持的安全时间间隔，单位为秒。
        - `kForwardMinSafeDistanceOnSameDirection`：当车辆与前方车辆同向行驶时，保持的最小安全距离，单位为米。
        - `kBackwardMinSafeDistanceOnSameDirection`：当车辆与后方车辆同向行驶时，保持的最小安全距离，单位为米。
        - `kForwardMinSafeDistanceOnOppositeDirection`：当车辆与前方车辆反向行驶时，保持的最小安全距离，单位为米。
        - `kBackwardMinSafeDistanceOnOppositeDirection`：当车辆与后方车辆反向行驶时，保持的最小安全距离，单位为米。
        - `kDistanceBuffer`：用于计算安全距离时的缓冲区间，单位为米。*/
        // double kForwardSafeDistance = 0.0;
        double kForwardSafeDistance = 0.0;  // time:6.19.21.41修改安全距离
        double kBackwardSafeDistance = 0.0;
        // 根据是否同向，计算前后安全距离
        // if (same_direction) {
        //   kForwardSafeDistance =
        //       std::fmax(kForwardMinSafeDistanceOnSameDirection,
        //                 (ego_v - obstacle->speed()) * kSafeTimeOnSameDirection);
        //   kBackwardSafeDistance =
        //       std::fmax(kBackwardMinSafeDistanceOnSameDirection,
        //                 (obstacle->speed() - ego_v) * kSafeTimeOnSameDirection);
        // } else {
        //   kForwardSafeDistance =
        //       std::fmax(kForwardMinSafeDistanceOnOppositeDirection,
        //                 (ego_v + obstacle->speed()) * kSafeTimeOnOppositeDirection);
        //   kBackwardSafeDistance = kBackwardMinSafeDistanceOnOppositeDirection;
        // }
        if (same_direction) {
            // 当车辆与前方车辆同向行驶时，计算前向安全距离
            // 安全距离取决于自车速度与前方车辆速度的差值乘以安全时间间隔和最小安全距离中的较大值
            kForwardSafeDistance = std::fmax(
                    kForwardMinSafeDistanceOnSameDirection, (ego_v - obstacle->speed()) * kSafeTimeOnSameDirection);
            // 当车辆与后方车辆同向行驶时，计算后向安全距离
            // 安全距离取决于后方车辆速度与自车速度的差值乘以安全时间间隔和最小安全距离中的较大值
            kBackwardSafeDistance = std::fmax(
                    kBackwardMinSafeDistanceOnSameDirection, (obstacle->speed() - ego_v) * kSafeTimeOnSameDirection);
        } else {
            // 当车辆与前方车辆反向行驶时，计算前向安全距离
            // 安全距离取决于自车速度与前方车辆速度的和乘以安全时间间隔和最小安全距离中的较大值
            kForwardSafeDistance = std::fmax(
                    kForwardMinSafeDistanceOnOppositeDirection,
                    (ego_v + obstacle->speed()) * kSafeTimeOnOppositeDirection);
            // 当车辆与后方车辆反向行驶时，后向安全距离直接取最小安全距离
            kBackwardSafeDistance = kBackwardMinSafeDistanceOnOppositeDirection;
        }

        // 如果障碍物阻挡了车道更换，设置障碍物为阻挡状态
        if (HysteresisFilter(
                    ego_start_s - end_s, kBackwardSafeDistance, kDistanceBuffer, obstacle->IsLaneChangeBlocking())
            && HysteresisFilter(
                    start_s - ego_end_s, kForwardSafeDistance, kDistanceBuffer, obstacle->IsLaneChangeBlocking())) {
            reference_line_info->path_decision()->Find(obstacle->Id())->SetLaneChangeBlocking(true);
            //->SetLaneChangeBlocking(false);//这里做破坏测试，直接返回false,time:6.19.21.41
            // 这里做破坏测试，直接返回false,time:6.19.21.41
            ADEBUG << "Lane Change is blocked by obstacle" << obstacle->Id();
            return false;
            // return true;//这里做破坏测试，直接返回true,time:6.19.21.41
        } else {
            reference_line_info->path_decision()->Find(obstacle->Id())->SetLaneChangeBlocking(false);
        }
    }
    // 如果没有障碍物阻挡，返回true
    return true;
}

// void LaneChangePath::GetLaneChangeStartPoint(
//     const ReferenceLine& reference_line, double adc_frenet_s,
//     common::math::Vec2d* start_xy) {
//   double lane_change_start_s =
//       config_.lane_change_prepare_length() + adc_frenet_s;
//   common::SLPoint lane_change_start_sl;
//   lane_change_start_sl.set_s(lane_change_start_s);
//   lane_change_start_sl.set_l(0.0);
//   reference_line.SLToXY(lane_change_start_sl, start_xy);
// }
// 定义一个函数，用于获取车道变更的起始点
void LaneChangePath::GetLaneChangeStartPoint(
        const ReferenceLine& reference_line,  // 参考线，通常是车道中心线
        double adc_frenet_s,                  // 自车在参考线上的s坐标，即沿着参考线的距离
        common::math::Vec2d* start_xy) {      // 输出参数，用于存储车道变更起始点的XY坐标
    // 计算车道变更的起始点在参考线上的s坐标
    // 该坐标等于车道变更准备长度（即车辆开始变更前需要行驶的距离）加上自车当前的s坐标
    double lane_change_start_s = config_.lane_change_prepare_length() + adc_frenet_s;
    // 定义一个SL坐标点，用于存储车道变更起始点的SL坐标
    common::SLPoint lane_change_start_sl;
    // 设置该SL坐标点的s坐标
    lane_change_start_sl.set_s(lane_change_start_s);
    // 设置该SL坐标点的l坐标为0，表示车道变更起始点在参考线上
    lane_change_start_sl.set_l(0.0);
    // 将车道变更起始点的SL坐标转换为XY坐标，并存储到输出参数start_xy中
    reference_line.SLToXY(lane_change_start_sl, start_xy);
}

// void LaneChangePath::GetBoundaryFromLaneChangeForbiddenZone(
//     PathBoundary* const path_bound) {
//   // Sanity checks.
//   CHECK_NOTNULL(path_bound);

//   if (is_clear_to_change_lane_) {
//     is_exist_lane_change_start_position_ = false;
//     return;
//   }
//   double lane_change_start_s = 0.0;
//   const ReferenceLine& reference_line = reference_line_info_->reference_line();
//   // If there is a pre-determined lane-change starting position, then use it;
//   // otherwise, decide one.
//   if (is_exist_lane_change_start_position_) {
//     common::SLPoint point_sl;
//     reference_line.XYToSL(lane_change_start_xy_, &point_sl);
//     lane_change_start_s = point_sl.s();
//   } else {
//     // TODO(jiacheng): train ML model to learn this.
//     lane_change_start_s =
//         config_.lane_change_prepare_length() + init_sl_state_.first[0];

//     // Update the lane_change_start_xy_ decided by lane_change_start_s
//     GetLaneChangeStartPoint(reference_line, init_sl_state_.first[0],
//                             &lane_change_start_xy_);
//   }

//   // Remove the target lane out of the path-boundary, up to the decided S.
//   if (lane_change_start_s < init_sl_state_.first[0]) {
//     // If already passed the decided S, then return.
//     return;
//   }
//   double adc_half_width =
//       VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;
//   for (size_t i = 0; i < path_bound->size(); ++i) {
//     double curr_s = (*path_bound)[i].s;
//     if (curr_s > lane_change_start_s) {
//       break;
//     }
//     double curr_lane_left_width = 0.0;
//     double curr_lane_right_width = 0.0;
//     double offset_to_map = 0.0;
//     reference_line.GetOffsetToMap(curr_s, &offset_to_map);
//     if (reference_line.GetLaneWidth(curr_s, &curr_lane_left_width,
//                                     &curr_lane_right_width)) {
//       double offset_to_lane_center = 0.0;
//       reference_line.GetOffsetToMap(curr_s, &offset_to_lane_center);
//       curr_lane_left_width += offset_to_lane_center;
//       curr_lane_right_width -= offset_to_lane_center;
//     }
//     curr_lane_left_width -= offset_to_map;
//     curr_lane_right_width += offset_to_map;

//     (*path_bound)[i].l_lower.l = init_sl_state_.second[0] > curr_lane_left_width
//                                      ? curr_lane_left_width + adc_half_width
//                                      : (*path_bound)[i].l_lower.l;
//     (*path_bound)[i].l_lower.l =
//         std::fmin((*path_bound)[i].l_lower.l, init_sl_state_.second[0] - 0.1);
//     (*path_bound)[i].l_upper.l =
//         init_sl_state_.second[0] < -curr_lane_right_width
//             ? -curr_lane_right_width - adc_half_width
//             : (*path_bound)[i].l_upper.l;
//     (*path_bound)[i].l_upper.l =
//         std::fmax((*path_bound)[i].l_upper.l, init_sl_state_.second[0] + 0.1);
//   }
// }

void LaneChangePath::GetBoundaryFromLaneChangeForbiddenZone(PathBoundary* const path_bound) {
    // Sanity checks.
    CHECK_NOTNULL(path_bound);  // 检查路径边界是否为空

    // 如果可以更改车道，则将车道更改起始位置标志设置为false并返回
    if (is_clear_to_change_lane_) {
        is_exist_lane_change_start_position_ = false;
        return;
    }
    double lane_change_start_s = 0.0;
    const ReferenceLine& reference_line = reference_line_info_->reference_line();  // 获取参考线

    // 如果存在预定的车道更改起始位置，则使用它；否则，决定一个。
    if (is_exist_lane_change_start_position_) {
        common::SLPoint point_sl;
        reference_line.XYToSL(lane_change_start_xy_, &point_sl);  // 将车道更改起始位置的XY坐标转换为SL坐标
        lane_change_start_s = point_sl.s();                       // 获取车道更改起始位置的S坐标
    } else {
        // TODO(jiacheng): train ML model to learn this.
        // 计算车道更改起始位置的S坐标
        lane_change_start_s = config_.lane_change_prepare_length() + init_sl_state_.first[0];

        // 更新由lane_change_start_s决定的lane_change_start_xy_
        GetLaneChangeStartPoint(reference_line, init_sl_state_.first[0], &lane_change_start_xy_);
    }

    // 如果车道更改起始位置的S坐标小于当前位置的S坐标，则返回
    if (lane_change_start_s < init_sl_state_.first[0]) {
        return;
    }
    double adc_half_width = VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;  // 计算车辆宽度的一半

    // 遍历路径边界
    for (size_t i = 0; i < path_bound->size(); ++i) {
        double curr_s = (*path_bound)[i].s;  // 获取当前路径边界点的S坐标
        if (curr_s > lane_change_start_s) {
            break;
        }
        double curr_lane_left_width = 0.0;
        double curr_lane_right_width = 0.0;
        double offset_to_map = 0.0;
        reference_line.GetOffsetToMap(curr_s, &offset_to_map);  // 获取当前S坐标到地图的偏移量
        if (reference_line.GetLaneWidth(curr_s, &curr_lane_left_width,
                                        &curr_lane_right_width)) {  // 获取当前车道的宽度
            double offset_to_lane_center = 0.0;
            reference_line.GetOffsetToMap(curr_s, &offset_to_lane_center);  // 获取当前S坐标到车道中心的偏移量
            curr_lane_left_width += offset_to_lane_center;
            curr_lane_right_width -= offset_to_lane_center;
        }
        curr_lane_left_width -= offset_to_map;
        curr_lane_right_width += offset_to_map;

        // 更新路径边界点的左右边界
        (*path_bound)[i].l_lower.l = init_sl_state_.second[0] > curr_lane_left_width
                ? curr_lane_left_width + adc_half_width
                : (*path_bound)[i].l_lower.l;
        (*path_bound)[i].l_lower.l = std::fmin((*path_bound)[i].l_lower.l, init_sl_state_.second[0] - 0.1);
        (*path_bound)[i].l_upper.l = init_sl_state_.second[0] < -curr_lane_right_width
                ? -curr_lane_right_width - adc_half_width
                : (*path_bound)[i].l_upper.l;
        (*path_bound)[i].l_upper.l = std::fmax((*path_bound)[i].l_upper.l, init_sl_state_.second[0] + 0.1);
    }
}

/*这段代码定义了一个函数，该函数用于从车道更改禁区获取路径边界。
首先，它会检查路径边界是否为空，然后根据是否可以更改车道来决定
是否需要计算车道更改的起始位置。如果可以更改车道，那么它会将车道
更改起始位置标志设置为false并返回。如果不能更改车道，那么它会计算
车道更改的起始位置，并更新由该位置决定的XY坐标。然后，它会遍历路径
边界，对于每个路径边界点，如果其S坐标大于车道更改起始位置的S坐标，
那么它会跳出循环。否则，它会计算当前车道的宽度，并更新路径边界点的
左右边界。*/
// void LaneChangePath::UpdateStatus(double timestamp,
//                                   ChangeLaneStatus::Status status_code,
//                                   const std::string& path_id) {
//   auto* lane_change_status = injector_->planning_context()
//                                  ->mutable_planning_status()
//                                  ->mutable_change_lane();
//   AINFO << "lane change update from" << lane_change_status->DebugString()
//         << "to";
//   lane_change_status->set_timestamp(timestamp);
//   lane_change_status->set_path_id(path_id);
//   lane_change_status->set_status(status_code);
//   AINFO << lane_change_status->DebugString();
// }

void LaneChangePath::UpdateStatus(double timestamp, ChangeLaneStatus::Status status_code, const std::string& path_id) {
    // 获取当前的车道更改状态
    auto* lane_change_status = injector_->planning_context()->mutable_planning_status()->mutable_change_lane();
    // 打印当前的车道更改状态
    AINFO << "lane change update from" << lane_change_status->DebugString() << "to";
    // 设置新的时间戳
    lane_change_status->set_timestamp(timestamp);
    // 设置新的路径ID
    lane_change_status->set_path_id(path_id);
    // 设置新的状态码
    lane_change_status->set_status(status_code);
    // 打印更新后的车道更改状态
    AINFO << lane_change_status->DebugString();
}

// bool LaneChangePath::HysteresisFilter(const double obstacle_distance,
//                                       const double safe_distance,
//                                       const double distance_buffer,
//                                       const bool is_obstacle_blocking) {
//   if (is_obstacle_blocking) {
//     return obstacle_distance < safe_distance + distance_buffer;
//   } else {
//     return obstacle_distance < safe_distance - distance_buffer;
//   }
// }

bool LaneChangePath::HysteresisFilter(
        const double obstacle_distance,
        const double safe_distance,
        const double distance_buffer,
        const bool is_obstacle_blocking) {
    // 如果障碍物阻挡道路
    if (is_obstacle_blocking) {
        // 如果障碍物距离小于安全距离加上距离缓冲，返回true
        return obstacle_distance < safe_distance + distance_buffer;
    } else {
        // 如果障碍物距离小于安全距离减去距离缓冲，返回true
        return obstacle_distance < safe_distance - distance_buffer;
    }
}

// void LaneChangePath::SetPathInfo(PathData* const path_data) {
//   std::vector<PathPointDecision> path_decision;
//   PathAssessmentDeciderUtil::InitPathPointDecision(
//       *path_data, PathData::PathPointType::IN_LANE, &path_decision);
//   // Go through every path_point, and add in-lane/out-of-lane info.
//   const auto& discrete_path = path_data->discretized_path();
//   SLBoundary ego_sl_boundary;
//   for (size_t i = 0; i < discrete_path.size(); ++i) {
//     if (!GetSLBoundary(*path_data, i, reference_line_info_, &ego_sl_boundary)) {
//       ADEBUG << "Unable to get SL-boundary of ego-vehicle.";
//       continue;
//     }
//     double lane_left_width = 0.0;
//     double lane_right_width = 0.0;
//     double middle_s =
//         (ego_sl_boundary.start_s() + ego_sl_boundary.end_s()) / 2.0;
//     if (reference_line_info_->reference_line().GetLaneWidth(
//             middle_s, &lane_left_width, &lane_right_width)) {
//       // Rough sl boundary estimate using single point lane width
//       double back_to_inlane_extra_buffer = 0.2;
//       // For lane-change path, only transitioning part is labeled as
//       // out-of-lane.
//       if (ego_sl_boundary.start_l() > lane_left_width ||
//           ego_sl_boundary.end_l() < -lane_right_width) {
//         // This means that ADC hasn't started lane-change yet.
//         std::get<1>((path_decision)[i]) = PathData::PathPointType::IN_LANE;
//       } else if (ego_sl_boundary.start_l() >
//                      -lane_right_width + back_to_inlane_extra_buffer &&
//                  ego_sl_boundary.end_l() <
//                      lane_left_width - back_to_inlane_extra_buffer) {
//         // This means that ADC has safely completed lane-change with margin.
//         std::get<1>((path_decision)[i]) = PathData::PathPointType::IN_LANE;
//       } else {
//         // ADC is right across two lanes.
//         std::get<1>((path_decision)[i]) =
//             PathData::PathPointType::OUT_ON_FORWARD_LANE;
//       }
//     } else {
//       AERROR << "reference line not ready when setting path point guide";
//       return;
//     }
//   }
//   path_data->SetPathPointDecisionGuide(std::move(path_decision));
// }

void LaneChangePath::SetPathInfo(PathData* const path_data) {
    // 创建一个路径点决策向量
    std::vector<PathPointDecision> path_decision;
    // 初始化路径点决策
    PathAssessmentDeciderUtil::InitPathPointDecision(*path_data, PathData::PathPointType::IN_LANE, &path_decision);
    // 获取离散路径
    const auto& discrete_path = path_data->discretized_path();
    // 创建一个SL边界对象
    SLBoundary ego_sl_boundary;
    // 遍历离散路径的每一个点
    for (size_t i = 0; i < discrete_path.size(); ++i) {
        // 获取SL边界
        if (!GetSLBoundary(*path_data, i, reference_line_info_, &ego_sl_boundary)) {
            // 如果无法获取SL边界，则打印调试信息并跳过当前循环
            ADEBUG << "Unable to get SL-boundary of ego-vehicle.";
            continue;
        }
        // 初始化车道左宽和车道右宽
        double lane_left_width = 0.0;
        double lane_right_width = 0.0;
        // 计算中间s值
        double middle_s = (ego_sl_boundary.start_s() + ego_sl_boundary.end_s()) / 2.0;
        // 获取车道宽度
        if (reference_line_info_->reference_line().GetLaneWidth(middle_s, &lane_left_width, &lane_right_width)) {
            // 使用单点车道宽度进行粗略的SL边界估计
            double back_to_inlane_extra_buffer = 0.2;
            // 对于车道更改路径，只有过渡部分被标记为出车道
            if (ego_sl_boundary.start_l() > lane_left_width || ego_sl_boundary.end_l() < -lane_right_width) {
                // 这意味着ADC尚未开始车道更改
                std::get<1>((path_decision)[i]) = PathData::PathPointType::IN_LANE;
            } else if (
                    ego_sl_boundary.start_l() > -lane_right_width + back_to_inlane_extra_buffer
                    && ego_sl_boundary.end_l() < lane_left_width - back_to_inlane_extra_buffer) {
                // 这意味着ADC已经安全地完成了车道更改
                std::get<1>((path_decision)[i]) = PathData::PathPointType::IN_LANE;
            } else {
                // ADC正好在两条车道之间
                std::get<1>((path_decision)[i]) = PathData::PathPointType::OUT_ON_FORWARD_LANE;
            }
        } else {
            // 当设置路径点指南时，参考线还未准备好
            AERROR << "reference line not ready when setting path point guide";
            return;
        }
    }
    // 设置路径点决策指南
    path_data->SetPathPointDecisionGuide(std::move(path_decision));
}

// bool LaneChangePath::CheckLastFrameSucceed(
//     const apollo::planning::Frame* const last_frame) {
//   if (last_frame) {
//     for (const auto& reference_line_info : last_frame->reference_line_info()) {
//       if (!reference_line_info.IsChangeLanePath()) {
//         continue;
//       }
//       const auto history_trajectory_type =
//           reference_line_info.trajectory_type();
//       if (history_trajectory_type == ADCTrajectory::SPEED_FALLBACK) {
//         return false;
//       }
//     }
//   }
//   return true;
// }
// 检查上一帧是否成功完成车道变换

bool LaneChangePath::CheckLastFrameSucceed(const apollo::planning::Frame* const last_frame) {
    // 如果存在上一帧
    if (last_frame) {
        // 遍历上一帧中的所有参考线信息
        for (const auto& reference_line_info : last_frame->reference_line_info()) {
            // 如果当前参考线不是车道变换路径，跳过当前循环
            if (!reference_line_info.IsChangeLanePath()) {
                continue;
            }
            // 获取历史轨迹类型
            const auto history_trajectory_type = reference_line_info.trajectory_type();
            // 如果历史轨迹类型是速度回退，返回false
            if (history_trajectory_type == ADCTrajectory::SPEED_FALLBACK) {
                return false;
            }
        }
    }
    // 如果没有返回false，说明上一帧成功完成了车道变换，返回true
    return true;
}

}  // namespace planning
}  // namespace apollo

勾魂凉皮
关注
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值