diff --git a/.gitignore b/.gitignore deleted file mode 100644 index d919135a5f4..00000000000 --- a/.gitignore +++ /dev/null @@ -1,87 +0,0 @@ -# Bazel -bazel-* - -# Temporary files -*.pyc -*.swp -*.swo - -# Generated files -*.result -py_proto - -# Eclipse IDE files -.project -.cproject -.settings -.classpath - -# IntelliJ files -.idea - -# VS Code files -.vscode - -# Atom IDE files -*.gch - -# Logs -nohup.out -*.bak -*.stderr -*.stdout - -# Editor configs -.ycm_extra_conf.py - -# Data -/data -modules/tools/ota/update.ini -modules/tools/ota/vehicle_info.pb.txt -docker/build/syspkgs.txt - -# Localization -modules/localization/msf/local_map/test_data/ndt_map/map_data - -# Doxygen -cyber/doxy-docs/build -cyber/doxy-docs/xml - -# Esd can lib -third_party/can_card_library/esd_can/include -third_party/can_card_library/esd_can/lib - -# Map data files -modules/map/data - -# python proto -*_pb2.py - -# calibration files -modules/calibration/data - -# Data bag -docs/demo_guide/*.bag -docs/demo_guide/*.record - -# Bash Environment -.dev_bash_hist - -# bazel cache and others -.cache/ -.apollo.bazelrc -.errors.log - -# Directory for binary distribution -output - -# Flamegraph files -tools/FlameGraph - -# jetbrain profiler -.DS_Store - -# dv plugins -modules/studio_connector - -#WORKSPACE diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000000..6662c1b65b6 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,95 @@ +{ + "files.associations": { + "tuple": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "complex": "cpp", + "cstdint": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "optional": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "type_traits": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "memory": "cpp", + "new": "cpp", + "ostream": "cpp", + "numeric": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "cinttypes": "cpp", + "utility": "cpp", + "typeinfo": "cpp", + "iterator": "cpp", + "memory_resource": "cpp", + "random": "cpp", + "string": "cpp", + "bit": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "condition_variable": "cpp", + "deque": "cpp", + "fstream": "cpp", + "mutex": "cpp", + "ratio": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "map": "cpp", + "set": "cpp", + "shared_mutex": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "any": "cpp", + "regex": "cpp", + "future": "cpp", + "variant": "cpp", + "forward_list": "cpp", + "list": "cpp", + "valarray": "cpp", + "bitset": "cpp", + "csignal": "cpp" + }, + "C_Cpp.errorSquiggles": "disabled", + + // ✅ 护眼配色部分从这里开始 + "workbench.colorCustomizations": { + "editor.background": "#E8F5E9", // 浅绿色背景（柔和明亮） + "editor.foreground": "#2E4A2E", // 深墨绿色文字 + "activityBar.background": "#DDEEDD", + "sideBar.background": "#E3F2E3", + "statusBar.background": "#CDE5CD", + "editorLineNumber.foreground": "#6B8E6B", + "editorCursor.foreground": "#2E7D32", // 墨绿光标 + "editor.selectionBackground": "#C5E1C5", // 柔和选中高亮 + "editor.inactiveSelectionBackground": "#D0E6D0", + "editor.lineHighlightBackground": "#DDF0DD", + "terminal.foreground": "#2E4A2E", + "terminal.background": "#E8F5E9" + }, + "editor.fontFamily": "JetBrains Mono, Fira Code, Consolas, monospace", + "editor.fontSize": 14, + "editor.lineHeight": 1.6, + "editor.fontLigatures": true +} diff --git a/1.cpp b/1.cpp new file mode 100644 index 00000000000..ba41a8af03f --- /dev/null +++ b/1.cpp @@ -0,0 +1,129 @@ +bool Planning::FillPlanningResult( + const apollo::planning::Frame& frame, + const std::vector& stitching_trajectory, + bestway::pnc::planning::ADCTrajectory* const planning_result) { + + if (frame.reference_line_info().empty()) { + BERROR << "No reference line info in frame."; + return false; + } + + const auto& reference_line_info = frame.reference_line_info().front(); + const auto& trajectory = reference_line_info.trajectory(); + RETURN_VAL_IF(trajectory.empty(), false); + + BINFO << "Final traj length: " << trajectory.GetSpatialLength() + << ", path length: " << reference_line_info.path_data().discretized_path().Length(); + + VehicleSignal::TurnSignal desired_turn_signal = VehicleSignal::TURN_NONE; + + constexpr double kMaxLookaheadDist = 10.0; // 沿参考线看10米 + constexpr double kMinTurnIntensity = 0.2; // 总曲率强度阈值 + constexpr double kDominanceRatio = 1.2; // 主导方向需强20% + + if (!trajectory.empty()) { + const auto& reference_line = reference_line_info.reference_line(); + + const auto& ego_path_point = trajectory.front().path_point(); + apollo::common::SLPoint ego_sl; + if (!reference_line.XYToSL({ego_path_point.x(), ego_path_point.y()}, &ego_sl)) { + BERROR << "Failed to project ego pose to reference line for turn signal logic."; + } else { + double ego_s = ego_sl.s(); + double left_turn_intensity = 0.0; + double right_turn_intensity = 0.0; + bool has_valid_kappa = false; + double prev_s = ego_s; // 上一个有效点的 s + + for (size_t i = 0; i < trajectory.size(); ++i) { + const auto& pt = trajectory[i]; + if (!pt.path_point().has_kappa()) continue; + + apollo::common::SLPoint curr_sl; + if (!reference_line.XYToSL({pt.path_point().x(), pt.path_point().y()}, &curr_sl)) { + continue; + } + + double curr_s = curr_sl.s(); + // 忽略后方点 + if (curr_s < ego_s) continue; + + // 超出预瞄距离，停止 + if (curr_s - ego_s > kMaxLookaheadDist) break; + + double ds = curr_s - prev_s; + if (ds < 1e-3) { + // 点太近，不积分，但更新 prev_s 防止累积误差 + prev_s = curr_s; + continue; + } + + double kappa = pt.path_point().kappa(); + if (kappa > 0) { + left_turn_intensity += kappa * ds; + } else if (kappa < 0) { + right_turn_intensity += (-kappa) * ds; + } + + prev_s = curr_s; + has_valid_kappa = true; + } + + // 判断是否打灯 + if (has_valid_kappa) { + double total_intensity = left_turn_intensity + right_turn_intensity; + if (total_intensity >= kMinTurnIntensity) { + if (left_turn_intensity > right_turn_intensity * kDominanceRatio) { + desired_turn_signal = VehicleSignal::TURN_LEFT; + } else if (right_turn_intensity > left_turn_intensity * kDominanceRatio) { + desired_turn_signal = VehicleSignal::TURN_RIGHT; + } + } + } + } + } + + // --- 接近目标点时取消转向灯 --- + double delta_s_to_goal = std::numeric_limits::max(); + const auto& goal = frame.goal(); + const auto& reference_line = frame.reference_line_info().front().reference_line(); + const auto& adc_pose = trajectory.front().path_point(); + + apollo::common::SLPoint adc_sl, goal_sl; + if (reference_line.XYToSL({adc_pose.x(), adc_pose.y()}, &adc_sl) && + reference_line.XYToSL({goal.x(), goal.y()}, &goal_sl)) { + delta_s_to_goal = goal_sl.s() - adc_sl.s(); + } + BERROR << "delta_s_to_goal: " << delta_s_to_goal; + + constexpr double kTurnSignalCancelDistAlongS = 8.0; + if (delta_s_to_goal > 0 && delta_s_to_goal < kTurnSignalCancelDistAlongS) { + BERROR << "Near goal (Δs=" << delta_s_to_goal << "m), turn signal canceled."; + desired_turn_signal = VehicleSignal::TURN_NONE; + } + + for (size_t i = 0; i < stitching_trajectory.size(); ++i) { + auto* point = planning_result->add_trajectory_point(); + ConvertApolloToBestwayPoint(stitching_trajectory[i], point); + } + + for (const auto& apollo_point : trajectory) { + auto* point = planning_result->add_trajectory_point(); + ConvertApolloToBestwayPoint(apollo_point, point); + } + + switch (desired_turn_signal) { + case VehicleSignal::TURN_LEFT: + planning_result->set_turn_signal(bestway::pnc::TurnSignal::TURN_LEFT); + break; + case VehicleSignal::TURN_RIGHT: + planning_result->set_turn_signal(bestway::pnc::TurnSignal::TURN_RIGHT); + break; + default: + planning_result->set_turn_signal(bestway::pnc::TurnSignal::TURN_NONE); + break; + } + + planning_result->set_gear(bestway::pnc::GearPosition::GEAR_D); + return true; +} \ No newline at end of file diff --git a/apollo.sh b/apollo.sh index 730330477f0..07435ea99ec 100755 --- a/apollo.sh +++ b/apollo.sh @@ -1,20 +1,36 @@ +## 指定使用 bash 作为解释器执行该脚本 #! /usr/bin/env bash +# 在脚本执行过程中，任何一个命令如果返回非零值（即执行失败），脚本就会立即退出，以防止错误继续传播 set -e +# 计算当前脚本所在的绝对路径，即 Apollo 项目根目录 +# BASH_SOURCE[0] 代表当前脚本的路径 +# dirname "${BASH_SOURCE[0]}" 获取脚本所在目录 +# cd "$(dirname "${BASH_SOURCE[0]}")" 进入该目录 +# pwd -P 获取当前目录的绝对路径（防止软链接影响） TOP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd -P)" +# 加载 Apollo 的 bashrc 配置文件，初始化环境变量 source "${TOP_DIR}/scripts/apollo.bashrc" -ARCH="$(uname -m)" -SUPPORTED_ARCHS=" x86_64 aarch64 " -APOLLO_VERSION="@non-git" -APOLLO_ENV="" - -USE_ESD_CAN=false -: ${STAGE:=dev} +ARCH="$(uname -m)" # 获取当前 CPU 架构（例如 x86_64 或 aarch64） +SUPPORTED_ARCHS=" x86_64 aarch64 " # 定义 Apollo 仅支持的 CPU 架构 +APOLLO_VERSION="@non-git" # 默认 Apollo 版本号，稍后会动态更新 +APOLLO_ENV="" # Apollo 运行环境变量 +USE_ESD_CAN=false # 是否使用 ESD CAN 设备，默认 false +: ${STAGE:=dev} # 如果 STAGE 没有被定义，则默认设置为 dev（开发环境） +# AVAILABLE_COMMANDS：定义 Apollo 支持的命令，例如 build（编译）、test（运行测试）、clean（清理编译缓存）等 +# build：编译 Apollo +# test：运行单元测试 +# clean：清理构建产物 +# lint：代码风格检查 +# format：格式化代码 +# help：显示帮助信息 AVAILABLE_COMMANDS="config build build_dbg build_opt build_cpu build_gpu build_opt_gpu test coverage lint \ buildify check build_fe build_teleop build_prof doc clean format usage -h --help" +# check_architecture_support()：检查当前系统的 CPU 架构是否被 Apollo 支持 +# if [[ "${SUPPORTED_ARCHS}" != *" ${ARCH} "* ]]; then ... exit 1：如果 CPU 架构不在支持列表内，输出错误信息并终止脚本 function check_architecture_support() { if [[ "${SUPPORTED_ARCHS}" != *" ${ARCH} "* ]]; then error "Unsupported CPU arch: ${ARCH}. Currently, Apollo only" \ @@ -23,7 +39,8 @@ function check_architecture_support() { exit 1 fi } - +# check_platform_support()：检查操作系统是否为 Linux，否则终止脚本 +# uname -s 获取 当前操作系统名称 function check_platform_support() { local platform="$(uname -s)" if [[ "${platform}" != "Linux" ]]; then @@ -32,7 +49,10 @@ function check_platform_support() { exit 1 fi } - +# check_minimal_memory_requirement()：检查系统内存是否小于 2GB，并发出警告 +# free -m：获取系统内存（单位：MB） +# awk '/Mem:/ {printf("%0.2f", $2 / 1024.0)}'：获取总内存值，并转换为 GB +# bc -l：使用 浮点运算 判断 actual_mem_gb < minimal_mem_gb，如果 内存不足 2GB，则 警告 用户 function check_minimal_memory_requirement() { local minimal_mem_gb="2.0" local actual_mem_gb="$(free -m | awk '/Mem:/ {printf("%0.2f", $2 / 1024.0)}')" @@ -41,7 +61,7 @@ function check_minimal_memory_requirement() { "[${minimal_mem_gb}G]. Apollo build could fail." fi } - +# determine_esdcan_use()：检测 esd_can 驱动文件是否存在，并决定是否启用 ESD CAN 设备 function determine_esdcan_use() { local esdcan_dir="${APOLLO_ROOT_DIR}/third_party/can_card_library/esd_can" local use_esd=false @@ -52,7 +72,7 @@ function determine_esdcan_use() { fi USE_ESD_CAN="${use_esd}" } - +# 检查 Apollo 版本 function check_apollo_version() { local branch="$(git_branch)" if [ "${branch}" == "${APOLLO_VERSION}" ]; then @@ -62,15 +82,15 @@ function check_apollo_version() { local stamp="$(git_date)" APOLLO_VERSION="${branch}-${stamp}-${sha1}" } - +# apollo_env_setup()：依次调用 check_apollo_version()、check_architecture_support() 等函数，确保 Apollo 运行环境正确 function apollo_env_setup() { - check_apollo_version + check_apollo_version #检查 Apollo 版本 - check_architecture_support - check_platform_support - check_minimal_memory_requirement - setup_gpu_support - determine_esdcan_use + check_architecture_support # 检查 CPU 架构 + check_platform_support # 检查操作系统 + check_minimal_memory_requirement # 检查内存 + setup_gpu_support # 设置 GPU 支持 + determine_esdcan_use # 检查 CAN 总线设备 APOLLO_ENV="STAGE=${STAGE}" APOLLO_ENV="${APOLLO_ENV} USE_ESD_CAN=${USE_ESD_CAN}" @@ -100,6 +120,7 @@ function apollo_env_setup() { } #TODO(all): Update node modules +# build_dreamview_frontend()：进入 dreamview/frontend 目录，运行 yarn build 构建前端代码 function build_dreamview_frontend() { pushd "${APOLLO_ROOT_DIR}/modules/dreamview/frontend" > /dev/null yarn build @@ -164,12 +185,16 @@ function check_config_cpu() { fi } +# main()：主函数，根据用户输入的命令执行对应的 Apollo 任务 +# ./apollo.sh build -> 运行 apollo_build.sh 进行编译 +# ./apollo.sh test -> 运行 apollo_test.sh 进行单元测试 +# ./apollo.sh clean -> 清理编译产物 function main() { if [ "$#" -eq 0 ]; then _usage exit 0 fi - +# 先运行 apollo_env_setup 初始化环境 check_config_cpu "$@" apollo_env_setup @@ -180,51 +205,67 @@ function main() { local coverage_sh="${APOLLO_ROOT_DIR}/scripts/apollo_coverage.sh" local ci_sh="${APOLLO_ROOT_DIR}/scripts/apollo_ci.sh" - local cmd="$1" - shift + local cmd="$1" #获取第一个命令行参数，并存入变量 cmd + shift # 左移参数列表，删除第一个参数，使 $2 变为 $1，$3 变为 $2 case "${cmd}" in +# config 命令调用 apollo_config.sh 脚本进行环境配置。 +# env ${APOLLO_ENV}：设置 APOLLO_ENV 环境变量。 +# "$@"：传递所有剩余的参数 config) env ${APOLLO_ENV} bash "${APOLLO_ROOT_DIR}/scripts/apollo_config.sh" "$@" ;; build) env ${APOLLO_ENV} bash "${build_sh}" "$@" ;; + # 优化编译（--config=opt） build_opt) env ${APOLLO_ENV} bash "${build_sh}" --config=opt "$@" ;; + # 调试模式编译（--config=dbg） build_dbg) env ${APOLLO_ENV} bash "${build_sh}" --config=dbg "$@" ;; + # CPU 版本编译（--config=cpu） build_cpu) env ${APOLLO_ENV} bash "${build_sh}" --config=cpu "$@" ;; + # GPU 版本编译（--config=gpu） build_gpu) env ${APOLLO_ENV} bash "${build_sh}" --config=gpu "$@" ;; + # 针对 Nvidia GPU 进行编译（--config=nvidia） build_nvidia) env ${APOLLO_ENV} bash "${build_sh}" --config=nvidia "$@" ;; + # 针对 AMD GPU 进行编译（--config=amd） build_amd) env ${APOLLO_ENV} bash "${build_sh}" --config=amd "$@" ;; + # 优化模式 + GPU 编译 build_opt_gpu) env ${APOLLO_ENV} bash "${build_sh}" --config=opt --config=gpu "$@" ;; + # 优化 + GPU，仅编译 cyber、planning、prediction 等关键模块 build_opt_gpu_pnc) env ${APOLLO_ENV} bash "${build_sh}" --config=opt --config=gpu "cyber planning prediction control routing dreamview external_command tools common_msgs" ;; + # 调试模式 + GPU，仅编译 PNC 相关模块 build_pnc) env ${APOLLO_ENV} bash "${build_sh}" --config=dbg --config=gpu "cyber planning prediction control routing dreamview external_command tools common_msgs" ;; + # 编译指定的单个软件包 build_pkg) env ${APOLLO_ENV} bash "${build_pkg_sh}" "$@" ;; + # 优化模式编译单个包 build_pkg_opt) env ${APOLLO_ENV} bash "${build_pkg_sh}" --config=opt "$@" ;; + # 优化 + GPU 编译单个包 build_pkg_opt_gpu) env ${APOLLO_ENV} bash "${build_pkg_sh}" --config=opt --config=gpu "$@" ;; + # 调试模式编译单个包 build_pkg_dbg) env ${APOLLO_ENV} bash "${build_pkg_sh}" --config=dbg "$@" ;; @@ -234,60 +275,78 @@ function main() { build_opt_amd) env ${APOLLO_ENV} bash "${build_sh}" --config=opt --config=amd "$@" ;; + # 性能分析模式编译（--config=prof） build_prof) env ${APOLLO_ENV} bash "${build_sh}" --config=prof "$@" ;; + # 远程驾驶模式编译（--config=teleop） build_teleop) env ${APOLLO_ENV} bash "${build_sh}" --config=teleop "$@" ;; + # 编译 DreamView 前端 build_fe) build_dreamview_frontend ;; + # 运行单元测试（--config=unit_test） test) env ${APOLLO_ENV} bash "${test_sh}" --config=unit_test "$@" ;; + # 运行代码覆盖率测试 coverage) env ${APOLLO_ENV} bash "${coverage_sh}" "$@" ;; + # CI 编译 cibuild) env ${APOLLO_ENV} bash "${ci_sh}" "build" ;; + # CI 测试 citest) env ${APOLLO_ENV} bash "${ci_sh}" "test" ;; + # CI 代码风格检查（Lint） cilint) env ${APOLLO_ENV} bash "${ci_sh}" "lint" ;; + # 编译、测试和 Lint 一起执行 check) build_test_and_lint ;; + # 格式化 Bazel 相关文件 buildify) env ${APOLLO_ENV} bash "${APOLLO_ROOT_DIR}/scripts/apollo_buildify.sh" ;; + # 运行代码风格检查 lint) env ${APOLLO_ENV} bash "${APOLLO_ROOT_DIR}/scripts/apollo_lint.sh" "$@" ;; + # 清理编译生成的文件 clean) env ${APOLLO_ENV} bash "${APOLLO_ROOT_DIR}/scripts/apollo_clean.sh" "$@" ;; + # 执行发布操作 release) env ${APOLLO_ENV} bash "${APOLLO_ROOT_DIR}/scripts/apollo_release.sh" "$@" ;; + # 安装 DreamView 插件 install_dv_plugins) env ${APOLLO_ENV} bash "${APOLLO_ROOT_DIR}/scripts/install_dv_plugins.sh" ;; + # 生成 Apollo 文档 doc) env ${APOLLO_ENV} bash "${APOLLO_ROOT_DIR}/scripts/apollo_docs.sh" "$@" ;; + # 格式化代码 format) env ${APOLLO_ENV} bash "${APOLLO_ROOT_DIR}/scripts/apollo_format.sh" "$@" ;; + # usage 或 -h / --help：显示帮助信息 usage) _usage ;; -h | --help) _usage ;; + # 如果 cmd 不匹配任何已定义的命令，则执行 _check_command 进行检查 *) _check_command "${cmd}" ;; diff --git a/cyber/class_loader/class_loader_manager.cc b/cyber/class_loader/class_loader_manager.cc index 59bb39a4c86..ffba13ef1b4 100644 --- a/cyber/class_loader/class_loader_manager.cc +++ b/cyber/class_loader/class_loader_manager.cc @@ -36,9 +36,13 @@ std::vector ClassLoaderManager::GetAllValidClassLoaders() { return class_loaders; } +/// @brief 获取所有有效的库路径 +/// @return std::vector ClassLoaderManager::GetAllValidLibPath() { - std::vector libpath; + std::vector libpath; // 存储有效的库路径 for (auto& lib_class_loader : libpath_loader_map_) { + // libpath_loader_map_ 应该是一个映射（例如 std::map 或 std::unordered_map）， + // 其键是库路径（std::string 类型），值是某种类的实例（可能是指向类加载器的指针） if (lib_class_loader.second != nullptr) { libpath.emplace_back(lib_class_loader.first); } @@ -46,15 +50,23 @@ std::vector ClassLoaderManager::GetAllValidLibPath() { return libpath; } +/// @brief 检查给定的库名称 library_name 是否存在于有效的库路径列表中 +/// @param library_name 需要检查的库名称 +/// @return bool ClassLoaderManager::IsLibraryValid(const std::string& library_name) { + // 存储了所有被认为是有效的库路径 std::vector valid_libraries = GetAllValidLibPath(); return (valid_libraries.end() != std::find(valid_libraries.begin(), valid_libraries.end(), library_name)); } +/// @brief +/// @param library_path +/// @return bool ClassLoaderManager::LoadLibrary(const std::string& library_path) { std::lock_guard lck(libpath_loader_map_mutex_); + // 检查给定路径的库是否有效 if (!IsLibraryValid(library_path)) { libpath_loader_map_[library_path] = new class_loader::ClassLoader(library_path); diff --git a/cyber/common/file.cc b/cyber/common/file.cc index 29b1930629d..f783f8dbb8d 100644 --- a/cyber/common/file.cc +++ b/cyber/common/file.cc @@ -65,25 +65,32 @@ bool SetProtoToASCIIFile(const google::protobuf::Message &message, } return SetProtoToASCIIFile(message, fd); } - +/// @brief +/// @param file_name 文件路径 +/// @param message protobuf 解析结果存储对象 +/// @return bool GetProtoFromASCIIFile(const std::string &file_name, google::protobuf::Message *message) { using google::protobuf::TextFormat; using google::protobuf::io::FileInputStream; using google::protobuf::io::ZeroCopyInputStream; + // open() 以 只读模式 (O_RDONLY) 打开 file_name int file_descriptor = open(file_name.c_str(), O_RDONLY); - if (file_descriptor < 0) { + if (file_descriptor < 0) { // 打开失败 AERROR << "Failed to open file " << file_name << " in text mode."; // Failed to open; return false; } - + // Google Protobuf 提供的 流式读取工具，适用于 大文件 解析 + // ZeroCopyInputStream 避免不必要的数据拷贝，提高性能 ZeroCopyInputStream *input = new FileInputStream(file_descriptor); bool success = TextFormat::Parse(input, message); if (!success) { AERROR << "Failed to parse file " << file_name << " as text proto."; } + // 释放 FileInputStream 对象，防止内存泄漏 delete input; + // close(file_descriptor); 关闭文件描述符，防止文件句柄泄漏 close(file_descriptor); return success; } @@ -108,14 +115,22 @@ bool GetProtoFromBinaryFile(const std::string &file_name, } return true; } - +/// @brief 从文件中读取 protobuf 数据，并解析到 message 对象中 +/// @param file_name protobuf 配置文件的路径 +/// @param message 指向 protobuf 消息对象，用于存储解析后的数据 +/// @return bool GetProtoFromFile(const std::string &file_name, google::protobuf::Message *message) { + // 检查文件是否存在 if (!PathExists(file_name)) { AERROR << "File [" << file_name << "] does not exist! "; return false; } // Try the binary parser first if it's much likely a binary proto. + // 判断是否是二进制 .bin 文件 + // 检查文件扩展名是否为 .bin + // kBinExt.rbegin() 代表 ".bin" 反向迭代器（即从 n 开始向前匹配） + // file_name.rbegin() 代表 file_name 反向迭代器（即从 file_name 末尾向前匹配） static const std::string kBinExt = ".bin"; if (std::equal(kBinExt.rbegin(), kBinExt.rend(), file_name.rbegin())) { return GetProtoFromBinaryFile(file_name, message) || diff --git a/cyber/common/file.h b/cyber/common/file.h index eee9a97223e..8df9a9407fd 100644 --- a/cyber/common/file.h +++ b/cyber/common/file.h @@ -250,12 +250,19 @@ bool DeleteFile(const std::string &filename); bool GetType(const std::string &filename, FileType *type); bool CreateDir(const std::string &dir); - +/// @brief +/// @tparam T 用于泛化支持任何类型的 protobuf +/// @param relative_path 配置文件的相对路径 +/// @param config 指向 protobuf 对象的指针，用于接收解析后的配置 +/// @return template bool LoadConfig(const std::string &relative_path, T *config) { + // 断言 config 不为 nullptr，使用的是 Google 的 glog 或 gflags 中的宏，常用于调试 CHECK_NOTNULL(config); // todo: get config base relative path + // 存储解析后的真实路径 std::string actual_config_path; + // 一个工具函数，试图在环境变量 APOLLO_CONF_PATH 指定的路径中拼接出 relative_path if (!GetFilePathWithEnv(relative_path, "APOLLO_CONF_PATH", &actual_config_path)) { AERROR << "conf file [" << relative_path @@ -263,6 +270,7 @@ bool LoadConfig(const std::string &relative_path, T *config) { return false; } AINFO << "load conf file: " << actual_config_path; + // 调用 GetProtoFromFile 来读取 protobuf 文件内容并解析到 config 对象中，返回其结果 return GetProtoFromFile(actual_config_path, config); } diff --git a/cyber/component/component_base.h b/cyber/component/component_base.h index 42766da47f4..e9048275692 100644 --- a/cyber/component/component_base.h +++ b/cyber/component/component_base.h @@ -58,9 +58,13 @@ class ComponentBase : public std::enable_shared_from_this { } scheduler::Instance()->RemoveTask(node_->Name()); } - + /// @brief 读取配置文件并解析成 Proto 配置对象 + /// @tparam T + /// @param config + /// @return template bool GetProtoConfig(T* config) const { + // 读取 config_file_path_（配置文件路径）中的内容，并填充 config 对象 return common::GetProtoFromFile(config_file_path_, config); } diff --git a/cyber/plugin_manager/plugin_manager.cc b/cyber/plugin_manager/plugin_manager.cc index b0f64d538f6..bdd789259f0 100644 --- a/cyber/plugin_manager/plugin_manager.cc +++ b/cyber/plugin_manager/plugin_manager.cc @@ -192,7 +192,11 @@ bool PluginManager::LoadInstalledPlugins() { return true; } +/// @brief +/// @param library_path 库文件的路径 +/// @return bool PluginManager::LoadLibrary(const std::string& library_path) { + // 尝试加载指定路径的插件库 if (!class_loader_manager_.LoadLibrary(library_path)) { AWARN << "plugin library[" << library_path << "] load failed"; return false; diff --git a/cyber/plugin_manager/plugin_manager.h b/cyber/plugin_manager/plugin_manager.h index a45b2aa6417..de70d075c39 100644 --- a/cyber/plugin_manager/plugin_manager.h +++ b/cyber/plugin_manager/plugin_manager.h @@ -146,6 +146,10 @@ class PluginManager { static PluginManager* instance_; }; +/// @brief 用于 创建插件类的实例，并返回一个 std::shared_ptr 指针 +/// @tparam Base 基类类型，表示插件的基本类型 +/// @param class_name 插件的具体类名（字符串） +/// @return std::shared_ptr：插件实例的 智能指针 template std::shared_ptr PluginManager::CreateInstance( const std::string& derived_class) { @@ -188,51 +192,78 @@ std::string PluginManager::GetPluginClassHomePath( return ""; } +/// @brief 根据插件类的名称和配置文件名称，返回插件配置文件的路径 +/// @tparam Base +/// @param class_name 插件类的名称 +/// @param conf_name 配置文件的名称 +/// @return 插件配置文件的路径 template std::string PluginManager::GetPluginConfPath(const std::string& class_name, const std::string& conf_name) { +// 获取插件类的主路径（即插件的根目录路径）。它将插件类名 class_name 作为参数传递，并且 Base 是模板参数类型 std::string plugin_home_path = GetPluginClassHomePath(class_name); + // 测路径是否为绝对路径 if (apollo::cyber::common::PathIsAbsolute(plugin_home_path)) { // can not detect the plugin relative path + // 如果插件路径是绝对路径，则输出警告信息，表示无法检测插件的相对路径，并且配置文件路径将相对于插件描述文件进行解析 AWARN << "plugin of class " << class_name << " load from absolute path, " << "conf path will be relative to it's description file"; } - +// 将插件主路径 plugin_home_path 和配置文件名 conf_name 拼接成一个相对的配置文件路径 relative_conf_path std::string relative_conf_path = plugin_home_path + "/" + conf_name; - std::string actual_conf_path; + std::string actual_conf_path; // 存储实际的配置文件路径‘ +// 根据环境变量 APOLLO_CONF_PATH 查找实际的配置文件路径。如果能找到配置文件，函数返回 true，并将实际路径存储在 actual_conf_path 中 if (apollo::cyber::common::GetFilePathWithEnv( relative_conf_path, "APOLLO_CONF_PATH", &actual_conf_path)) { return actual_conf_path; } +// 如果 GetFilePathWithEnv 没有找到配置文件路径，函数将返回由插件主路径 plugin_home_path 和配置文件名 conf_name 拼接而成的默认路径 return plugin_home_path + "/" + conf_name; } +/// @brief 检查一个插件库是否已经加载 +/// @tparam Base 模板类型参数，表示插件的基类 +/// @param class_name 传入的类名，用来在插件中查找特定的类 +/// @return template bool PluginManager::IsLibraryLoaded(const std::string& class_name) { - int status = 0; + int status = 0; // 接收 abi::__cxa_demangle 的返回状态（该函数用来获取类型名称的可读格式） +// typeid(Base).name() 获取 Base 类型的原始名称（通常是经过编译器编码的）， +// 然后通过 abi::__cxa_demangle 转换为人类可读的类型名称，并赋值给 base_class_name std::string base_class_name = abi::__cxa_demangle(typeid(Base).name(), 0, 0, &status); +// 在 plugin_class_plugin_name_map_ 中查找一个由 class_name 和 base_class_name 组成的键值对 if (plugin_class_plugin_name_map_.find({class_name, base_class_name}) == plugin_class_plugin_name_map_.end()) { // not found return false; } +// 如果找到了插件，取出插件的名字，赋值给 plugin_name std::string plugin_name = plugin_class_plugin_name_map_[{class_name, base_class_name}]; +// 在 plugin_loaded_map_ 中查找 plugin_name 是否存在。如果未找到，表示该插件未加载 if (plugin_loaded_map_.find(plugin_name) == plugin_loaded_map_.end()) { // not found return false; } - + // 返回该插件的加载状态值 return plugin_loaded_map_[plugin_name]; } +/// @brief 检查并加载指定的插件库 +/// @tparam Base +/// @param class_name +// 接受一个类型 Base 和一个参数 class_name，该参数是插件类的名称，返回一个布尔值，表示插件库是否成功加载 +/// @return template bool PluginManager::CheckAndLoadPluginLibrary(const std::string& class_name) { + // 检查是否已经加载了指定的插件库 if (IsLibraryLoaded(class_name)) { return true; } int status = 0; + // typeid(Base).name() 获取 Base 类型的名字，然后通过 abi::__cxa_demangle 函数将其转化为人类可读的格式， + // 存储在 base_class_name 变量中。如果转换失败，status 会被设置为非零值 std::string base_class_name = abi::__cxa_demangle(typeid(Base).name(), 0, 0, &status); if (plugin_class_plugin_name_map_.find({class_name, base_class_name}) == diff --git a/modules/common/configs/config_gflags.cc b/modules/common/configs/config_gflags.cc index a63d93d0979..bbedc5fdb16 100644 --- a/modules/common/configs/config_gflags.cc +++ b/modules/common/configs/config_gflags.cc @@ -15,11 +15,11 @@ *****************************************************************************/ #include "modules/common/configs/config_gflags.h" - +// map_dir DEFINE_string(map_dir, "/apollo/modules/map/data/sunnyvale_loop", "Directory which contains a group of related maps."); DEFINE_int32(local_utm_zone_id, 10, "UTM zone id."); - +// hd_map DEFINE_string(test_base_map_filename, "", "If not empty, use this test base map files."); diff --git a/modules/common/math/box2d.cc b/modules/common/math/box2d.cc index f0d4002d929..555574558a3 100644 --- a/modules/common/math/box2d.cc +++ b/modules/common/math/box2d.cc @@ -50,6 +50,11 @@ double PtSegDistance(double query_x, double query_y, double start_x, } // namespace +/// @brief 初始化一个有中心、旋转角度、长宽属性的矩形盒子，并确保输入合法 +/// @param center 设置矩形的中心位置 +/// @param heading 设置矩形的旋转角度 +/// @param length 设置矩形的长度 +/// @param width 设置矩形的宽度 Box2d::Box2d(const Vec2d ¢er, const double heading, const double length, const double width) : center_(center), @@ -60,8 +65,10 @@ Box2d::Box2d(const Vec2d ¢er, const double heading, const double length, heading_(heading), cos_heading_(cos(heading)), sin_heading_(sin(heading)) { + // 检查矩形的长度和宽度是否大于一个非常小的常量kMathEpsilon，以防负值或无效输入 CHECK_GT(length_, -kMathEpsilon); CHECK_GT(width_, -kMathEpsilon); + // 初始化矩形的四个角的坐标 InitCorners(); } @@ -95,7 +102,7 @@ Box2d::Box2d(const LineSegment2d &axis, const double width) CHECK_GT(width_, -kMathEpsilon); InitCorners(); } - +/// @brief 初始化矩形盒子的四个角点，并计算出矩形在二维空间中的最大和最小 x、y 坐标 void Box2d::InitCorners() { const double dx1 = cos_heading_ * half_length_; const double dy1 = sin_heading_ * half_length_; @@ -344,19 +351,20 @@ bool Box2d::HasOverlap(const Box2d &box) const { box.min_y() > max_y()) { return false; } - + // 相对位移 const double shift_x = box.center_x() - center_.x(); const double shift_y = box.center_y() - center_.y(); - + // 当前矩形四个角点相对于中心的偏移量 const double dx1 = cos_heading_ * half_length_; const double dy1 = sin_heading_ * half_length_; const double dx2 = sin_heading_ * half_width_; const double dy2 = -cos_heading_ * half_width_; + // box矩形的四个角相对于中心的偏移 const double dx3 = box.cos_heading() * box.half_length(); const double dy3 = box.sin_heading() * box.half_length(); const double dx4 = box.sin_heading() * box.half_width(); const double dy4 = -box.cos_heading() * box.half_width(); - + // SAT：如果存在一条轴使得两个形状在该轴上没有重叠，那么两个形状就不相交 return std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <= std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) + std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) + diff --git a/modules/common/math/cartesian_frenet_conversion.cc b/modules/common/math/cartesian_frenet_conversion.cc index f4fa38b4bc5..db565f81aaa 100644 --- a/modules/common/math/cartesian_frenet_conversion.cc +++ b/modules/common/math/cartesian_frenet_conversion.cc @@ -31,35 +31,36 @@ void CartesianFrenetConverter::cartesian_to_frenet( const double v, const double a, const double theta, const double kappa, std::array* const ptr_s_condition, std::array* const ptr_d_condition) { + // 计算目标点(x，y)与参考路径点(rx,ry)的坐标差 const double dx = x - rx; const double dy = y - ry; - + // 计算参考路径方向的余弦和正弦值 const double cos_theta_r = std::cos(rtheta); const double sin_theta_r = std::sin(rtheta); - + // 计算目标点相对于参考路径的横向距离 const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx; ptr_d_condition->at(0) = std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd); - + // 计算目标点与参考路径之间的角度差 const double delta_theta = theta - rtheta; const double tan_delta_theta = std::tan(delta_theta); const double cos_delta_theta = std::cos(delta_theta); - + // 计算路径曲率对横向距离的影响，并存储在d_condition[1]中 const double one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0); ptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta; - + // 计算曲率的导数，表示路径曲率对横向偏差变化的影响 const double kappa_r_d_prime = rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1); - + // 计算横向加速度，并存储在d_condition[2]中 ptr_d_condition->at(2) = -kappa_r_d_prime * tan_delta_theta + one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta * (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa); - + // 将参考路径的弧长（rs）存储在s_condition[0]中 ptr_s_condition->at(0) = rs; - + // 计算目标点沿路径的速度，并存储在s_condition[1]中 ptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d; - + // 计算目标点的加速度，并存储在s_condition[2]中 const double delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa; ptr_s_condition->at(2) = diff --git a/modules/common/util/message_util.h b/modules/common/util/message_util.h index 99db79a18c4..8e0cd9102de 100644 --- a/modules/common/util/message_util.h +++ b/modules/common/util/message_util.h @@ -37,16 +37,21 @@ namespace apollo { namespace common { namespace util { - +/// @brief +/// @tparam T 接受一个指向类型为 T 的指针 msg +/// @tparam type +/// @param module_name +/// @param msg template ::value, int>::type = 0> static void FillHeader(const std::string& module_name, T* msg) { - static std::atomic sequence_num = {0}; - auto* header = msg->mutable_header(); - double timestamp = ::apollo::cyber::Clock::NowInSeconds(); + static std::atomic sequence_num = {0}; // sequence_num 是一个原子类型，表示它是一个可以在多线程环境下安全地进行读写操作的整数 + auto* header = msg->mutable_header(); // 返回一个可以修改的 header 指针 + double timestamp = ::apollo::cyber::Clock::NowInSeconds(); // 存储当前时间戳 s header->set_module_name(module_name); header->set_timestamp_sec(timestamp); + // sequence_num.fetch_add(1)：对 sequence_num 进行原子操作，递增 1 并返回递增前的值 header->set_sequence_num( static_cast(sequence_num.fetch_add(1))); } diff --git a/modules/common/util/util.h b/modules/common/util/util.h index ad194426a71..d7cd58fc62c 100644 --- a/modules/common/util/util.h +++ b/modules/common/util/util.h @@ -43,10 +43,18 @@ namespace apollo { namespace common { namespace util { +// 模板函数IsProtoEqual，用于 比较两个 Protobuf 消息对象是否相等 +// ProtoA 和 ProtoB 是两个 泛型 Protobuf 消息类型 template bool IsProtoEqual(const ProtoA& a, const ProtoB& b) { +// 通过 GetTypeName() 获取 Protobuf 消息的 类型名称（字符串表示） +// 将 Protobuf 消息对象转换为二进制字符串，这个二进制字符串 完全代表该对象的内容 +// 思路: 序列化+字符串 return a.GetTypeName() == b.GetTypeName() && a.SerializeAsString() == b.SerializeAsString(); + +// 以下是官方提供的 Protobuf 消息比较函数，但它的性能较低 +// MessageDifferencer::Equals() 会逐个字段进行递归比较，处理更复杂，性能较低 // Test shows that the above method is 5 times faster than the // API: google::protobuf::util::MessageDifferencer::Equals(a, b); } diff --git a/modules/common/vehicle_model/vehicle_model.cc b/modules/common/vehicle_model/vehicle_model.cc index 728a87965c6..01c9f2d93c1 100644 --- a/modules/common/vehicle_model/vehicle_model.cc +++ b/modules/common/vehicle_model/vehicle_model.cc @@ -21,7 +21,11 @@ namespace apollo { namespace common { - +/// @brief 基于后置中心运动学自行车模型预测车辆的未来状态 +/// @param vehicle_model_config 车辆模型的配置参数 +/// @param predicted_time_horizon 预测的时间范围，表示从当前时刻开始，预测的车辆状态持续的时间：规划周期 100ms +/// @param cur_vehicle_state 当前车辆的状态，包含位置、速度、加速度、航向角等信息 +/// @param predicted_vehicle_state 指向预测车辆状态的指针，函数会修改该指针指向的状态 void VehicleModel::RearCenteredKinematicBicycleModel( const VehicleModelConfig& vehicle_model_config, const double predicted_time_horizon, const VehicleState& cur_vehicle_state, @@ -29,24 +33,34 @@ void VehicleModel::RearCenteredKinematicBicycleModel( // Kinematic bicycle model centered at rear axis center by Euler forward // discretization // Assume constant control command and constant z axis position + // 模型假设控制命令是常量的，并且车辆沿z轴的运动是恒定的 + // 预测的时间范围必须是正值 CHECK_GT(predicted_time_horizon, 0.0); + // 配置中获取时间步长（dt），用于更新车辆状态 0.06 double dt = vehicle_model_config.rc_kinematic_bicycle_model().dt(); double cur_x = cur_vehicle_state.x(); double cur_y = cur_vehicle_state.y(); double cur_z = cur_vehicle_state.z(); + // 车辆的航向角（方向角） double cur_phi = cur_vehicle_state.heading(); + // 车辆的线速度 double cur_v = cur_vehicle_state.linear_velocity(); + // 车辆的线性加速度 double cur_a = cur_vehicle_state.linear_acceleration(); double next_x = cur_x; double next_y = cur_y; double next_phi = cur_phi; double next_v = cur_v; + // 确保在预测时间内不会超出 if (dt >= predicted_time_horizon) { dt = predicted_time_horizon; } + // 初始化倒计时（countdown_time）为预测时间范围 100ms double countdown_time = predicted_time_horizon; + // 标记是否完成预测 bool finish_flag = false; + // 用于在倒计时接近结束时，避免出现数值误差问题 static constexpr double kepsilon = 1e-8; while (countdown_time > kepsilon && !finish_flag) { countdown_time -= dt; @@ -54,6 +68,8 @@ void VehicleModel::RearCenteredKinematicBicycleModel( dt = countdown_time + dt; finish_flag = true; } + // 采用了运动学单轨模型的欧拉前向法来离散化车辆运动 + // 估算车辆在当前时间步长内的中间航向角（phi） double intermidiate_phi = cur_phi + 0.5 * dt * cur_v * cur_vehicle_state.kappa(); next_phi = @@ -64,6 +80,7 @@ void VehicleModel::RearCenteredKinematicBicycleModel( cur_y + dt * (cur_v + 0.5 * dt * cur_a) * std::sin(intermidiate_phi); next_v = cur_v + dt * cur_a; + //更新当前状态为下一时刻的状态，准备进行下一次的迭代 cur_x = next_x; cur_y = next_y; cur_phi = next_phi; @@ -79,24 +96,33 @@ void VehicleModel::RearCenteredKinematicBicycleModel( predicted_vehicle_state->set_linear_acceleration( cur_vehicle_state.linear_acceleration()); } - +/// @brief 根据当前车辆的状态（cur_vehicle_state）和预测的时间范围（predicted_time_horizon），使用指定的车辆模型来预测车辆在未来一段时间内的状态，并返回预测结果 +/// @param predicted_time_horizon 预测的时间范围，表示从当前时刻起，预测未来的这段时间内车辆的状态 规划周期 +/// @param cur_vehicle_state 当前车辆的状态，包含了如位置、速度、加速度、方向等信息 +/// @return VehicleState VehicleModel::Predict(const double predicted_time_horizon, const VehicleState& cur_vehicle_state) { +// 保存车辆模型的配置 VehicleModelConfig vehicle_model_config; - +// apollo/modules/common/configs/config_gflags.cc +// 配置文件是一个protobuf文件，GetProtoFromFile函数会将文件内容加载到vehicle_model_config对象中。如果加载失败，会输出错误信息并终止程序 ACHECK(cyber::common::GetProtoFromFile(FLAGS_vehicle_model_config_filename, &vehicle_model_config)) << "Failed to load vehicle model config file " << FLAGS_vehicle_model_config_filename; // Some models not supported for now + // 检查车辆模型的类型，确保当前使用的车辆模型类型不是不支持的模型类型 ACHECK(vehicle_model_config.model_type() != VehicleModelConfig::COM_CENTERED_DYNAMIC_BICYCLE_MODEL); ACHECK(vehicle_model_config.model_type() != VehicleModelConfig::MLP_MODEL); +// 保存预测得到的车辆状态 VehicleState predicted_vehicle_state; + // 一种基于后置中心运动学单轨模型的车辆运动模型： 利用自行车模型预测未来的位置 if (vehicle_model_config.model_type() == VehicleModelConfig::REAR_CENTERED_KINEMATIC_BICYCLE_MODEL) { + // 车辆状态预测 RearCenteredKinematicBicycleModel(vehicle_model_config, predicted_time_horizon, cur_vehicle_state, &predicted_vehicle_state); diff --git a/modules/common/vehicle_state/vehicle_state_provider.cc b/modules/common/vehicle_state/vehicle_state_provider.cc index fb456129ff8..ae8baac9524 100644 --- a/modules/common/vehicle_state/vehicle_state_provider.cc +++ b/modules/common/vehicle_state/vehicle_state_provider.cc @@ -31,13 +31,16 @@ namespace common { Status VehicleStateProvider::Update( const localization::LocalizationEstimate &localization, const canbus::Chassis &chassis) { + // 定位赋值 original_localization_ = localization; + // 构造期望的线性速度 if (!ConstructExceptLinearVelocity(localization)) { std::string msg = absl::StrCat( "Fail to update because ConstructExceptLinearVelocity error.", "localization:\n", localization.DebugString()); return Status(ErrorCode::LOCALIZATION_ERROR, msg); } + // 获取定位时间戳，如果没有定位数据则使用底盘时间戳 if (localization.has_measurement_time()) { vehicle_state_.set_timestamp(localization.measurement_time()); } else if (localization.header().has_timestamp_sec()) { @@ -47,25 +50,26 @@ Status VehicleStateProvider::Update( "time instead."; vehicle_state_.set_timestamp(chassis.header().timestamp_sec()); } - +// 获取车辆档位 if (chassis.has_gear_location()) { vehicle_state_.set_gear(chassis.gear_location()); } else { vehicle_state_.set_gear(canbus::Chassis::GEAR_NONE); } - +// 获取车辆速度 if (chassis.has_speed_mps()) { vehicle_state_.set_linear_velocity(chassis.speed_mps()); +// 车辆的档位是倒档（GEAR_REVERSE）且没有启用反向驾驶状态 if (!FLAGS_reverse_heading_vehicle_state && vehicle_state_.gear() == canbus::Chassis::GEAR_REVERSE) { vehicle_state_.set_linear_velocity(-vehicle_state_.linear_velocity()); } } - +// 获取车辆方向盘转角百分比 if (chassis.has_steering_percentage()) { vehicle_state_.set_steering_percentage(chassis.steering_percentage()); } - +// 计算车辆行驶曲率 static constexpr double kEpsilon = 0.1; if (std::abs(vehicle_state_.linear_velocity()) < kEpsilon) { vehicle_state_.set_kappa(0.0); @@ -73,7 +77,7 @@ Status VehicleStateProvider::Update( vehicle_state_.set_kappa(vehicle_state_.angular_velocity() / vehicle_state_.linear_velocity()); } - +// 获取驾驶模式 vehicle_state_.set_driving_mode(chassis.driving_mode()); return Status::OK(); @@ -81,6 +85,7 @@ Status VehicleStateProvider::Update( bool VehicleStateProvider::ConstructExceptLinearVelocity( const localization::LocalizationEstimate &localization) { +// 如果定位没有位姿，则返回错误 if (!localization.has_pose()) { AERROR << "Invalid localization input."; return false; @@ -91,7 +96,7 @@ bool VehicleStateProvider::ConstructExceptLinearVelocity( ADEBUG << "Skip localization update when it is in use_navigation_mode."; return true; } - +// 拷贝定位位姿 vehicle_state_.mutable_pose()->CopyFrom(localization.pose()); if (localization.pose().has_position()) { vehicle_state_.set_x(localization.pose().position().x()); @@ -100,29 +105,32 @@ bool VehicleStateProvider::ConstructExceptLinearVelocity( } const auto &orientation = localization.pose().orientation(); - +// 如果定位有航向角，则直接赋值 if (localization.pose().has_heading()) { vehicle_state_.set_heading(localization.pose().heading()); - } else { + } else {// 否则根据四元数推算 vehicle_state_.set_heading( math::QuaternionToHeading(orientation.qw(), orientation.qx(), orientation.qy(), orientation.qz())); } - if (FLAGS_enable_map_reference_unify) { + if (FLAGS_enable_map_reference_unify) { // true + // 如果车辆参考坐标系坐标(后轴中心)没有角速度输出，则返回错误 if (!localization.pose().has_angular_velocity_vrf()) { AERROR << "localization.pose().has_angular_velocity_vrf() must be true " "when FLAGS_enable_map_reference_unify is true."; return false; } + // 添加角速度 vehicle_state_.set_angular_velocity( localization.pose().angular_velocity_vrf().z()); - + // 没有线性加速度 if (!localization.pose().has_linear_acceleration_vrf()) { AERROR << "localization.pose().has_linear_acceleration_vrf() must be " "true when FLAGS_enable_map_reference_unify is true."; return false; } + // 添加线性加速度，注意车辆坐标系y轴指向车辆行进方向 vehicle_state_.set_linear_acceleration( localization.pose().linear_acceleration_vrf().y()); } else { @@ -140,12 +148,13 @@ bool VehicleStateProvider::ConstructExceptLinearVelocity( vehicle_state_.set_linear_acceleration( localization.pose().linear_acceleration().y()); } - + // 如果存在欧拉角，则设置翻滚，俯仰，偏航角度 if (localization.pose().has_euler_angles()) { vehicle_state_.set_roll(localization.pose().euler_angles().y()); vehicle_state_.set_pitch(localization.pose().euler_angles().x()); vehicle_state_.set_yaw(localization.pose().euler_angles().z()); } else { + // // 否则根据四元数计算翻滚，俯仰，偏航角度 math::EulerAnglesZXYd euler_angle(orientation.qw(), orientation.qx(), orientation.qy(), orientation.qz()); vehicle_state_.set_roll(euler_angle.roll()); @@ -212,29 +221,55 @@ const VehicleState &VehicleStateProvider::vehicle_state() const { return vehicle_state_; } +/// @brief 基于当前速度、角速度和姿态信息预测车辆 t 秒后的位置，用于时间对齐（AlignTimeStamp）或轨迹规划 +/// @param t 预测的时间间隔 +/// @return 车辆在 t 秒后的预计位置 (x, y)，使用 math::Vec2d 表示 math::Vec2d VehicleStateProvider::EstimateFuturePosition(const double t) const { + // 存储 t 秒后的位置增量 (Δx, Δy, Δz) Eigen::Vector3d vec_distance(0.0, 0.0, 0.0); +// 获取车辆线性速度 double v = vehicle_state_.linear_velocity(); + /* + r = v / angular_velocity + angular = angular_velocity * t + // 弧形运动 + */ // Predict distance travel vector + // 角速度几乎为零：车辆为直线运动 if (std::fabs(vehicle_state_.angular_velocity()) < 0.0001) { - vec_distance[0] = 0.0; - vec_distance[1] = v * t; + vec_distance[0] = 0.0; // x + vec_distance[1] = v * t; // y 前进方向 } else { + // 如果车辆不为直线运动，进行轨迹预测 + // 车辆有角速度时，按圆周轨迹预测 + // Δx = - (v / ω) * (1 - cos(ω * t)) + // Δy = (v / ω) * sin(ω * t) + // 角速度 ω 不变时，车辆沿半径 r = v / ω 的圆轨迹运动 vec_distance[0] = -v / vehicle_state_.angular_velocity() * (1.0 - std::cos(vehicle_state_.angular_velocity() * t)); vec_distance[1] = std::sin(vehicle_state_.angular_velocity() * t) * v / vehicle_state_.angular_velocity(); } + // 旋转修正（考虑姿态信息） // If we have rotation information, take it into consideration. + // 判断是否有姿态信息 + // 车辆具有旋转信息时，vec_distance 需要转换到全局坐标系 if (vehicle_state_.pose().has_orientation()) { const auto &orientation = vehicle_state_.pose().orientation(); + // 构造一个 Eigen::Quaternion 类型的四元数对象 quaternion + // 1.获取车辆的四元数 (quaternion)，表示当前姿态 Eigen::Quaternion quaternion(orientation.qw(), orientation.qx(), orientation.qy(), orientation.qz()); + // 获取车辆的当前位置（x, y, z 坐标）并构造一个 Eigen::Vector3d 类型的向量 pos_vec，表示车辆当前的三维坐标 Eigen::Vector3d pos_vec(vehicle_state_.x(), vehicle_state_.y(), vehicle_state_.z()); +// 使用四元数的旋转矩阵将位移向量 vec_distance 转换到全局坐标系中（旋转的影响），然后加上车辆当前的位置 pos_vec，得到车辆在未来时间 t 的三维位置 future_pos_3d + // 2.计算旋转矩阵（quaternion.toRotationMatrix()），将 vec_distance 旋转到全局坐标系 const Eigen::Vector3d future_pos_3d = quaternion.toRotationMatrix() * vec_distance + pos_vec; +// 车辆在未来位置的估计值 +// 3.加上当前车辆位置，得到 t 秒后在全局坐标系下的位置 return math::Vec2d(future_pos_3d[0], future_pos_3d[1]); } diff --git a/modules/common_msgs/chassis_msgs/chassis.proto b/modules/common_msgs/chassis_msgs/chassis.proto index 77e9fb74f24..eb0ed0e3a7c 100644 --- a/modules/common_msgs/chassis_msgs/chassis.proto +++ b/modules/common_msgs/chassis_msgs/chassis.proto @@ -12,103 +12,120 @@ import "modules/common_msgs/basic_msgs/vehicle_signal.proto"; // next id :31 message Chassis { + // 驾驶模式 DrivingMode enum DrivingMode { - COMPLETE_MANUAL = 0; // human drive - COMPLETE_AUTO_DRIVE = 1; - AUTO_STEER_ONLY = 2; // only steer - AUTO_SPEED_ONLY = 3; // include throttle and brake + COMPLETE_MANUAL = 0; // human drive // 人工驾驶 + COMPLETE_AUTO_DRIVE = 1; // 全自动驾驶 + AUTO_STEER_ONLY = 2; // only steer // 仅自动转向 + AUTO_SPEED_ONLY = 3; // include throttle and brake // 仅自动控制油门和刹车 // security mode when manual intervention happens, only response status - EMERGENCY_MODE = 4; + EMERGENCY_MODE = 4; // 手动接管时的安全模式 } + // 错误码 ErrorCode enum ErrorCode { - NO_ERROR = 0; + NO_ERROR = 0; // 无错误 - CMD_NOT_IN_PERIOD = 1; // control cmd not in period + CMD_NOT_IN_PERIOD = 1; // control cmd not in period // 控制命令不在规定周期内 // car chassis report error, like steer, brake, throttle, gear fault - CHASSIS_ERROR = 2; + CHASSIS_ERROR = 2; // 底盘系统错误 // classify the types of the car chassis errors - CHASSIS_ERROR_ON_STEER = 6; - CHASSIS_ERROR_ON_BRAKE = 7; - CHASSIS_ERROR_ON_THROTTLE = 8; - CHASSIS_ERROR_ON_GEAR = 9; + CHASSIS_ERROR_ON_STEER = 6; // 转向系统错误 + CHASSIS_ERROR_ON_BRAKE = 7; // 制动系统错误 + CHASSIS_ERROR_ON_THROTTLE = 8; // 油门系统错误 + CHASSIS_ERROR_ON_GEAR = 9; // 档位系统错误 - MANUAL_INTERVENTION = 3; // human manual intervention + MANUAL_INTERVENTION = 3; // human manual intervention // 人工干预 // receive car chassis can frame not in period - CHASSIS_CAN_NOT_IN_PERIOD = 4; + CHASSIS_CAN_NOT_IN_PERIOD = 4; // CAN 信号超时 - UNKNOWN_ERROR = 5; + UNKNOWN_ERROR = 5; // 未知错误 } - + + // 档位 GearPosition enum GearPosition { - GEAR_NEUTRAL = 0; - GEAR_DRIVE = 1; - GEAR_REVERSE = 2; - GEAR_PARKING = 3; - GEAR_LOW = 4; - GEAR_INVALID = 5; - GEAR_NONE = 6; + GEAR_NEUTRAL = 0; // 空挡 + GEAR_DRIVE = 1; // 前进挡 + GEAR_REVERSE = 2; // 倒挡 + GEAR_PARKING = 3; // 停车挡 + GEAR_LOW = 4; // 低速挡 + GEAR_INVALID = 5; // 无效挡 + GEAR_NONE = 6; // 未上挡 } +// 防撞传感器事件 BumperEvent enum BumperEvent { BUMPER_INVALID = 0; BUMPER_NORMAL = 1; - BUMPER_PRESSED = 2; + BUMPER_PRESSED = 2; // 防撞器被压缩，表示发生碰撞 } - optional bool engine_started = 3; +// 车辆状态字段说明（部分） + optional bool engine_started = 3; // 引擎是否启动 // Engine speed in RPM. + // 引擎转速，单位 RPM optional float engine_rpm = 4 [default = nan]; // Vehicle Speed in meters per second. + // 车辆当前速度，单位 m/s optional float speed_mps = 5 [default = nan]; // Vehicle odometer in meters. + // 总里程，单位 m optional float odometer_m = 6 [default = nan]; // Fuel range in meters. + // 燃油续航里程，单位 m optional int32 fuel_range_m = 7; // Real throttle location in [%], ranging from 0 to 100. + // 油门开度，百分比 optional float throttle_percentage = 8 [default = nan]; // Real brake location in [%], ranging from 0 to 100. + // 刹车开度，百分比 optional float brake_percentage = 9 [default = nan]; // Real steering location in [%], ranging from -100 to 100. // steering_angle / max_steering_angle // Clockwise: negative // CountClockwise: positive + // 方向盘转角（百分比表示） optional float steering_percentage = 11 [default = nan]; // Applied steering torque in [Nm]. + // 转向力矩，单位 Nm optional float steering_torque_nm = 12 [default = nan]; // Parking brake status. + // 是否拉了驻车 optional bool parking_brake = 13; // Light signals. + // 灯光与信号（部分字段已废弃） optional bool high_beam_signal = 14 [deprecated = true]; optional bool low_beam_signal = 15 [deprecated = true]; optional bool left_turn_signal = 16 [deprecated = true]; optional bool right_turn_signal = 17 [deprecated = true]; optional bool horn = 18 [deprecated = true]; - optional bool wiper = 19; + optional bool wiper = 19; // 雨刷状态 optional bool disengage_status = 20 [deprecated = true]; - optional DrivingMode driving_mode = 21 [default = COMPLETE_MANUAL]; - optional ErrorCode error_code = 22 [default = NO_ERROR]; - optional GearPosition gear_location = 23; + optional DrivingMode driving_mode = 21 [default = COMPLETE_MANUAL]; // 当前驾驶模式 + optional ErrorCode error_code = 22 [default = NO_ERROR]; // 当前错误码 + optional GearPosition gear_location = 23; // 当前挡位 // timestamp for steering module + // 转向模块的时间戳 optional double steering_timestamp = 24; // In seconds, with 1e-6 accuracy // chassis also needs it own sending timestamp + // 消息头，包括时间戳、帧 ID optional apollo.common.Header header = 25; optional int32 chassis_error_mask = 26 [default = 0]; @@ -136,15 +153,18 @@ message Chassis { optional int32 battery_soc_percentage = 34 [default = -1]; // Real send throttle location in [%], ranging from 0 to 100. + // 实际发送给车辆的油门开度 optional float throttle_percentage_cmd = 35 [default = nan]; // Real send brake location in [%], ranging from 0 to 100. + // 实际刹车命令 optional float brake_percentage_cmd = 36 [default = nan]; // Real send steering location in [%], ranging from -100 to 100. // steering_angle / max_steering_angle // Clockwise: negative // CountClockwise: positive + // 实际转向命令 optional float steering_percentage_cmd = 37 [default = nan]; optional BumperEvent front_bumper_event = 38; @@ -154,13 +174,15 @@ message Chassis { optional CheckResponse check_response = 40; // Custom chassis operation command defined by user for extensibility. + // 用户自定义扩展状态 optional google.protobuf.Any custom_status = 41; } +// ChassisGPS：GPS 信息 message ChassisGPS { - optional double latitude = 1; - optional double longitude = 2; - optional bool gps_valid = 3; + optional double latitude = 1; // 纬度 + optional double longitude = 2; // 经度 + optional bool gps_valid = 3; // GPS 是否有效 optional int32 year = 4; optional int32 month = 5; @@ -179,7 +201,7 @@ message ChassisGPS { optional double vdop = 17; optional GpsQuality quality = 18; optional int32 num_satellites = 19; - optional double gps_speed = 20; + optional double gps_speed = 20; // GPS 速度 } enum GpsQuality { @@ -189,6 +211,7 @@ enum GpsQuality { FIX_INVALID = 3; } +// WheelSpeed：四轮转速信息 message WheelSpeed { enum WheelSpeedType { FORWARD = 0; @@ -196,9 +219,9 @@ message WheelSpeed { STANDSTILL = 2; INVALID = 3; } - optional bool is_wheel_spd_rr_valid = 1 [default = false]; + optional bool is_wheel_spd_rr_valid = 1 [default = false]; // 后右轮速度是否有效 optional WheelSpeedType wheel_direction_rr = 2 [default = INVALID]; - optional double wheel_spd_rr = 3 [default = 0.0]; + optional double wheel_spd_rr = 3 [default = 0.0]; // 后右轮速度 optional bool is_wheel_spd_rl_valid = 4 [default = false]; optional WheelSpeedType wheel_direction_rl = 5 [default = INVALID]; optional double wheel_spd_rl = 6 [default = 0.0]; @@ -216,8 +239,9 @@ message Sonar { optional apollo.common.Quaternion rotation = 3; } +// Surround：环视与雷达传感器数据 message Surround { - optional bool cross_traffic_alert_left = 1; + optional bool cross_traffic_alert_left = 1; // 左侧横向交通警报 optional bool cross_traffic_alert_left_enabled = 2; optional bool blind_spot_left_alert = 3; optional bool blind_spot_left_alert_enabled = 4; @@ -225,7 +249,7 @@ message Surround { optional bool cross_traffic_alert_right_enabled = 6; optional bool blind_spot_right_alert = 7; optional bool blind_spot_right_alert_enabled = 8; - optional double sonar00 = 9; + optional double sonar00 = 9; // 超声波传感器数据（总共 12 个） optional double sonar01 = 10; optional double sonar02 = 11; optional double sonar03 = 12; @@ -240,7 +264,7 @@ message Surround { optional bool sonar_enabled = 21; optional bool sonar_fault = 22; repeated double sonar_range = 23; - repeated Sonar sonar = 24; + repeated Sonar sonar = 24;// 所有雷达对象数据 } message License { @@ -248,12 +272,13 @@ message License { } // CheckResponseSignal +// CheckResponse：ECU 状态诊断 message CheckResponse { - optional bool is_eps_online = 1 [default = false]; + optional bool is_eps_online = 1 [default = false];// 电动助力转向是否在线 optional bool is_epb_online = 2 [default = false]; optional bool is_esp_online = 3 [default = false]; optional bool is_vtog_online = 4 [default = false]; optional bool is_scu_online = 5 [default = false]; optional bool is_switch_online = 6 [default = false]; - optional bool is_vcu_online = 7 [default = false]; + optional bool is_vcu_online = 7 [default = false]; // 整车控制器是否在线 } diff --git a/modules/common_msgs/control_msgs/control_cmd.proto b/modules/common_msgs/control_msgs/control_cmd.proto index 4950f0e1664..950f79f955a 100644 --- a/modules/common_msgs/control_msgs/control_cmd.proto +++ b/modules/common_msgs/control_msgs/control_cmd.proto @@ -16,64 +16,69 @@ enum TurnSignal { TURN_RIGHT = 2; } +// 延迟统计信息（用于调试） message LatencyStats { - optional double total_time_ms = 1; - repeated double controller_time_ms = 2; - optional bool total_time_exceeded = 3; + optional double total_time_ms = 1; // 总耗时，单位为毫秒 + repeated double controller_time_ms = 2; // 控制器各部分耗时 + optional bool total_time_exceeded = 3; // 总时间是否超出阈值 } // next id : 27 +// 控制命令（核心控制输出） message ControlCommand { - optional apollo.common.Header header = 1; + optional apollo.common.Header header = 1; // 通用消息头 // target throttle in percentage [0, 100] - optional double throttle = 3; + optional double throttle = 3; // 油门百分比（0~100） // target brake in percentage [0, 100] - optional double brake = 4; + optional double brake = 4; // 刹车百分比（0~100） // target non-directional steering rate, in percentage of full scale per // second [0, 100] - optional double steering_rate = 6; + optional double steering_rate = 6; // 转向速率（百分比） // target steering angle, in percentage of full scale [-100, 100] - optional double steering_target = 7; + optional double steering_target = 7; // 目标转角（百分比，-100~100） // parking brake engage. true: engaged - optional bool parking_brake = 8; + optional bool parking_brake = 8; // 驻车制动（true 表示激活） // target speed, in m/s - optional double speed = 9; + optional double speed = 9; // 目标车速（m/s） // target acceleration in m`s^-2 - optional double acceleration = 10; + optional double acceleration = 10; // 目标加速度（m/s²） // model reset - optional bool reset_model = 16 [deprecated = true]; + optional bool reset_model = 16 [deprecated = true]; // 是否重置控制模型（已废弃） // engine on/off, true: engine on - optional bool engine_on_off = 17; + optional bool engine_on_off = 17; // 发动机启停 // completion percentage of trajectory planned in last cycle - optional double trajectory_fraction = 18; + optional double trajectory_fraction = 18; // 上一周期轨迹完成度（0~1） optional apollo.canbus.Chassis.DrivingMode driving_mode = 19 - [deprecated = true]; - optional apollo.canbus.Chassis.GearPosition gear_location = 20; + [deprecated = true]; // 驾驶模式（已废弃） + optional apollo.canbus.Chassis.GearPosition gear_location = 20; // 档位状态 - optional Debug debug = 22; - optional apollo.common.VehicleSignal signal = 23; - optional LatencyStats latency_stats = 24; - optional PadMessage pad_msg = 25; - optional apollo.common.EngageAdvice engage_advice = 26; - optional bool is_in_safe_mode = 27 [default = false]; + optional Debug debug = 22; // 控制调试信息 + optional apollo.common.VehicleSignal signal = 23; // 灯光/喇叭等信号 + optional LatencyStats latency_stats = 24; // 延迟统计信息 + optional PadMessage pad_msg = 25; // 来自人机交互的 Pad 消息 + optional apollo.common.EngageAdvice engage_advice = 26; // 系统是否建议 engage + optional bool is_in_safe_mode = 27 [default = false]; // 是否处于安全模式（例如应急状态） + // 以下字段已废弃 // deprecated fields optional bool left_turn = 13 [deprecated = true]; optional bool right_turn = 14 [deprecated = true]; optional bool high_beam = 11 [deprecated = true]; optional bool low_beam = 12 [deprecated = true]; optional bool horn = 15 [deprecated = true]; - optional TurnSignal turnsignal = 21 [deprecated = true]; + optional TurnSignal turnsignal = 21 [deprecated = true]; // 转向灯状态（已弃用，建议使用 VehicleSignal） } +// 简单纵向控制调试信息 message SimpleLongitudinalDebug { + // 包含站点误差、速度误差、加速度误差、控制输出、PID饱和状态等。 optional double station_reference = 1; optional double station_error = 2; optional double station_error_limited = 3; @@ -88,13 +93,13 @@ message SimpleLongitudinalDebug { optional double acceleration_cmd = 12; optional double acceleration_lookup = 13; optional double speed_lookup = 14; - optional double calibration_value = 15; - optional double throttle_cmd = 16; - optional double brake_cmd = 17; - optional bool is_full_stop = 18; + optional double calibration_value = 15; // 表示经过标定曲线处理后的值 + optional double throttle_cmd = 16; // 油门输出值 + optional double brake_cmd = 17; // 刹车输出值 + optional bool is_full_stop = 18; // 表示是否完全停车 optional double slope_offset_compensation = 19; - optional double current_station = 20; - optional double path_remain = 21; + optional double current_station = 20; // 表示当前位置 + optional double path_remain = 21; // 剩余路径长度 optional int32 pid_saturation_status = 22; optional int32 leadlag_saturation_status = 23; optional double speed_offset = 24; @@ -108,7 +113,7 @@ message SimpleLongitudinalDebug { optional apollo.common.TrajectoryPoint current_matched_point = 32; optional apollo.common.TrajectoryPoint current_reference_point = 33; optional apollo.common.TrajectoryPoint preview_reference_point = 34; - optional double acceleration_lookup_limit = 35; + optional double acceleration_lookup_limit = 35; // lookup 表示查表获得的控制值 optional double vehicle_pitch = 36; optional bool is_epb_brake = 37; optional double current_steer_interval = 38; @@ -118,6 +123,7 @@ message SimpleLongitudinalDebug { optional bool is_full_stop_soft = 42; } +// 简单横向控制调试信息 message SimpleLateralDebug { optional double lateral_error = 1; optional double ref_heading = 2; @@ -133,7 +139,7 @@ message SimpleLateralDebug { optional double steer_angle_heading_contribution = 12; optional double steer_angle_heading_rate_contribution = 13; optional double steer_angle_feedback = 14; - optional double steering_position = 15; + optional double steering_position = 15; // 当前方向盘位置 optional double ref_speed = 16; optional double steer_angle_limited = 17; @@ -163,19 +169,22 @@ message SimpleLateralDebug { optional double heading_error_feedback = 29; // current planning target point + // 当前规划点 optional apollo.common.TrajectoryPoint current_target_point = 30; // Augmented feedback control term in addition to LQR control - optional double steer_angle_feedback_augment = 31; + optional double steer_angle_feedback_augment = 31; // 增强反馈项 // Mrac control status and feedback states for steer control - optional MracDebug steer_mrac_debug = 32; + optional MracDebug steer_mrac_debug = 32; // MRAC 控制器调试信息 optional bool steer_mrac_enable_status = 33; // lat acc in ENU, in m/s^2 + // 车身离心加速度（ENU 坐标系） optional double lateral_centripetal_acceleration = 34; } +// MPC 控制调试信息（集成横纵向控制） message SimpleMPCDebug { optional double lateral_error = 1; optional double ref_heading = 2; @@ -258,24 +267,26 @@ message SimpleMPCDebug { optional double acceleration_vrf = 64; } +// MRAC 控制器调试信息（模型参考自适应控制） message MracDebug { - optional int32 mrac_model_order = 1; - repeated double mrac_reference_state = 2; - repeated double mrac_state_error = 3; - optional MracAdaptiveGain mrac_adaptive_gain = 4; - optional int32 mrac_reference_saturation_status = 5; - optional int32 mrac_control_saturation_status = 6; + optional int32 mrac_model_order = 1; // MRAC 模型阶数 + repeated double mrac_reference_state = 2; // 参考状态向量 + repeated double mrac_state_error = 3; // 状态误差 + optional MracAdaptiveGain mrac_adaptive_gain = 4; // 自适应增益 + optional int32 mrac_reference_saturation_status = 5; // 参考输入饱和状态 + optional int32 mrac_control_saturation_status = 6; // 控制输出饱和状态 } message MracAdaptiveGain { - repeated double state_adaptive_gain = 1; - repeated double input_adaptive_gain = 2; - repeated double nonlinear_adaptive_gain = 3; + repeated double state_adaptive_gain = 1; // 状态反馈增益 + repeated double input_adaptive_gain = 2; // 输入反馈增益 + repeated double nonlinear_adaptive_gain = 3; // 非线性反馈增益 } +// 调试结构体顶层封装 message Debug { - optional SimpleLongitudinalDebug simple_lon_debug = 1; - optional SimpleLateralDebug simple_lat_debug = 2; - optional InputDebug input_debug = 3; - optional SimpleMPCDebug simple_mpc_debug = 4; + optional SimpleLongitudinalDebug simple_lon_debug = 1; // 纵向调试信息 + optional SimpleLateralDebug simple_lat_debug = 2; // 横向调试信息 + optional InputDebug input_debug = 3; // 控制器输入调试 + optional SimpleMPCDebug simple_mpc_debug = 4; // MPC 调试信息 } diff --git a/modules/common_msgs/external_command_msgs/command_status.proto b/modules/common_msgs/external_command_msgs/command_status.proto index 475c9acb484..2a33d7c2e57 100644 --- a/modules/common_msgs/external_command_msgs/command_status.proto +++ b/modules/common_msgs/external_command_msgs/command_status.proto @@ -6,27 +6,29 @@ import "modules/common_msgs/basic_msgs/header.proto"; enum CommandStatusType { // Command is being executed without error. - RUNNING = 1; + RUNNING = 1; // // 命令正在执行中，无错误。 // Command is finished. - FINISHED = 2; + FINISHED = 2; // 命令执行完成。 // Command's execution has error. - ERROR = 3; + ERROR = 3; // 命令执行中发生了错误。 // Cannot get the status of command. - UNKNOWN = 4; + UNKNOWN = 4; // 无法获得命令状态。 } +// 用于查询某条命令的执行状态 message CommandStatusRequest { optional apollo.common.Header header = 1; // Unique identification for command. - optional int64 command_id = 2 [default = -1]; + optional int64 command_id = 2 [default = -1]; // 唯一的命令 ID，请求的目标 } +// 用于响应状态请求或主动上报命令状态 message CommandStatus { optional apollo.common.Header header = 1; // Unique identification for command. - optional int64 command_id = 2 [default = -1]; + optional int64 command_id = 2 [default = -1]; // 被查询的命令 ID // The status of command execution. - required CommandStatusType status = 3; + required CommandStatusType status = 3; // 执行状态 // The message for the status. - optional string message = 4; + optional string message = 4; // 可选的状态说明信息 } \ No newline at end of file diff --git a/modules/common_msgs/perception_msgs/perception_obstacle.proto b/modules/common_msgs/perception_msgs/perception_obstacle.proto index 0e400eda2a2..a26838676e3 100644 --- a/modules/common_msgs/perception_msgs/perception_obstacle.proto +++ b/modules/common_msgs/perception_msgs/perception_obstacle.proto @@ -62,15 +62,19 @@ message DebugMessage { } message PerceptionObstacle { + // 障碍物id optional int32 id = 1; // obstacle ID. // obstacle position in the world coordinate system. + // 三维坐标 optional apollo.common.Point3D position = 2; - + // 世界坐标系下的朝向 optional double theta = 3; // heading in the world coordinate system. + // 速度 optional apollo.common.Point3D velocity = 4; // obstacle velocity. // Size of obstacle bounding box. + // 长宽高 optional double length = 5; // obstacle length. optional double width = 6; // obstacle width. optional double height = 7; // obstacle height. @@ -79,7 +83,7 @@ message PerceptionObstacle { // duration of an obstacle since detection in s. optional double tracking_time = 9; - + // 类型 enum Type { UNKNOWN = 0; UNKNOWN_MOVABLE = 1; diff --git a/modules/common_msgs/routing_msgs/geometry.proto b/modules/common_msgs/routing_msgs/geometry.proto index 072ea6dcfb8..3b790a3fe29 100644 --- a/modules/common_msgs/routing_msgs/geometry.proto +++ b/modules/common_msgs/routing_msgs/geometry.proto @@ -4,6 +4,7 @@ package apollo.routing; import "modules/common_msgs/basic_msgs/geometry.proto"; +// 道路上的路径点，包含了id，长度和位置点信息 message LaneWaypoint { optional string id = 1; optional double s = 2; @@ -14,6 +15,7 @@ message LaneWaypoint { optional double heading = 4; } +// 道路的一段，包含了id和起止点信息 message LaneSegment { optional string id = 1; optional double start_s = 2; @@ -22,26 +24,30 @@ message LaneSegment { enum DeadEndRoutingType { ROUTING_OTHER = 0; - ROUTING_IN = 1; - ROUTING_OUT = 2; + ROUTING_IN = 1; // 当前路径是进入死胡同的路径（车辆要驶入一个无出口区域） + ROUTING_OUT = 2; // 当前路径是驶出死胡同的路径（车辆从尽头区域驶回正常车道） } +// 描述测量的距离 message Measurement { optional double distance = 1; } +// 道路的类型，有FORWARD，LEFT，RIGHT三种取值 enum ChangeLaneType { FORWARD = 0; LEFT = 1; RIGHT = 2; }; +// 一段通路，其中可以包含多个LaneSegment，以及ChangeLaneType message Passage { repeated LaneSegment segment = 1; optional bool can_exit = 2; optional ChangeLaneType change_lane_type = 3 [default = FORWARD]; } +// 道路的一段，拥有一个id，并可以包含多个Passage message RoadSegment { optional string id = 1; repeated Passage passage = 2; diff --git a/modules/common_msgs/routing_msgs/poi.proto b/modules/common_msgs/routing_msgs/poi.proto index 0a9f6c61614..0ee4aaf751a 100644 --- a/modules/common_msgs/routing_msgs/poi.proto +++ b/modules/common_msgs/routing_msgs/poi.proto @@ -19,6 +19,7 @@ message ParkingInfo { optional apollo.hdmap.Polygon corner_point = 4; } +// 地图上的一个点，包含了名称和位置信息 message Landmark { optional string name = 1; repeated LaneWaypoint waypoint = 2; @@ -27,6 +28,7 @@ message Landmark { optional int32 cycle_number = 5; } +// Point of interest的缩写，一个POI中可以包含多个Landmark message POI { repeated Landmark landmark = 1; } diff --git a/modules/common_msgs/routing_msgs/routing.proto b/modules/common_msgs/routing_msgs/routing.proto index 65ebe82276d..f1b069308bd 100644 --- a/modules/common_msgs/routing_msgs/routing.proto +++ b/modules/common_msgs/routing_msgs/routing.proto @@ -7,6 +7,7 @@ import "modules/common_msgs/basic_msgs/header.proto"; import "modules/common_msgs/basic_msgs/error_code.proto"; import "modules/common_msgs/routing_msgs/poi.proto"; +// 描述了路由请求的信息，一次路由请求可以包含多个路径点 message RoutingRequest { optional apollo.common.Header header = 1; // at least two points. The first is start point, the end is final point. @@ -20,6 +21,7 @@ message RoutingRequest { optional bool is_start_pose_set = 7 [default = false]; } +// 路由请求的响应结果，可以包含多个RoadSegment，距离等信息 message RoutingResponse { optional apollo.common.Header header = 1; repeated apollo.routing.RoadSegment road = 2; diff --git a/modules/control/control_component/control_component.cc b/modules/control/control_component/control_component.cc index a5b78e84b92..c6d99e23593 100644 --- a/modules/control/control_component/control_component.cc +++ b/modules/control/control_component/control_component.cc @@ -40,12 +40,17 @@ const double kDoubleEpsilon = 1e-6; ControlComponent::ControlComponent() : monitor_logger_buffer_(common::monitor::MonitorMessageItem::CONTROL) {} +/// @brief 初始化控制模块：依赖注入、读取配置、初始化控制器、订阅各类传感器和规划话题、设置发布器、车辆状态初始化等 +/// @return bool ControlComponent::Init() { + // 完成配置文件参数读取，并完成控制器初始化 injector_ = std::make_shared(); init_time_ = Clock::Now(); AINFO << "Control init, starting ..."; - + + // 读取 .pb.txt 格式的 ControlPipelineConfig + // 该配置定义了控制模块中各个 task（如 longitudinal、lateral）的执行顺序与参数 ACHECK( cyber::common::GetProtoFromFile(FLAGS_pipeline_file, &control_pipeline_)) << "Unable to load control pipeline file: " + FLAGS_pipeline_file; @@ -66,9 +71,10 @@ bool ControlComponent::Init() { } } + // 创建Control输入输出node,从话题通道中拿数据 cyber::ReaderConfig chassis_reader_config; chassis_reader_config.channel_name = FLAGS_chassis_topic; - chassis_reader_config.pending_queue_size = FLAGS_chassis_pending_queue_size; + chassis_reader_config.pending_queue_size = FLAGS_chassis_pending_queue_size; // 10 控制缓存大小，防止高频数据堆积延迟处理 chassis_reader_ = node_->CreateReader(chassis_reader_config, nullptr); @@ -76,7 +82,7 @@ bool ControlComponent::Init() { cyber::ReaderConfig planning_reader_config; planning_reader_config.channel_name = FLAGS_planning_trajectory_topic; - planning_reader_config.pending_queue_size = FLAGS_planning_pending_queue_size; + planning_reader_config.pending_queue_size = FLAGS_planning_pending_queue_size; // 10 trajectory_reader_ = node_->CreateReader(planning_reader_config, nullptr); @@ -86,7 +92,7 @@ bool ControlComponent::Init() { planning_command_status_reader_config.channel_name = FLAGS_planning_command_status; planning_command_status_reader_config.pending_queue_size = - FLAGS_planning_status_msg_pending_queue_size; + FLAGS_planning_status_msg_pending_queue_size; // 10 planning_command_status_reader_ = node_->CreateReader( planning_command_status_reader_config, nullptr); @@ -95,7 +101,7 @@ bool ControlComponent::Init() { cyber::ReaderConfig localization_reader_config; localization_reader_config.channel_name = FLAGS_localization_topic; localization_reader_config.pending_queue_size = - FLAGS_localization_pending_queue_size; + FLAGS_localization_pending_queue_size; // 10 localization_reader_ = node_->CreateReader( localization_reader_config, nullptr); @@ -103,31 +109,35 @@ bool ControlComponent::Init() { cyber::ReaderConfig pad_msg_reader_config; pad_msg_reader_config.channel_name = FLAGS_pad_topic; - pad_msg_reader_config.pending_queue_size = FLAGS_pad_msg_pending_queue_size; + pad_msg_reader_config.pending_queue_size = FLAGS_pad_msg_pending_queue_size; // 10 pad_msg_reader_ = node_->CreateReader(pad_msg_reader_config, nullptr); ACHECK(pad_msg_reader_ != nullptr); if (!FLAGS_use_control_submodules) { + // 如果不使用子模块，控制模块直接发布 ControlCommand 给底盘 control_cmd_writer_ = - node_->CreateWriter(FLAGS_control_command_topic); + node_->CreateWriter(FLAGS_control_command_topic); // /apollo/control ACHECK(control_cmd_writer_ != nullptr); } else { + // 如果使用子模块控制逻辑，将拼装完整的 LocalView（含定位、底盘、规划等信息）给子模块处理 local_view_writer_ = - node_->CreateWriter(FLAGS_control_local_view_topic); + node_->CreateWriter(FLAGS_control_local_view_topic); // /apollo/control/localview ACHECK(local_view_writer_ != nullptr); } // set initial vehicle state by cmd // need to sleep, because advertised channel is not ready immediately // simple test shows a short delay of 80 ms or so + // 车辆状态初始化延迟：为了确保所有话题都完成了注册与广播，等待 1 秒避免早期状态获取失败 AINFO << "Control resetting vehicle state, sleeping for 1000 ms ..."; std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // should init_vehicle first, let car enter work status, then use status msg // trigger control +// 设置默认驾驶模式（如START、STOP等），通过 pad_msg_ 来控制行为 AINFO << "Control default driving action is " << DrivingAction_Name((enum DrivingAction)FLAGS_action); pad_msg_.set_action((enum DrivingAction)FLAGS_action); @@ -181,21 +191,29 @@ void ControlComponent::OnMonitor( } } +/// @brief 负责生成一个周期的控制指令 control_command +/// @param control_command +/// @return Status ControlComponent::ProduceControlCommand( ControlCommand *control_command) { + // CheckInput()：判断各类消息是否齐全、是否异常（如缺失、时间不合法等），若失败直接设置 estop_ Status status = CheckInput(&local_view_); // check data if (!status.ok()) { + // 每 100 次输出一次错误日志，提示控制输入失败，并打印具体错误原因 AERROR_EVERY(100) << "Control input data failed: " << status.error_message(); + // 设置车辆不能进入自动驾驶状态 control_command->mutable_engage_advice()->set_advice( apollo::common::EngageAdvice::DISALLOW_ENGAGE); + // 给出不能 engage 的原因，用于上层系统或 UI 显示 control_command->mutable_engage_advice()->set_reason( status.error_message()); estop_ = true; estop_reason_ = status.error_message(); } else { estop_ = false; + // CheckTimestamp()：判断输入消息时间戳是否超时（如超过容忍周期） Status status_ts = CheckTimestamp(local_view_); if (!status_ts.ok()) { AERROR << "Input messages timeout"; @@ -205,14 +223,17 @@ Status ControlComponent::ProduceControlCommand( // latest_trajectory_.Clear(); estop_ = true; status = status_ts; + // 如果当前驾驶模式不是自动驾驶 if (local_view_.chassis().driving_mode() != apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) { + // 禁止自动驾驶，提示超时原因 control_command->mutable_engage_advice()->set_advice( apollo::common::EngageAdvice::DISALLOW_ENGAGE); control_command->mutable_engage_advice()->set_reason( status.error_message()); } } else { + // 如果输入检查和时间戳检查都通过，则设置为“可以 engage”，取消 estop control_command->mutable_engage_advice()->set_advice( apollo::common::EngageAdvice::READY_TO_ENGAGE); estop_ = false; @@ -220,10 +241,17 @@ Status ControlComponent::ProduceControlCommand( } // check estop + // 是否允许 estop 状态持续跨周期存在 + // 若配置了“持续 estop 模式”，一旦 estop 触发，需显式清除，否则将持续保持；否则根据当前 trajectory 中的 estop 状态判断 estop_ = FLAGS_enable_persistent_estop ? estop_ || local_view_.trajectory().estop().is_estop() : local_view_.trajectory().estop().is_estop(); - + /* + 多种情况会触发 estop_： + planning estop 标志 + trajectory 为空 + 齿轮前进但轨迹速度为负（Gear Drive + Negative Velocity） + */ if (local_view_.trajectory().estop().is_estop()) { estop_ = true; estop_reason_ = "estop from planning : "; @@ -248,6 +276,7 @@ Status ControlComponent::ProduceControlCommand( } if (!estop_) { + // 如果是手动驾驶模式，重置控制器 if (local_view_.chassis().driving_mode() == Chassis::COMPLETE_MANUAL) { control_task_agent_.Reset(); AINFO_EVERY(100) << "Reset Controllers in Manual Mode"; @@ -260,6 +289,7 @@ Status ControlComponent::ProduceControlCommand( debug->mutable_trajectory_header()->CopyFrom( local_view_.trajectory().header()); +// 若当前轨迹是重新规划生成的（如变道、避障触发），记录其 header 信息 if (local_view_.trajectory().is_replan()) { latest_replan_trajectory_header_ = local_view_.trajectory().header(); } @@ -272,6 +302,7 @@ Status ControlComponent::ProduceControlCommand( if (!local_view_.trajectory().trajectory_point().empty()) { // controller agent + // 使用轨迹、定位、底盘数据调用控制器计算输出命令 Status status_compute = control_task_agent_.ComputeControlCommand( &local_view_.localization(), &local_view_.chassis(), &local_view_.trajectory(), control_command); @@ -293,6 +324,7 @@ Status ControlComponent::ProduceControlCommand( } // if planning set estop, then no control process triggered + // 设置刹车、零速、保持当前转角，避免车辆有任何动作 if (estop_) { AWARN_EVERY(100) << "Estop triggered! No control core method executed!"; // set Estop command @@ -304,6 +336,8 @@ Status ControlComponent::ProduceControlCommand( injector_->previous_control_command_mutable()->steering_target(); control_command->set_steering_target(previous_steering_command_); } + + // 设置转向灯等信号信息：如果轨迹中包含转向灯、喇叭等信号，写入控制命令中 // check signal if (local_view_.trajectory().decision().has_vehicle_signal()) { control_command->mutable_signal()->CopyFrom( @@ -312,7 +346,10 @@ Status ControlComponent::ProduceControlCommand( return status; } +/// @brief 采集消息 ➜ 处理消息 ➜ 执行控制逻辑 ➜ 发布控制指令 +/// @return bool ControlComponent::Proc() { + // 获取当前时间，准备统计周期延迟 const auto start_time = Clock::Now(); chassis_reader_->Observe(); @@ -361,6 +398,7 @@ bool ControlComponent::Proc() { OnPad(pad_msg); } +// 此处加锁，保证 local_view_ 在其他线程使用时的一致性 { // TODO(SHU): to avoid redundent copy std::lock_guard lock(mutex_); @@ -372,6 +410,7 @@ bool ControlComponent::Proc() { } } +// 若启用子模块，直接发布 LocalView，由子模块处理控制逻辑 // use control submodules if (FLAGS_use_control_submodules) { local_view_.mutable_header()->set_lidar_timestamp( @@ -395,6 +434,7 @@ bool ControlComponent::Proc() { return true; } +// 若收到 RESET 指令，清除当前急停状态（estop） if (pad_msg != nullptr) { ADEBUG << "pad_msg: " << pad_msg_.ShortDebugString(); if (pad_msg_.action() == DrivingAction::RESET) { @@ -405,6 +445,7 @@ bool ControlComponent::Proc() { pad_received_ = true; } +// 控制测试模式检查:在持续一段时间后自动退出 if (FLAGS_is_control_test_mode && FLAGS_control_test_duration > 0 && (start_time - init_time_).ToSecond() > FLAGS_control_test_duration) { AERROR << "Control finished testing. exit"; @@ -417,6 +458,7 @@ bool ControlComponent::Proc() { ControlCommand control_command; Status status; + // 若车辆未进入自动驾驶模式，不生成正常控制量，发布零控制指令（防止误操作） if (local_view_.chassis().driving_mode() == apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) { status = ProduceControlCommand(&control_command); @@ -455,6 +497,7 @@ bool ControlComponent::Proc() { return true; } +// 若 pitch 角为 0，调用额外函数更新 pitch（用定位+底盘融合） if (fabs(control_command.debug().simple_lon_debug().vehicle_pitch()) < kDoubleEpsilon) { injector_->vehicle_state()->Update(local_view_.localization(), diff --git a/modules/control/control_component/control_component.h b/modules/control/control_component/control_component.h index 3a06f4cd6aa..ed3be791f00 100644 --- a/modules/control/control_component/control_component.h +++ b/modules/control/control_component/control_component.h @@ -53,63 +53,77 @@ class ControlComponent final : public apollo::cyber::TimerComponent { friend class ControlTestBase; public: + // 构造函数 ControlComponent(); - bool Init() override; - + // 初始化函数 + bool Init() override; // 初始化订阅+写入 + + // 每周期执行函数 bool Proc() override; private: // Upon receiving pad message + // 处理驾驶员操作指令（如启停） void OnPad(const std::shared_ptr &pad); + // 处理底盘状态信息 void OnChassis(const std::shared_ptr &chassis); + // 接收规划模块输出的轨迹 void OnPlanning( const std::shared_ptr &trajectory); - + + // 接收任务状态 void OnPlanningCommandStatus( const std::shared_ptr &planning_command_status); - + + // 接收定位信息 void OnLocalization( const std::shared_ptr &localization); + // 用于日志/系统监控 // Upon receiving monitor message void OnMonitor( const apollo::common::monitor::MonitorMessage &monitor_message); - + + // 计算控制命令（转向、加速、刹车） common::Status ProduceControlCommand(ControlCommand *control_command); + // 检查输入是否完整或有效 common::Status CheckInput(LocalView *local_view); + // 时间戳对齐检查 common::Status CheckTimestamp(const LocalView &local_view); common::Status CheckPad(); + // // 特殊情况输出零控制命令（如急停） void ResetAndProduceZeroControlCommand(ControlCommand *control_command); + // 获取车辆俯仰角辅助控制 void GetVehiclePitchAngle(ControlCommand *control_command); private: - apollo::cyber::Time init_time_; + apollo::cyber::Time init_time_; // 控制器初始化时间 - localization::LocalizationEstimate latest_localization_; - canbus::Chassis latest_chassis_; - planning::ADCTrajectory latest_trajectory_; - external_command::CommandStatus planning_command_status_; - PadMessage pad_msg_; - common::Header latest_replan_trajectory_header_; + localization::LocalizationEstimate latest_localization_; // 最新定位信息 + canbus::Chassis latest_chassis_; // 最新底盘状态 + planning::ADCTrajectory latest_trajectory_; // 最新规划轨迹 + external_command::CommandStatus planning_command_status_; // 任务状态 + PadMessage pad_msg_; // 驾驶员操作信息 + common::Header latest_replan_trajectory_header_; // 最近的轨迹重规划头信息 - ControlTaskAgent control_task_agent_; + ControlTaskAgent control_task_agent_; // 控制任务分发与执行 - bool estop_ = false; - std::string estop_reason_; - bool pad_received_ = false; + bool estop_ = false; // 紧急停车标志 + std::string estop_reason_; // 紧急停车原因 + bool pad_received_ = false; // 是否收到 Pad 信息 unsigned int status_lost_ = 0; unsigned int status_sanity_check_failed_ = 0; unsigned int total_status_lost_ = 0; unsigned int total_status_sanity_check_failed_ = 0; - ControlPipeline control_pipeline_; + ControlPipeline control_pipeline_; // 控制流水线（如预处理、主控制器） - std::mutex mutex_; + std::mutex mutex_; // 互斥锁保护共享资源 std::shared_ptr> chassis_reader_; std::shared_ptr> pad_msg_reader_; @@ -120,17 +134,17 @@ class ControlComponent final : public apollo::cyber::TimerComponent { std::shared_ptr> planning_command_status_reader_; - std::shared_ptr> control_cmd_writer_; + std::shared_ptr> control_cmd_writer_; // 控制命令发布器 // when using control submodules - std::shared_ptr> local_view_writer_; + std::shared_ptr> local_view_writer_; // 用于子模块调试或结构解耦 - common::monitor::MonitorLogBuffer monitor_logger_buffer_; + common::monitor::MonitorLogBuffer monitor_logger_buffer_; // 监控日志输出缓冲区 - LocalView local_view_; + LocalView local_view_; // 本周期使用的数据快照 - std::shared_ptr injector_; + std::shared_ptr injector_; // 控制器依赖注入器 - double previous_steering_command_ = 0.0; + double previous_steering_command_ = 0.0; // 上一次控制器的转向输出（用于滤波/约束） }; CYBER_REGISTER_COMPONENT(ControlComponent) diff --git a/modules/control/control_component/controller_task_base/common/trajectory_analyzer.cc b/modules/control/control_component/controller_task_base/common/trajectory_analyzer.cc index 5acc315f95d..efa37683ab7 100644 --- a/modules/control/control_component/controller_task_base/common/trajectory_analyzer.cc +++ b/modules/control/control_component/controller_task_base/common/trajectory_analyzer.cc @@ -147,8 +147,13 @@ void TrajectoryAnalyzer::ToTrajectoryFrame(const double x, const double y, *ptr_s_dot = v * cos_delta_theta / one_minus_kappa_r_d; } +/// @brief 根据相对路径时间戳找到参考轨迹对应点 +/// @param t ：绝对时间戳 +/// @return TrajectoryPoint TrajectoryAnalyzer::QueryNearestPointByAbsoluteTime( const double t) const { +// header_time_为参考轨迹的全局时间戳 +// 相对路径时间戳 = 绝对时间戳 - 参考轨迹全局时间戳 return QueryNearestPointByRelativeTime(t - header_time_); } diff --git a/modules/control/control_component/controller_task_base/control_task_agent.cc b/modules/control/control_component/controller_task_base/control_task_agent.cc index ee125e03be9..53121114bd9 100644 --- a/modules/control/control_component/controller_task_base/control_task_agent.cc +++ b/modules/control/control_component/controller_task_base/control_task_agent.cc @@ -57,10 +57,14 @@ Status ControlTaskAgent::ComputeControlCommand( const localization::LocalizationEstimate *localization, const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory, control::ControlCommand *cmd) { + // 遍历 controller_list_，每一个控制器（如 LQR、MPC 等）都将参与控制命令计算 for (auto &controller : controller_list_) { + // 打印当前正在处理的控制器的名称，帮助调试和性能分析 ADEBUG << "controller:" << controller->Name() << " processing ..."; + // 获取当前系统时间（秒），用于统计该控制器的计算耗时 double start_timestamp = Clock::NowInSeconds(); controller->ComputeControlCommand(localization, chassis, trajectory, cmd); + // 获取控制器计算后的时间，用于计算耗时 double end_timestamp = Clock::NowInSeconds(); const double time_diff_ms = (end_timestamp - start_timestamp) * 1000; diff --git a/modules/control/controllers/lon_based_pid_controller/lon_controller.cc b/modules/control/controllers/lon_based_pid_controller/lon_controller.cc index f64f797a2dd..5bc86dd309f 100644 --- a/modules/control/controllers/lon_based_pid_controller/lon_controller.cc +++ b/modules/control/controllers/lon_based_pid_controller/lon_controller.cc @@ -100,13 +100,14 @@ void LonController::Stop() { CloseLogFile(); } LonController::~LonController() { CloseLogFile(); } Status LonController::Init(std::shared_ptr injector) { + // /apollo/modules/control/lon_based_pid_controller/conf/controller_conf.pb.txt if (!ControlTask::LoadConfig( &lon_based_pidcontroller_conf_)) { AERROR << "failed to load control conf"; return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load lon control_conf"); } - + // /apollo/modules/control/control_component/conf/calibration_table.pb.txt if (!ControlTask::LoadCalibrationTable(&calibration_table_)) { AERROR << "failed to load calibration table"; return Status(ErrorCode::CONTROL_INIT_ERROR, @@ -121,7 +122,7 @@ Status LonController::Init(std::shared_ptr injector) { double ts = lon_based_pidcontroller_conf_.ts(); bool enable_leadlag = lon_based_pidcontroller_conf_.enable_reverse_leadlag_compensation(); - + // 位置、速度控制器初始化 station_pid_controller_.Init( lon_based_pidcontroller_conf_.station_pid_conf()); speed_pid_controller_.Init( @@ -136,7 +137,7 @@ Status LonController::Init(std::shared_ptr injector) { vehicle_param_.CopyFrom( common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param()); - + // 俯仰角低通滤波 SetDigitalFilterPitchAngle(); InitControlCalibrationTable(); @@ -166,6 +167,20 @@ void LonController::InitControlCalibrationTable() { << "Fail to load control calibration table"; } +/* +1. 基础校验 & 状态更新 +2. 控制器配置选择（不同工况选择不同 PID 参数） +3. 两级 PID 控制： + - station（位置）误差控制 → 生成 speed_offset + - speed（速度）误差控制 → 生成加速度命令 +4. 可选：Lead-Lag 前馈控制器补偿 +5. 坡度补偿（使用 pitch） +6. 最终加速度融合（闭环 + 规划期望 + 坡度） +7. full_stop 检查与处理（目的地 or 行人长期停车） +8. 查表生成 throttle / brake 控制量 +9. EPB（电子驻车）控制判断 +10. 写入调试信息，输出指令 +*/ Status LonController::ComputeControlCommand( const localization::LocalizationEstimate *localization, const canbus::Chassis *chassis, @@ -204,21 +219,27 @@ Status LonController::ComputeControlCommand( return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg); } ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts, - debug); + debug); // // 计算 station / speed / acceleration 误差 double station_error_limit = lon_based_pidcontroller_conf_.station_error_limit(); - double station_error_limited = 0.0; + double station_error_limited = 0.0; // 限幅后的纵向位置误差 + + // 考虑预瞄时纵向位置偏差的上下限不同 if (lon_based_pidcontroller_conf_.enable_speed_station_preview()) { + // preview_station_error = 预瞄点纵向位置 - 匹配点纵向位置 station_error_limited = common::math::Clamp(debug->preview_station_error(), -station_error_limit, station_error_limit); } else { + // station_error = 参考点纵向位置 - 匹配点纵向位置 station_error_limited = common::math::Clamp( debug->station_error(), -station_error_limit, station_error_limit); } +// a. 判断是否为倒车 if (trajectory_message_->gear() == canbus::Chassis::GEAR_REVERSE) { + // Pit 区检测（特殊工况，停车场？） if (CheckPit::CheckInPit(debug, &lon_based_pidcontroller_conf_, injector_->vehicle_state()->linear_velocity(), trajectory_message_->is_replan())) { @@ -233,23 +254,29 @@ Status LonController::ComputeControlCommand( speed_pid_controller_.SetPID( lon_based_pidcontroller_conf_.reverse_speed_pid_conf()); } + // d. Lead-Lag 控制是否启用 if (enable_leadlag) { station_leadlag_controller_.SetLeadlag( lon_based_pidcontroller_conf_.reverse_station_leadlag_conf()); speed_leadlag_controller_.SetLeadlag( lon_based_pidcontroller_conf_.reverse_speed_leadlag_conf()); } - } else if (injector_->vehicle_state()->linear_velocity() <= + } + // b. 判断是否低速（小于某个速度阈值） + else if (injector_->vehicle_state()->linear_velocity() <= lon_based_pidcontroller_conf_.switch_speed()) { if (CheckPit::CheckInPit(debug, &lon_based_pidcontroller_conf_, injector_->vehicle_state()->linear_velocity(), trajectory_message_->is_replan())) { ADEBUG << "in pit"; + + // 速度PID控制器对象speed_pid_controller_加载控制配置文件中低速PID参数 station_pid_controller_.SetPID( lon_based_pidcontroller_conf_.pit_station_pid_conf()); speed_pid_controller_.SetPID( lon_based_pidcontroller_conf_.pit_speed_pid_conf()); } else { + // 速度PID控制器对象加载控制配置文件中高速PID参数，通常低速PID参数要更大些 station_pid_controller_.SetPID( lon_based_pidcontroller_conf_.station_pid_conf()); speed_pid_controller_.SetPID( @@ -262,8 +289,13 @@ Status LonController::ComputeControlCommand( lon_based_pidcontroller_conf_.high_speed_pid_conf()); } + // 一阶段 PID: Station → speed offset + // 位置环输出到速度环输入补偿，也就是速度环为内环 + // delta_s = vt 我们在当前纵向偏差下的速度补偿 + // 速度偏差 = 位置PID控制器根据(限幅后位置误差，采样周期)计算出控制量即速度 double speed_offset = station_pid_controller_.Control(station_error_limited, ts); + // 可选 Lead-Lag 前馈补偿 if (enable_leadlag) { speed_offset = station_leadlag_controller_.Control(speed_offset, ts); } @@ -273,8 +305,10 @@ Status LonController::ComputeControlCommand( lon_based_pidcontroller_conf_.speed_controller_input_limit(); double speed_controller_input_limited = 0.0; if (lon_based_pidcontroller_conf_.enable_speed_station_preview()) { + // 速度控制器的输入 = 位置控制器计算出的speed_offset + 当前时间向前加上预瞄时间在轨迹上的对应点速度和当前车速的偏差 speed_controller_input = speed_offset + debug->preview_speed_error(); } else { + // 速度控制器的输入 = 位置控制器计算出的speed_offset + 参考点车速和当前车速的偏差 speed_controller_input = speed_offset + debug->speed_error(); } speed_controller_input_limited = @@ -282,7 +316,8 @@ Status LonController::ComputeControlCommand( speed_controller_input_limit); double acceleration_cmd_closeloop = 0.0; - + // 二阶段 PID: Speed → acceleration + // 求解速度环输出，给出加速度结果 v=at acceleration_cmd_closeloop = speed_pid_controller_.Control(speed_controller_input_limited, ts); debug->set_pid_saturation_status( @@ -299,6 +334,7 @@ Status LonController::ComputeControlCommand( station_pid_controller_.Reset_integral(); } +// 坡度补偿：滤波 double vehicle_pitch_rad = digital_filter_pitch_angle_.Filter(injector_->vehicle_state()->pitch()); double vehicle_pitch = @@ -308,17 +344,20 @@ Status LonController::ComputeControlCommand( // TODO(ALL): confirm the slope_offset_compensation whether is positive or not // when vehicle move uphill // Resume: uphill: + , downhill: - + // 定义斜坡补偿加速度 = (重力加速度*车辆俯仰角的正弦值) double slope_offset_compensation = lon_based_pidcontroller_conf_.use_opposite_slope_compensation() * GRA_ACC * std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180); +// 判断坡道补偿加速度是否为非数NaN,当浮点数过小下溢就可能出现NaN非数 if (std::isnan(slope_offset_compensation)) { slope_offset_compensation = 0; } debug->set_slope_offset_compensation(slope_offset_compensation); +// 总的加速度指令 = 闭环加速度指令 + 预瞄点参考加速度 + 坡道补偿加速度 double acceleration_cmd = acceleration_cmd_closeloop + debug->preview_acceleration_reference() + lon_based_pidcontroller_conf_.enable_slope_offset() * @@ -610,6 +649,7 @@ void LonController::ComputeLongitudinalErrors( double d_matched = 0.0; double d_dot_matched = 0.0; +//匹配最近点 auto vehicle_state = injector_->vehicle_state(); auto matched_point = trajectory_analyzer->QueryMatchedPathPoint( vehicle_state->x(), vehicle_state->y()); @@ -620,12 +660,18 @@ void LonController::ComputeLongitudinalErrors( &s_dot_matched, &d_matched, &d_dot_matched); // double current_control_time = Time::Now().ToSecond(); + // 当前时间确定参考点，预瞄时间确定预瞄点 + // 预瞄时间 = 预瞄窗口数目 * 预瞄窗口时间(20 * 0.01 = 0.2s) double current_control_time = ::apollo::cyber::Clock::NowInSeconds(); double preview_control_time = current_control_time + preview_time; + +// 根据绝对时间戳匹配参考点和预瞄点 +// 参考点：参考轨迹上，当前时刻车辆应该处于的位置 TrajectoryPoint reference_point = trajectory_analyzer->QueryNearestPointByAbsoluteTime( current_control_time); +// 预瞄点：参考轨迹上，预瞄绝对时间处车辆应该处于的位置 TrajectoryPoint preview_point = trajectory_analyzer->QueryNearestPointByAbsoluteTime( preview_control_time); @@ -658,9 +704,11 @@ void LonController::ComputeLongitudinalErrors( debug->set_station_reference(reference_point.path_point().s()); debug->set_current_station(s_matched); + // 纵向位置误差debug.station_error = 参考点路径点的累积弧长 - 匹配点的累积弧长(匹配点就是路径最近点) debug->set_station_error(reference_point.path_point().s() - s_matched); debug->set_speed_reference(reference_point.v()); debug->set_current_speed(lon_speed); + // 速度误差 = 参考点速度 - 匹配点速度 debug->set_speed_error(reference_point.v() - s_dot_matched); debug->set_acceleration_reference(reference_point.a()); debug->set_current_acceleration(lon_acceleration); @@ -676,7 +724,9 @@ void LonController::ComputeLongitudinalErrors( previous_acceleration_reference_ = debug->acceleration_reference(); previous_acceleration_ = debug->current_acceleration(); + // 预瞄点位置误差 = 预瞄点的纵向位置s - 匹配点纵向位置s debug->set_preview_station_error(preview_point.path_point().s() - s_matched); + // 预瞄点速度误差 = 预瞄点纵向速度 - 匹配点纵向速度 debug->set_preview_speed_error(preview_point.v() - s_dot_matched); debug->set_preview_speed_reference(preview_point.v()); debug->set_preview_acceleration_reference(preview_point.a()); diff --git a/modules/map/hdmap/adapter/xml_parser/header_xml_parser.cc b/modules/map/hdmap/adapter/xml_parser/header_xml_parser.cc index 916707944e5..dedb78a577b 100644 --- a/modules/map/hdmap/adapter/xml_parser/header_xml_parser.cc +++ b/modules/map/hdmap/adapter/xml_parser/header_xml_parser.cc @@ -56,27 +56,27 @@ Status HeaderXmlParser::Parse(const tinyxml2::XMLElement& xml_node, double east = 0.0; std::string vendor; int checker = - UtilXmlParser::QueryStringAttribute(*header_node, "revMajor", &rev_major); + UtilXmlParser::QueryStringAttribute(*header_node, "revMajor", &rev_major); // 主版本号 checker += - UtilXmlParser::QueryStringAttribute(*header_node, "revMinor", &rev_minor); + UtilXmlParser::QueryStringAttribute(*header_node, "revMinor", &rev_minor); // 次版本号 checker += UtilXmlParser::QueryStringAttribute(*header_node, "name", &database_name); checker += - UtilXmlParser::QueryStringAttribute(*header_node, "version", &version); - checker += UtilXmlParser::QueryStringAttribute(*header_node, "date", &date); - checker += header_node->QueryDoubleAttribute("north", &north); - checker += header_node->QueryDoubleAttribute("south", &south); - checker += header_node->QueryDoubleAttribute("east", &east); - checker += header_node->QueryDoubleAttribute("west", &west); + UtilXmlParser::QueryStringAttribute(*header_node, "version", &version); // 地图版本 + checker += UtilXmlParser::QueryStringAttribute(*header_node, "date", &date); // 地图创建时间 + checker += header_node->QueryDoubleAttribute("north", &north); // 地图北边界纬度 + checker += header_node->QueryDoubleAttribute("south", &south); // 地图南边界纬度 + checker += header_node->QueryDoubleAttribute("east", &east); // 地图东边界经度 + checker += header_node->QueryDoubleAttribute("west", &west); // 地图西边界经度 checker += - UtilXmlParser::QueryStringAttribute(*header_node, "vendor", &vendor); + UtilXmlParser::QueryStringAttribute(*header_node, "vendor", &vendor); // 地图供应商 if (checker != tinyxml2::XML_SUCCESS) { std::string err_msg = "Error parsing header attributes"; return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg); } - auto geo_reference_node = header_node->FirstChildElement("geoReference"); + auto geo_reference_node = header_node->FirstChildElement("geoReference"); // 地图使用的坐标系统 if (!geo_reference_node) { std::string err_msg = "Error parsing header geoReoference attributes"; return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg); diff --git a/modules/map/hdmap/hdmap_util.cc b/modules/map/hdmap/hdmap_util.cc index 917d73fa344..203b51de774 100644 --- a/modules/map/hdmap/hdmap_util.cc +++ b/modules/map/hdmap/hdmap_util.cc @@ -44,6 +44,7 @@ std::string FindFirstExist(const std::string& dir, const std::string& files) { } // namespace +// FindFirstExist：在指定目录和文件名列表中找到第一个存在的文件路径（通常是依次尝试多个文件名） std::string BaseMapFile() { if (FLAGS_use_navigation_mode) { AWARN << "base_map file is not used when FLAGS_use_navigation_mode is true"; @@ -77,8 +78,11 @@ std::unique_ptr CreateMap(const std::string& map_file_path) { AINFO << "Load HDMap success: " << map_file_path; return hdmap; } - +/// @brief +/// @param map_msg +/// @return std::unique_ptr CreateMap(const MapMsg& map_msg) { + // 创建一个 std::unique_ptr 类型的智能指针，并将其指向一个动态分配的 HDMap 对象 std::unique_ptr hdmap(new HDMap()); if (hdmap->LoadMapFromProto(map_msg.hdmap()) != 0) { AERROR << "Failed to load RelativeMap: " @@ -94,7 +98,9 @@ std::mutex HDMapUtil::base_map_mutex_; std::unique_ptr HDMapUtil::sim_map_ = nullptr; std::mutex HDMapUtil::sim_map_mutex_; - +/// @brief 返回一个指向常量 HDMap 类型的指针 +/// @param map_msg 一个常量引用类型的 MapMsg 对象 +/// @return const HDMap* HDMapUtil::BaseMapPtr(const MapMsg& map_msg) { std::lock_guard lock(base_map_mutex_); if (base_map_ != nullptr && diff --git a/modules/map/pnc_map/path.cc b/modules/map/pnc_map/path.cc index ad0e10ed62a..73010b690f5 100644 --- a/modules/map/pnc_map/path.cc +++ b/modules/map/pnc_map/path.cc @@ -445,7 +445,7 @@ void Path::InitWidth() { road_right_width_.clear(); road_right_width_.reserve(num_sample_points_); - double sample_s = 0; + double sample_s = 0; // 当前采样点的 s（沿路径的累计距离） double segment_end_s = -1.0; double segment_start_s = -1.0; double waypoint_s = 0.0; @@ -467,6 +467,7 @@ void Path::InitWidth() { } } // Find the width of the way point at the position of "sample_s". + // sample 在 path 的 s = 该 segment 起点在 lane 上的 s + 从 segment 起点到 sample_s 的距离 waypoint_s = cur_waypoint->s + sample_s - segment_start_s; cur_waypoint->lane->GetWidth(waypoint_s, &left_width, &right_width); lane_left_width_.push_back(left_width - cur_waypoint->l); @@ -586,20 +587,34 @@ void Path::InitOverlaps() { GetAllOverlaps(std::bind(&LaneInfo::parking_spaces, _1), &parking_space_overlaps_); } - +/// @brief 根据给定的 InterpolatedIndex 在路径上获取平滑的路径点 +/// @param index 一个 InterpolatedIndex，包含了路径段的索引 id 和相对于该路径段起始点的偏移量 offset +/// @return 返回一个 MapPathPoint，它表示路径上平滑插值后的位置，包括了位置、朝向、以及可能的车道信息 MapPathPoint Path::GetSmoothPoint(const InterpolatedIndex& index) const { + // 确保 index.id 在有效范围内（大于等于 0 且小于路径点的总数 num_points_） CHECK_GE(index.id, 0); CHECK_LT(index.id, num_points_); - - const MapPathPoint& ref_point = path_points_[index.id]; + // 获取路径上的参考点 ref_point，它是路径中 index.id 对应的点 + const MapPathPoint& ref_point = path_points_[index.id]; // path_points_：路径上的所有参考点 + // 如果偏移量 offset 大于一个小的容忍值 kMathEpsilon（即路径点有实际的偏移），就会进行插值计算 if (std::abs(index.offset) > kMathEpsilon) { + //使用路径段的单位方向向量 unit_directions_[index.id] 和偏移量 index.offset 来计算偏移量 delta， + //然后根据该偏移量调整参考点的位置。新的位置就是 ref_point 的坐标加上偏移量 delta + // lane_segments_to_next_point_：路径段对应的车道段信息 + // LaneWaypoint 和 MapPathPoint：表示路径上的车道点和路径点 const Vec2d delta = unit_directions_[index.id] * index.offset; MapPathPoint point({ref_point.x() + delta.x(), ref_point.y() + delta.y()}, ref_point.heading()); + + // 处理车道信息 + // 如果路径段包含车道信息（即 ref_point.lane_waypoints() 不为空），并且 index.id 小于路径段数 num_segments_，就会查找车道信息 if (index.id < num_segments_ && !ref_point.lane_waypoints().empty()) { + // 使用 lane_segments_to_next_point_ 来获取当前路径段对应的车道段 lane_segment， + // 然后根据车道段和参考点的车道信息，创建一个新的 LaneWaypoint 并将其添加到新的路径点 point 中 const LaneSegment& lane_segment = lane_segments_to_next_point_[index.id]; auto ref_lane_waypoint = ref_point.lane_waypoints()[0]; if (lane_segment.lane != nullptr) { + // 如果参考点包含多个车道信息，那么就会选择与当前车道段匹配的车道信息 for (const auto& lane_waypoint : ref_point.lane_waypoints()) { if (lane_waypoint.lane->id().id() == lane_segment.lane->id().id()) { ref_lane_waypoint = lane_waypoint; @@ -611,6 +626,8 @@ MapPathPoint Path::GetSmoothPoint(const InterpolatedIndex& index) const { ref_lane_waypoint.l)); } } + // 处理没有车道信息的情况 + // 如果新生成的路径点 point 没有车道信息，并且参考点 ref_point 中包含车道信息，那么就将参考点的车道信息复制到新路径点中 if (point.lane_waypoints().empty() && !ref_point.lane_waypoints().empty()) { point.add_lane_waypoint(ref_point.lane_waypoints()[0]); } @@ -633,7 +650,9 @@ double Path::GetSFromIndex(const InterpolatedIndex& index) const { } return accumulated_s_[index.id] + index.offset; } - +/// @brief 根据给定的 s 值（路径上的位置），返回该位置对应的路径段的索引及其相对 s 的偏移量 +/// @param s 路径上的某个位置的累积 s 值，通常表示路径从起点到当前位置的距离 +/// @return id：表示路径段的索引， offset：表示该路径段内给定 s 值的相对偏移 InterpolatedIndex Path::GetIndexFromS(double s) const { if (s <= 0.0) { return {0, 0.0}; @@ -642,15 +661,24 @@ InterpolatedIndex Path::GetIndexFromS(double s) const { if (s >= length_) { return {num_points_ - 1, 0.0}; } + // 计算 s 所在的样本点索引 sample_id，该索引是通过将 s 除以样本点间距 kSampleDistance 来得到的 const int sample_id = static_cast(s / kSampleDistance); + // 如果计算得到的样本点索引超出了样本点数组的范围，则返回路径的最后一个点 if (sample_id >= num_sample_points_) { return {num_points_ - 1, 0.0}; } + // 计算下一个样本点的索引，用于后续的二分查找 const int next_sample_id = sample_id + 1; + // low 表示路径段的起始索引，这个值是从 last_point_index_ 数组中获取的，该数组保存了每个样本点索引对应的路径段起始位置 int low = last_point_index_[sample_id]; + // high 表示路径段的结束索引，通过计算下一个样本点的路径段索引来确定。如果没有下一个样本点，则 high 取路径的总点数 num_points_ int high = (next_sample_id < num_sample_points_ ? std::min(num_points_, last_point_index_[next_sample_id] + 1) : num_points_); + // 二分查找的循环 + // 进入二分查找的循环，查找路径段的索引。根据 accumulated_s_（路径上每个点的累积 s 值）与给定的 s 值的大小关系， + // 不断调整 low 和 high，直到找到最合适的路径段。 + //mid 是当前查找范围的中点，通过比较 accumulated_s_[mid] 与 s 的关系，决定更新 low 还是 high while (low + 1 < high) { const int mid = (low + high) >> 1; if (accumulated_s_[mid] <= s) { @@ -659,6 +687,8 @@ InterpolatedIndex Path::GetIndexFromS(double s) const { high = mid; } } + // low：路径段的索引 + // s - accumulated_s_[low]：给定 s 值相对于该路径段的偏移量，即 s 在该段内的相对位置 return {low, s - accumulated_s_[low]}; } @@ -740,15 +770,25 @@ bool Path::GetProjection(const common::math::Vec2d& point, double* accumulate_s, double distance = 0.0; return GetProjection(point, accumulate_s, lateral, &distance); } - +/// @brief +/// @param heading 给定点的航向角 +/// @param point 在笛卡尔坐标系中的点 +/// @param accumulate_s 沿路径的累积 s 值 +/// @param lateral 投影点的横向偏移 +/// @return bool Path::GetProjection(const double heading, const common::math::Vec2d& point, double* accumulate_s, double* lateral) const { + // distance 被初始化为 0.0，但实际的值是在调用 GetProjection 的时候计算的 double distance = 0.0; return GetProjection(point, heading, accumulate_s, lateral, &distance); } - +/// @brief +/// @param point 给定点的坐标，类型为 common::math::Vec2d +/// @param accumulate_s 给定点的起始累积 s 值（即给定点的初始位置沿路径的距离） +/// @param lateral 输出参数，表示投影点在路径上的横向偏移（即 Frenet 坐标系中的 l，垂直于路径的距离） +/// @return bool Path::GetProjectionWithWarmStartS(const common::math::Vec2d& point, double* accumulate_s, double* lateral) const { @@ -758,6 +798,7 @@ bool Path::GetProjectionWithWarmStartS(const common::math::Vec2d& point, if (accumulate_s == nullptr || lateral == nullptr) { return false; } + // 确保 accumulate_s 在有效的范围内。它应该大于等于 0 且小于路径的总长度 length() if (*accumulate_s < 0.0) { *accumulate_s = 0.0; } else if (*accumulate_s > length()) { @@ -766,20 +807,30 @@ bool Path::GetProjectionWithWarmStartS(const common::math::Vec2d& point, CHECK_GE(num_points_, 2); double warm_start_s = *accumulate_s; // Find the segment at the position of "accumulate_s". + // 初始化二分查找的索引 + // 初始化左右边界索引（left_index 和 right_index）用于二分查找，mid_index 是中间点的索引 int left_index = 0; int right_index = num_segments_; int mid_index = 0; // Find the segment with projection of the given point on it. while (right_index > left_index + 1) { + // 根据当前的 warm_start_s 计算中间索引 mid_index FindIndex(left_index, right_index, warm_start_s, &mid_index); + // 获取路径段的起点（start_point）并计算给定点到路径段起点的偏移（delta_x 和 delta_y） const auto& segment = segments_[mid_index]; const auto& start_point = segment.start(); double delta_x = point.x() - start_point.x(); double delta_y = point.y() - start_point.y(); + // unit_direction：路径段的单位方向向量 const auto& unit_direction = segment.unit_direction(); + // 计算投影值 proj，即给定点到路径段起点的向量在路径段单位方向上的投影 double proj = delta_x * unit_direction.x() + delta_y * unit_direction.y(); *accumulate_s = accumulated_s_[mid_index] + proj; + // 计算横向偏移 lateral，即路径段方向与给定点偏移的叉积（表示离路径的横向距离） *lateral = unit_direction.x() * delta_y - unit_direction.y() * delta_x; + // 如果投影 proj 为正且小于当前路径段的长度，表示点在当前路径段上 + // 如果投影超过当前路径段的长度，则更新 left_index 或 right_index，继续二分查找，直到找到最合适的路径段 + // 调整 warm_start_s，确保下一次查找从正确的位置开始 if (proj > 0.0) { if (proj < segment.length()) { return true; @@ -859,9 +910,11 @@ bool Path::GetProjectionWithHueristicParams(const Vec2d& point, bool Path::GetProjection(const Vec2d& point, double* accumulate_s, double* lateral, double* min_distance) const { + // 如果路径的所有线段为空（即没有路径），则返回 false，表示投影计算失败 if (segments_.empty()) { return false; } + // 判断参数不为空指针 if (accumulate_s == nullptr || lateral == nullptr || min_distance == nullptr) { return false; @@ -870,8 +923,11 @@ bool Path::GetProjection(const Vec2d& point, double* accumulate_s, return approximation_.GetProjection(*this, point, accumulate_s, lateral, min_distance); } + // 确保路径中至少有两个点，否则该断言会触发错误 CHECK_GE(num_points_, 2); + // 初始化最小距离为正无穷大（infinity） *min_distance = std::numeric_limits::infinity(); + // 初始化 min_index 变量，用于记录距离最近的线段的索引 int min_index = 0; for (int i = 0; i < num_segments_; ++i) { const double distance = segments_[i].DistanceSquareTo(point); @@ -880,50 +936,76 @@ bool Path::GetProjection(const Vec2d& point, double* accumulate_s, *min_distance = distance; } } + // 计算最小距离的平方根，得到实际的最小距离 *min_distance = std::sqrt(*min_distance); + // 获取距离最近的线段（nearest_seg） const auto& nearest_seg = segments_[min_index]; + // 计算点 point 在 nearest_seg 这条线段的单位向量上的投影积（即点到线段的垂直投影距离的标量值） const auto prod = nearest_seg.ProductOntoUnit(point); + // 计算点 point 投影到 nearest_seg 单位向量上的值，表示点到线段的投影位置 const auto proj = nearest_seg.ProjectOntoUnit(point); - if (min_index == 0) { + + if (min_index == 0) {// 如果距离最近的线段是路径的第一个线段 + // *accumulate_s 设为投影值和当前线段长度的最小值 *accumulate_s = std::min(proj, nearest_seg.length()); if (proj < 0) { - *lateral = prod; + *lateral = prod; // 表示投影在该线段的延长线上，此时侧向距离 lateral 等于投影积 prod } else { - *lateral = (prod > 0.0 ? 1 : -1) * *min_distance; + *lateral = (prod > 0.0 ? 1 : -1) * *min_distance; // 根据投影积 prod 的符号来设置，表示点与路径的侧向距离 } - } else if (min_index == num_segments_ - 1) { - *accumulate_s = accumulated_s_[min_index] + std::max(0.0, proj); + } else if (min_index == num_segments_ - 1) { // 如果距离最近的线段是路径的最后一个线段 + *accumulate_s = accumulated_s_[min_index] + std::max(0.0, proj); // *accumulate_s 设为累积的路径长度加上投影值与 0 的最大值 if (proj > 0) { *lateral = prod; } else { *lateral = (prod > 0.0 ? 1 : -1) * *min_distance; } - } else { + } else { // 如果距离最近的线段是路径的中间部分 *accumulate_s = accumulated_s_[min_index] + - std::max(0.0, std::min(proj, nearest_seg.length())); + std::max(0.0, std::min(proj, nearest_seg.length())); // *accumulate_s 设为累积的路径长度加上投影值与当前线段长度的最小值与 0 的最大值 *lateral = (prod > 0.0 ? 1 : -1) * *min_distance; } return true; } - +/// @brief +/// @param point 该点在笛卡尔坐标系中的坐标 +/// @param heading 给定点的航向角（角度） +/// @param accumulate_s 输出，投影点在路径上的累计 s 值 +/// @param lateral 输出，投影点的横向偏移 l（即离路径的横向距离） +/// @param min_distance 输出，给定点到路径投影点的最小距离 +/// @return +// 起始点投影用这个 +// 将一个 笛卡尔坐标系中的点 (x, y) 投影到一条路径 Path 上，并计算出该点在路径上的 +// s（纵向投影位置，即沿路径的累计距离） +// l（横向偏移距离，正负代表左/右偏） +// min_distance（到路径的最近欧氏距离） bool Path::GetProjection(const Vec2d& point, const double heading, double* accumulate_s, double* lateral, double* min_distance) const { +// 如果路径没有任何段（segments_ 为空），或者输出参数为空，函数返回 false，表示无法计算投影 if (segments_.empty()) { return false; } + +// 判断指针非空，防止访问非法内存 if (accumulate_s == nullptr || lateral == nullptr || min_distance == nullptr) { return false; } + + // 判断是否启用路径近似投影 if (use_path_approximation_) { return approximation_.GetProjection(*this, point, accumulate_s, lateral, min_distance); } CHECK_GE(num_points_, 2); + // 将最小距离初始化为正无穷（表示开始时找不到任何路径段），并初始化投影点所在路径段的索引为 0 *min_distance = std::numeric_limits::infinity(); int min_index = 0; + + /// 搜索最近段落 for (int i = 0; i < num_segments_; ++i) { + // 过滤路径段的航向角与给定点航向角 heading 之间的角度大于90的点 右 x 0 前 y 90 逆时针为正 if (abs(common::math::AngleDiff(segments_[i].heading(), heading)) >= M_PI_2) continue; const double distance = segments_[i].DistanceSquareTo(point); @@ -932,27 +1014,49 @@ bool Path::GetProjection(const Vec2d& point, const double heading, *min_distance = distance; } } + // 通过上述的计算获取了最近距离以及对应的索引 + + // 开始正式投影 *min_distance = std::sqrt(*min_distance); + // 计算投影点在路径上的 s 和 l const auto& nearest_seg = segments_[min_index]; - const auto prod = nearest_seg.ProductOntoUnit(point); - const auto proj = nearest_seg.ProjectOntoUnit(point); + const auto prod = nearest_seg.ProductOntoUnit(point); // prod点到路径的叉积(判断左右偏) + const auto proj = nearest_seg.ProjectOntoUnit(point); // proj：点到路径段的投影（可能为负值或大于路径段的长度） + + // 根据点在路径段的位置（起始、中间、末尾）分别处理 + // 对于起点，s 是 proj 和路径段长度的最小值，l 是点积 prod 或最小距离 + // 起点段 if (min_index == 0) { + // s 最多就是这一段的长度 *accumulate_s = std::min(proj, nearest_seg.length()); if (proj < 0) { + // 若投影在段前，l 直接用 prod *lateral = prod; } else { + // l 按 prod 的符号赋值为 ±min_distance *lateral = (prod > 0.0 ? 1 : -1) * *min_distance; } - } else if (min_index == num_segments_ - 1) { + } + + // 对于终点，s 是累积的 s 加上投影值，l 同样取决于投影的符号和最小距离 + // 终点段 + else if (min_index == num_segments_ - 1) { + // s 是累积 s + 投影位置（不能小于 0） *accumulate_s = accumulated_s_[min_index] + std::max(0.0, proj); if (proj > 0) { *lateral = prod; } else { *lateral = (prod > 0.0 ? 1 : -1) * *min_distance; } - } else { + } + + // 对于中间段，s 是累积的 s 加上投影值（或路径段长度的最小值），l 同样是点积 prod 和最小距离 + // 中间段 + else { + // s 在累积基础上加上夹在当前段内的投影 *accumulate_s = accumulated_s_[min_index] + std::max(0.0, std::min(proj, nearest_seg.length())); + // l 直接根据 prod 正负判断左右偏 *lateral = (prod > 0.0 ? 1 : -1) * *min_distance; } return true; @@ -1022,7 +1126,7 @@ double Path::GetSample(const std::vector& samples, if (s <= 0.0) { return samples[0]; } - const int idx = static_cast(s / kSampleDistance); + const int idx = static_cast(s / kSampleDistance); // 0.25 if (idx >= num_sample_points_ - 1) { return samples.back(); } diff --git a/modules/perception/traffic_light_detection/detector/caffe_detection/detection.h b/modules/perception/traffic_light_detection/detector/caffe_detection/detection.h index 84e81f5aab7..d80be33b73c 100644 --- a/modules/perception/traffic_light_detection/detector/caffe_detection/detection.h +++ b/modules/perception/traffic_light_detection/detector/caffe_detection/detection.h @@ -15,6 +15,15 @@ *****************************************************************************/ #pragma once +/* +功能 方法 +初始化 Init() +检测交通灯 Detect() +运行神经网络 Inference() +筛选交通灯框 SelectOutputBoxes() +去除重叠框 ApplyNMS() +*/ + #include #include #include @@ -53,6 +62,9 @@ class TrafficLightDetection : public BaseTrafficLightDetector { * @return true * @return false */ + // 初始化交通灯检测器，加载模型参数 + // 初始化交通灯检测器 + // 可能涉及 加载模型参数、设置推理器、初始化 GPU 资源等 bool Init(const TrafficLightDetectorInitOptions &options) override; /** @@ -62,6 +74,12 @@ class TrafficLightDetection : public BaseTrafficLightDetector { * @return true * @return false */ + // 在图像中检测交通信号灯 + // 从输入图像中检测交通信号灯 + // 主要调用： + // 1.Inference()：使用深度学习模型进行交通灯检测 + // 2.SelectOutputBoxes()：筛选有效检测结果 + // 3.ApplyNMS()：去除重叠的框 bool Detect(camera::TrafficLightFrame *frame) override; /** * @brief Dump output of inference results. @@ -73,6 +91,9 @@ class TrafficLightDetection : public BaseTrafficLightDetector { * @return true * @return false */ + // 对推理结果进行后处理，筛选有效的交通灯框 + // 对模型输出的检测框进行筛选和转换 + // 考虑缩放比例，将推理结果映射回原始图像坐标 bool SelectOutputBoxes(const std::vector &crop_box_list, const std::vector &resize_scale_list_col, const std::vector &resize_scale_list_row, @@ -83,6 +104,9 @@ class TrafficLightDetection : public BaseTrafficLightDetector { * @param lights * @param iou_thresh */ + // 使用非极大值抑制（NMS）过滤重叠的检测框 + // 使用非极大值抑制（NMS），过滤重叠的检测框，保留最优结果 + // iou_thresh（交并比阈值）用于判定两个框是否重叠 void ApplyNMS(std::vector *lights, double iou_thresh = 0.6); /** @@ -93,6 +117,9 @@ class TrafficLightDetection : public BaseTrafficLightDetector { * @return true * @return false */ + // 执行神经网络推理（深度学习模型） + // 运行神经网络模型，进行推理（Inference） + // 使用 rt_net_ 执行深度学习计算，预测交通灯的位置 bool Inference(std::vector *lights, camera::DataProvider *data_provider); /** @@ -105,26 +132,29 @@ class TrafficLightDetection : public BaseTrafficLightDetector { } private: - trafficlight::ModelParam detection_param_; - std::string detection_root_dir; + trafficlight::ModelParam detection_param_; // 存储交通灯检测模型的参数 + std::string detection_root_dir; // 存储模型文件路径 - camera::DataProvider::ImageOptions data_provider_image_option_; - std::shared_ptr rt_net_ = nullptr; - std::shared_ptr image_ = nullptr; - std::shared_ptr> param_blob_; - std::shared_ptr> mean_buffer_; - std::shared_ptr crop_; - std::vector detected_bboxes_; - std::vector selected_bboxes_; - std::vector net_inputs_; + camera::DataProvider::ImageOptions data_provider_image_option_; // 图像预处理参数 + // rt_net_ 是神经网络推理器，用来执行深度学习推理，可能基于 TensorRT、Caffe、ONNX Runtime 等 + std::shared_ptr rt_net_ = nullptr; // 神经网络推理器，执行深度学习推理 + std::shared_ptr image_ = nullptr; // 处理后的图像 + std::shared_ptr> param_blob_; // 神经网络的输入参数 + std::shared_ptr> mean_buffer_; // 归一化的均值 + std::shared_ptr crop_; // 裁剪算法，用于从原始图像裁剪交通灯区域 + // detected_bboxes_：存储所有检测到的交通灯（可能包含误检） + std::vector detected_bboxes_; // 存储所有检测到的交通灯框 + // selected_bboxes_：经过筛选后的交通灯 + std::vector selected_bboxes_; // 存储筛选后的交通灯框 + std::vector net_inputs_; std::vector net_outputs_; - Select select_; + Select select_; // 选择有效的交通灯 int max_batch_size_; int param_blob_length_; float mean_[3]; std::vector crop_box_list_; std::vector resize_scale_list_; - int gpu_id_; + int gpu_id_; // 指定 GPU 设备 ID DISALLOW_COPY_AND_ASSIGN(TrafficLightDetection); }; // class TrafficLightDetection diff --git a/modules/planning/planners/public_road/public_road_planner.cc b/modules/planning/planners/public_road/public_road_planner.cc index f8dfeef3555..05dabab9162 100644 --- a/modules/planning/planners/public_road/public_road_planner.cc +++ b/modules/planning/planners/public_road/public_road_planner.cc @@ -24,12 +24,18 @@ namespace planning { using apollo::common::Status; using apollo::common::TrajectoryPoint; - +/// @brief 实例化一个全局的scenario_manager_对象来进行场景管理 +/// @param injector +/// @param config_path +/// @return +// 初始化 PublicRoadPlanner 类的对象，包括初始化父类、加载配置文件、初始化场景管理器，并最终返回一个成功状态 Status PublicRoadPlanner::Init( const std::shared_ptr& injector, const std::string& config_path) { + // 先调用父类的初始化操作 Planner::Init(injector, config_path); - LoadConfig(config_path, &config_); + // 加载配置文件，并将其内容存储到 config_ 成员变量中 + LoadConfig(config_path, &config_); // config_path： public_road_planner_config.pb.txt scenario_manager_.Init(injector, config_); return Status::OK(); } @@ -37,12 +43,15 @@ Status PublicRoadPlanner::Init( Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point, Frame* frame, ADCTrajectory* ptr_computed_trajectory) { + // 更新场景，决策当前应该执行什么场景 scenario_manager_.Update(planning_start_point, frame); + // 获取当前场景 scenario_ = scenario_manager_.mutable_scenario(); if (!scenario_) { return Status(apollo::common::ErrorCode::PLANNING_ERROR, "Unknown Scenario"); } + // 调用Scenario的Process函数，对具体的场景进行处理 auto result = scenario_->Process(planning_start_point, frame); if (FLAGS_enable_record_debug) { @@ -53,12 +62,13 @@ Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point, scenario_debug->set_stage_plugin_type(scenario_->GetStage()); scenario_debug->set_msg(scenario_->GetMsg()); } - + // 当前场景完成 if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_DONE) { // only updates scenario manager when previous scenario's status is // STATUS_DONE scenario_manager_.Update(planning_start_point, frame); } else if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_UNKNOWN) { + // 当前场景失败 return Status(common::PLANNING_ERROR, result.GetTaskStatus().error_message()); } diff --git a/modules/planning/planners/public_road/scenario_manager.cc b/modules/planning/planners/public_road/scenario_manager.cc index e80bab8235e..1add2b60f53 100644 --- a/modules/planning/planners/public_road/scenario_manager.cc +++ b/modules/planning/planners/public_road/scenario_manager.cc @@ -19,51 +19,73 @@ #include #include #include - +// 引入 Apollo 的 插件管理器，用于 动态创建场景对象（通过类名字符串） #include "cyber/plugin_manager/plugin_manager.h" +// 引入状态宏（如 ACHECK, AINFO），用于日志和断言 #include "modules/common/status/status.h" +// 引入配置工具类 ConfigUtil，用于 拼接完整的 C++ 类名（从配置中的简写名转为带命名空间的全名) #include "modules/planning/planning_base/common/util/config_util.h" +// 引入 Scenario 基类定义。所有具体场景（如车道跟随、红绿灯等）都继承自它 #include "modules/planning/planning_interface_base/scenario_base/scenario.h" namespace apollo { namespace planning { using apollo::cyber::plugin_manager::PluginManager; - +/// @brief 场景注册 +/// @param injector +/// @param planner_config +/// @return bool ScenarioManager::Init(const std::shared_ptr& injector, const PlannerPublicRoadConfig& planner_config) { if (init_) { return true; } + //保存依赖注入器到成员变量 injector_，供后续创建场景时使用 injector_ = injector; + // 加载并创建所有 Scenario for (int i = 0; i < planner_config.scenario_size(); i++) { + // 根据配置中的类型字符串（如 "LAYER_FOLLOW", "PARK_AND_GO" 等），通过 插件系统动态创建对应的 Scenario 对象 + // 利用 插件机制（PluginManager） 动态创建每个 Scenario 实例 auto scenario = PluginManager::Instance()->CreateInstance( ConfigUtil::GetFullPlanningClassName( planner_config.scenario(i).type())); + // 用依赖注入器和场景名称初始化该 Scenario ACHECK(scenario->Init(injector_, planner_config.scenario(i).name())) << "Can not init scenario" << planner_config.scenario(i).name(); + // 把创建好的场景加入场景列表 scenario_list_.push_back(scenario); if (planner_config.scenario(i).name() == "LANE_FOLLOW") { default_scenario_type_ = scenario; } } AINFO << "Load scenario list:" << planner_config.DebugString(); + // 默认为LANE_FOLLOW current_scenario_ = default_scenario_type_; init_ = true; return true; } - +/// @brief 更新场景状态 +/// @param ego_point 自车规划的起始点 +/// @param frame void ScenarioManager::Update(const common::TrajectoryPoint& ego_point, Frame* frame) { + // 如果frame为空指针，程序将触发断言失败，终止执行 CHECK_NOTNULL(frame); for (auto scenario : scenario_list_) { + // current_scenario_是一个智能指针，get()方法返回原始指针 + // 如果当前正在处理的场景（current_scenario_）就是遍历到的这个 scenario，并且它的状态是 正在处理中（STATUS_PROCESSING） if (current_scenario_.get() == scenario.get() && current_scenario_->GetStatus() == ScenarioStatusType::STATUS_PROCESSING) { // The previous scenario has higher priority + // 如果当前场景与遍历到的场景相同，并且当前场景正在处理中，则返回，意味着不需要切换场景 return; } + // 如果当前场景不是在正在处理状态，接着判断场景是否可以切换 + // 检查该 scenario 是否可以从当前场景 转移过来 if (scenario->IsTransferable(current_scenario_.get(), *frame)) { + // 如果场景切换可行，退出当前场景 current_scenario_->Exit(frame); AINFO << "switch scenario from" << current_scenario_->Name() << " to " << scenario->Name(); diff --git a/modules/planning/planning_base/common/dependency_injector.h b/modules/planning/planning_base/common/dependency_injector.h index d81aa0585e6..b5f23136fc7 100644 --- a/modules/planning/planning_base/common/dependency_injector.h +++ b/modules/planning/planning_base/common/dependency_injector.h @@ -30,14 +30,20 @@ class DependencyInjector { public: DependencyInjector() = default; ~DependencyInjector() = default; - + // PlanningContext 存储了当前规划任务的上下文（如前一帧信息、紧急状态等） PlanningContext* planning_context() { return &planning_context_; } + // FrameHistory 存储了过去的多个帧数据，用于规划模块进行时序分析 FrameHistory* frame_history() { return &frame_history_; } + // History 存储了全局历史信息，用于回溯过去的规划决策 History* history() { return &history_; } + // EgoInfo 存储了当前自动驾驶车辆的状态（如速度、位置、加速度等） EgoInfo* ego_info() { return &ego_info_; } + // VehicleStateProvider 提供了车辆状态信息 + // 当前位置、速度、方向角、车辆运动学信息 apollo::common::VehicleStateProvider* vehicle_state() { return &vehicle_state_; } + // LearningBasedData 用于存储学习数据，供离线/在线机器学习模块使用，优化规划算法 LearningBasedData* learning_based_data() { return &learning_based_data_; } private: diff --git a/modules/planning/planning_base/common/ego_info.cc b/modules/planning/planning_base/common/ego_info.cc index e8742904eff..95e8eff5350 100644 --- a/modules/planning/planning_base/common/ego_info.cc +++ b/modules/planning/planning_base/common/ego_info.cc @@ -40,7 +40,8 @@ bool EgoInfo::Update(const common::TrajectoryPoint& start_point, CalculateEgoBox(vehicle_state); return true; } - +/// @brief 计算和更新 EgoInfo 类中的车辆边界框（ego_box_） +/// @param vehicle_state void EgoInfo::CalculateEgoBox(const common::VehicleState& vehicle_state) { const auto& param = ego_vehicle_config_.vehicle_param(); ADEBUG << "param: " << param.DebugString(); @@ -48,10 +49,12 @@ void EgoInfo::CalculateEgoBox(const common::VehicleState& vehicle_state) { Vec2d vec_to_center( (param.front_edge_to_center() - param.back_edge_to_center()) / 2.0, (param.left_edge_to_center() - param.right_edge_to_center()) / 2.0); - +// position: 后轴中心点坐标 Vec2d position(vehicle_state.x(), vehicle_state.y()); + // 车辆相对于当前坐标系的几何中心位置 + //center是车辆几何中心的大地坐标 Vec2d center(position + vec_to_center.rotate(vehicle_state.heading())); - +//调用Box2d()构造函数，通过车辆几何中心，heading航向角，车辆的长度，宽度，构建二维边界盒对象ego_box_ ego_box_ = Box2d(center, vehicle_state.heading(), param.length(), param.width()); } diff --git a/modules/planning/planning_base/common/ego_info.h b/modules/planning/planning_base/common/ego_info.h index 8894e234a6d..b26fe438e87 100644 --- a/modules/planning/planning_base/common/ego_info.h +++ b/modules/planning/planning_base/common/ego_info.h @@ -64,10 +64,12 @@ class EgoInfo { void set_start_point(const common::TrajectoryPoint& start_point) { start_point_ = start_point; + // apollo/modules/common/configs/config_gflags.cc const auto& param = ego_vehicle_config_.vehicle_param(); + // 根据这些参数调整 start_point_ 的加速度，确保其在合理的范围内（不超过最大加速度且不低于最大减速度） start_point_.set_a( std::fmax(std::fmin(start_point_.a(), param.max_acceleration()), - param.max_deceleration())); + param.max_deceleration())); // 2 -6 } void CalculateEgoBox(const common::VehicleState& vehicle_state); diff --git a/modules/planning/planning_base/common/frame.cc b/modules/planning/planning_base/common/frame.cc index 227b0ffd0a3..fcf823350be 100644 --- a/modules/planning/planning_base/common/frame.cc +++ b/modules/planning/planning_base/common/frame.cc @@ -57,6 +57,7 @@ FrameHistory::FrameHistory() Frame::Frame(uint32_t sequence_num) : sequence_num_(sequence_num), + //modules\common\monitor_log\proto\monitor_log.proto定义的message类MonitorMessageItem，监视的msg模块是planning monitor_logger_buffer_(common::monitor::MonitorMessageItem::PLANNING) {} Frame::Frame(uint32_t sequence_num, const LocalView &local_view, @@ -70,6 +71,8 @@ Frame::Frame(uint32_t sequence_num, const LocalView &local_view, reference_line_provider_(reference_line_provider), monitor_logger_buffer_(common::monitor::MonitorMessageItem::PLANNING) {} +//Frame类带参构造函数，跟上面的构造函数区别就是没有最后一个参数参考线提供器 +//但其实调用的还是上面的构造函数，只不过最后一个参数传个nullptr空指针 Frame::Frame(uint32_t sequence_num, const LocalView &local_view, const common::TrajectoryPoint &planning_start_point, const common::VehicleState &vehicle_state) @@ -84,21 +87,32 @@ const common::VehicleState &Frame::vehicle_state() const { return vehicle_state_; } +//重新规划全局路径函数，输入参数是planning_context就是规划状态 +//就是从自车位置出发再次routing,主要是更新routing_request的起始点为自车位置 +//PlanningContext里包含routing_request +//这里找到自车最近的匹配车道上的最近点作为routing_request的起始点重新routing +//来设置PlanningContext里的rerouting对象 bool Frame::Rerouting(PlanningContext *planning_context) { + //导航模式下不支持rerouting，默认不是导航模式 if (FLAGS_use_navigation_mode) { AERROR << "Rerouting not supported in navigation mode"; return false; } + //如果local_view_里的routing结果里是空指针的话报错，说明没有先前的可用的routing if (local_view_.planning_command == nullptr) { AERROR << "No previous routing available"; return false; } + //如果hdmap_也就是空指针则报错返回 if (!hdmap_) { AERROR << "Invalid HD Map."; return false; } + //让指针rerouting指向规划状态planning_context里的 + //成员planning_status里的rerouting重新导航请求 auto *rerouting = planning_context->mutable_planning_status()->mutable_rerouting(); + //设置rerouting重新导航请求里需要重新导航为true rerouting->set_need_rerouting(true); auto *lane_follow_command = rerouting->mutable_lane_follow_command(); if (future_route_waypoints_.size() < 1) { @@ -133,6 +147,8 @@ void Frame::UpdateReferenceLinePriority( for (const auto &pair : id_to_priority) { const auto id = pair.first; const auto priority = pair.second; + //找到这个id对应的参考线信息对象，遍历所有的参考线信息对象，find_if返回id相等的那 + //一个放入ref_line_info_itr，就是pair这对数据对应的参考线信息类对象 auto ref_line_info_itr = std::find_if(reference_line_info_.begin(), reference_line_info_.end(), [&id](const ReferenceLineInfo &ref_line_info) { @@ -147,35 +163,63 @@ void Frame::UpdateReferenceLinePriority( bool Frame::CreateReferenceLineInfo( const std::list &reference_lines, const std::list &segments) { + //首先清空数据成员参考线信息类列表reference_line_info_ reference_line_info_.clear(); if (reference_lines.empty()) { return true; } + //获取参考线列表里的第一个对象ref_line_iter ，准备参与迭代 auto ref_line_iter = reference_lines.begin(); + //获取导航路径段列表里的第一个对象segments_iter ，准备参与迭代 auto segments_iter = segments.begin(); + //遍历每一条参考线列表里的每一条参考线 + //这个while循环的作用就是把参数里的参考线列表以及导航路径段列表用来构造参考线信息类对 + //象,挨个塞入Frame类数据成员reference_line_info_参考线信息类对象列表 while (ref_line_iter != reference_lines.end()) { + //若导航路径段因为到达终点附件而停止， + //设置数据成员标志位is_near_destination_ 足够靠近终点为true if (segments_iter->StopForDestination()) { is_near_destination_ = true; } + //将当前车辆状态，规划起始点，当前遍历的参考线对象，当前遍历的导航路径段对象一起塞 + //入类的数据成员reference_line_info_参考线信息类对象列表里 reference_line_info_.emplace_back(vehicle_state_, planning_start_point_, *ref_line_iter, *segments_iter); + //迭代器的参考线对象指向列表里的下一个 ++ref_line_iter; + //迭代器导航路径段对象指向列表里的下一个 ++segments_iter; } - + + //若Frame类数据成员参考线信息类对象列表里元素个数为2 if (reference_line_info_.size() == 2) { + //首先将车辆当前位置点xy构建一个Vec2d二维向量就是点坐标 common::math::Vec2d xy_point(vehicle_state_.x(), vehicle_state_.y()); + //定义一个空的frenet系坐标点first_sl (s,l) common::SLPoint first_sl; + //首先获取参考线信息类对象列表里的第一条参考线，将车辆当前位置点坐标xy_point + //投影到列表reference_line_info_里第一条参考线的frenet系下，投影的sl坐标 + //放入first_sl + //自车XY坐标转列表第一条参考线下的frenet坐标，结果放入first_sl if (!reference_line_info_.front().reference_line().XYToSL(xy_point, &first_sl)) { return false; } common::SLPoint second_sl; + //获取参考线信息类对象列表里的最后一条参考线，将车辆当前位置点坐标xy_point + //投影到列表reference_line_info_里最后一条参考线的frenet系下，投影的sl坐标 + //放入second_sl + //自车XY坐标转列表最后一条参考线下的frenet坐标，结果放入second_sl + //这个是在参考线信息类对象列表里元素为2，所以参考线其实就2条？ if (!reference_line_info_.back().reference_line().XYToSL(xy_point, &second_sl)) { return false; } + //计算自车坐标投影到第一条参考线和投影最后一条参考线的frenet系下的l坐标的偏差 + //这个是在参考线信息类对象列表里元素为2，所以参考线其实就2条？ const double offset = first_sl.l() - second_sl.l(); + //设置ReferenceLineInfo类对象列表reference_line_info_的数据成员 + //这个offset其实就是这两条参考线在横向上的相对偏移？ reference_line_info_.front().SetOffsetToOtherReferenceLine(offset); reference_line_info_.back().SetOffsetToOtherReferenceLine(-offset); } @@ -183,12 +227,16 @@ bool Frame::CreateReferenceLineInfo( if (local_view_.planning_command->has_target_speed()) { target_speed = local_view_.planning_command->target_speed(); } + //定义一个标志位 有有效的参考线？初始为false bool has_valid_reference_line = false; + //遍历Frame类的数据成员reference_line_info_参考线信息类对象列表 for (auto iter = reference_line_info_.begin(); iter != reference_line_info_.end();) { + //用障碍物列表去初始化当前遍历的参考线信息类对象 if (!iter->Init(obstacles(), target_speed)) { reference_line_info_.erase(iter++); } else { + //初始化成功就设置有有效的参考线标志位为true has_valid_reference_line = true; iter++; } @@ -206,19 +254,28 @@ bool Frame::CreateReferenceLineInfo( const Obstacle *Frame::CreateStopObstacle( ReferenceLineInfo *const reference_line_info, const std::string &obstacle_id, const double obstacle_s) { + //如果参考线信息类对象列表为空，报错返回 if (reference_line_info == nullptr) { AERROR << "reference_line_info nullptr"; return nullptr; } - + //获取输入参数reference_line_info参考线信息里的参考线 const auto &reference_line = reference_line_info->reference_line(); + //障碍物盒对应的中心点的frenet系s坐标=障碍物s坐标 + 0.1 / 2.0 + //virtual_stop_wall_length是gflags文件中定义的变量，为0.1 const double box_center_s = obstacle_s + FLAGS_virtual_stop_wall_length / 2.0; + //用参考线对象根据障碍物盒中心点s坐标获取其对应的参考点对象 auto box_center = reference_line.GetReferencePoint(box_center_s); + //获取这个障碍物盒中心点对应参考线上的参考点的Heading角 double heading = reference_line.GetReferencePoint(obstacle_s).heading(); + //定义虚拟障碍物盒的宽度为4.0m，宽度对应着车道宽度方向 + //长度对应着纵向 static constexpr double kStopWallWidth = 4.0; + //构建虚拟障碍物停止墙边界盒，用障碍物盒的中心点对应的参考点对象，障碍物盒的中心点对 + //应的参考线上参考点对象heading角，边界盒长度0.1m，边界盒宽度4.0m Box2d stop_wall_box{box_center, heading, FLAGS_virtual_stop_wall_length, kStopWallWidth}; - + //然后通过构建的虚拟障碍物的边界盒对象stop_wall_box和障碍物id去创建虚拟障碍物 return CreateStaticVirtualObstacle(obstacle_id, stop_wall_box); } @@ -244,19 +301,24 @@ const Obstacle *Frame::CreateStopObstacle(const std::string &obstacle_id, AERROR << "Failed to find lane[" << lane_id << "]"; return nullptr; } - + //终点的车道s坐标就是输入参数lane_s double dest_lane_s = std::max(0.0, lane_s); + //获取终点的车道s坐标对应的ENU大地系坐标点 auto dest_point = lane->GetSmoothPoint(dest_lane_s); double lane_left_width = 0.0; double lane_right_width = 0.0; + //用参数lane_id对应的车道对象lane去获取终点dest_lane_s处的车道中心线左右宽度。 + //获取的结果放入引用变量lane_left_width，lane_right_width lane->GetWidth(dest_lane_s, &lane_left_width, &lane_right_width); - + //构建停止墙边界盒对象stop_wall_box + //参数终点处x,y 终点处对应的车道heading,默认的虚拟墙长度0.1m，车道宽度(中心线做宽 + //度 + 中心线右宽度) Box2d stop_wall_box{{dest_point.x(), dest_point.y()}, lane->Heading(dest_lane_s), FLAGS_virtual_stop_wall_length, lane_left_width + lane_right_width}; - + //最后返回根据障碍物id和停止墙边界盒对象构建的虚拟障碍物对象 return CreateStaticVirtualObstacle(obstacle_id, stop_wall_box); } @@ -275,6 +337,7 @@ const Obstacle *Frame::CreateStaticObstacle( const auto &reference_line = reference_line_info->reference_line(); // start_xy + // 将输入参数障碍物起始位置从SL坐标转化成XY坐标obstacle_start_xy common::SLPoint sl_point; sl_point.set_s(obstacle_start_s); sl_point.set_l(0.0); @@ -285,6 +348,7 @@ const Obstacle *Frame::CreateStaticObstacle( } // end_xy + // 将输入参数障碍物终点位置从SL坐标转化成XY坐标obstacle_end_s sl_point.set_s(obstacle_end_s); sl_point.set_l(0.0); common::math::Vec2d obstacle_end_xy; @@ -295,22 +359,30 @@ const Obstacle *Frame::CreateStaticObstacle( double left_lane_width = 0.0; double right_lane_width = 0.0; + //获取障碍物起始点处对应的车道中心线左右宽度 if (!reference_line.GetLaneWidth(obstacle_start_s, &left_lane_width, &right_lane_width)) { AERROR << "Failed to get lane width at s[" << obstacle_start_s << "]"; return nullptr; } - + //根据障碍物起始点，终点的xy坐标构成的二维线段类对象 + //以及车道在虚拟障碍物起始点处的总宽度去构建 + //构建障碍物的二维障碍物边界盒对象obstacle_box common::math::Box2d obstacle_box{ common::math::LineSegment2d(obstacle_start_xy, obstacle_end_xy), left_lane_width + right_lane_width}; - + //根据障碍物id和障碍物对应的边界盒对象作为参数去调用构建虚拟障碍物的函数， + //返回一个虚拟障碍物对象类 return CreateStaticVirtualObstacle(obstacle_id, obstacle_box); } +//这个函数还是创建虚拟障碍物对象，返回的类型为Obstacle 类 +//输入参数障碍物id，以及障碍物对应的二维边界盒box const Obstacle *Frame::CreateStaticVirtualObstacle(const std::string &id, const Box2d &box) { + //在障碍物列表中寻找输入参数id对应的障碍物对象 const auto *object = obstacles_.Find(id); + //如果object不为空，说明找到了对应id的障碍物，障碍物还没创建就已经存在，报警告并返回 if (object) { AWARN << "obstacle " << id << " already exist."; return object; @@ -322,7 +394,13 @@ const Obstacle *Frame::CreateStaticVirtualObstacle(const std::string &id, } return ptr; } - +/// @brief 初始化一帧数据的函数，用来准备轨迹规划所需的信息 +/// @param vehicle_state_provider +/// @param reference_lines +/// @param segments +/// @param future_route_waypoints +/// @param ego_info +/// @return Status Frame::Init( const common::VehicleStateProvider *vehicle_state_provider, const std::list &reference_lines, @@ -330,11 +408,13 @@ Status Frame::Init( const std::vector &future_route_waypoints, const EgoInfo *ego_info) { // TODO(QiL): refactor this to avoid redundant nullptr checks in scenarios. + // 初始化基本数据，比如车辆状态、Ego 车辆信息等 auto status = InitFrameData(vehicle_state_provider, ego_info); if (!status.ok()) { AERROR << "failed to init frame:" << status.ToString(); return status; } + // 将参考线和路径段匹配成参考线信息 if (!CreateReferenceLineInfo(reference_lines, segments)) { const std::string msg = "Failed to init reference line info."; AERROR << msg; diff --git a/modules/planning/planning_base/common/frame.h b/modules/planning/planning_base/common/frame.h index a91a7e15204..2c4eb904656 100644 --- a/modules/planning/planning_base/common/frame.h +++ b/modules/planning/planning_base/common/frame.h @@ -56,13 +56,20 @@ namespace planning { /** * @class Frame * - * @brief Frame holds all data for one planning cycle. + * @brief Frame holds all data for one planning cycle. Frame类储存了一个规划周期的所有数据 */ class Frame { public: + /// @brief + /// @param sequence_num 序列号 explicit Frame(uint32_t sequence_num); - + /// @brief + /// @param sequence_num + /// @param local_view LocalView类包含了规划模块输入所需的所有数据 + /// @param planning_start_point 规划轨迹起始点 + /// @param vehicle_state 车辆状态 + /// @param reference_line_provider 参考线提供器 Frame(uint32_t sequence_num, const LocalView &local_view, const common::TrajectoryPoint &planning_start_point, const common::VehicleState &vehicle_state, @@ -71,11 +78,17 @@ class Frame { Frame(uint32_t sequence_num, const LocalView &local_view, const common::TrajectoryPoint &planning_start_point, const common::VehicleState &vehicle_state); - + //析构函数 virtual ~Frame() = default; - + //获取Frame类的数据成员规划起始点planning_start_point_ const common::TrajectoryPoint &PlanningStartPoint() const; - + /// @brief 用输入参数去初始化Frame类的数据成员 + /// @param vehicle_state_provider 车辆状态提供器类指针对象 + /// @param reference_lines 参考线类对象列表 + /// @param segments 导航路径段类对象列表 + /// @param future_route_waypoints 将来的导航的车道航点序列 + /// @param ego_info 自车信息类指针对象 + /// @return common::Status Init( const common::VehicleStateProvider *vehicle_state_provider, const std::list &reference_lines, @@ -90,30 +103,47 @@ class Frame { uint32_t SequenceNum() const; std::string DebugString() const; - + // 返回待发布的规划轨迹 const PublishableTrajectory &ComputedTrajectory() const; - + + //其实这个函数就记录planning的所有输入数据用于debug + //这些输入数据几乎都是从Frame类成员local_view_里拷贝的 + //planning_internal::Debug 是modules\planning\proto\planning_internal.proto + //里定义的message类，Debug下面包含planning_data + //planning_data里又包含规划的相关数据，如自车位置，底盘反馈，routing响应， + //起始点，路径，速度规划，st图，sl坐标，障碍物，信号灯，前方畅通距离，场景信息等等，几 + //乎所有的planning输入debug数据 void RecordInputDebug(planning_internal::Debug *debug); - + //获取Frame类成员参考线信息类对象reference_line_info_，它是一个参考线信息的列表list const std::list &reference_line_info() const; + //返回Frame类成员参考线信息类对象reference_line_info_的地址 std::list *mutable_reference_line_info(); - + //在障碍物列表中寻找输入参数id对应的障碍物对象 Obstacle *Find(const std::string &id); - + //找到Frame类数据成员reference_line_info_道路参考线列表中cost最小且可行驶的道路参考线对象 const ReferenceLineInfo *FindDriveReferenceLineInfo(); - + //找目标道路参考线，返回的是道路参考线信息ReferenceLineInfo类对象 const ReferenceLineInfo *FindTargetReferenceLineInfo(); - + //查找失败的参考线信息，返回道路参考线信息类ReferenceLineInfo类对象 + //遍历道路参考线列表，找到不可驾驶的变道参考线信息类对象 const ReferenceLineInfo *FindFailedReferenceLineInfo(); - + //返回Frame类的数据成员驾驶参考线信息类对象 const ReferenceLineInfo *DriveReferenceLineInfo() const; - + //返回Frame类的数据成员障碍物列表obstacles_ const std::vector obstacles() const; - + /// @brief 对虚拟静态障碍物的位置建立虚拟障碍物对象，如红绿灯停止线等 + /// @param reference_line_info 参考线信息类对象reference_line_info，这里不是列表了，只是一条参考线信息 + /// @param obstacle_id 障碍物obstacle_id,引用变量&应该是用来存放构建的虚拟障碍物的Id + /// @param obstacle_s 障碍物对应的frenet系的s坐标 + /// @return const Obstacle *CreateStopObstacle( ReferenceLineInfo *const reference_line_info, const std::string &obstacle_id, const double obstacle_s); - + /// @brief + /// @param obstacle_id + /// @param lane_id + /// @param lane_s 车道对应的终点的frenet系s坐标lane_s + /// @return const Obstacle *CreateStopObstacle(const std::string &obstacle_id, const std::string &lane_id, const double lane_s); @@ -126,11 +156,13 @@ class Frame { bool Rerouting(PlanningContext *planning_context); const common::VehicleState &vehicle_state() const; - + /// @brief 实现预测时间对齐planning起始时间 + /// @param planning_start_time + /// @param prediction_obstacles static void AlignPredictionTime( const double planning_start_time, prediction::PredictionObstacles *prediction_obstacles); - + //设置当前frame帧的规划轨迹，用ADCTrajectory类轨迹对象 void set_current_frame_planned_trajectory( ADCTrajectory current_frame_planned_trajectory) { current_frame_planned_trajectory_ = @@ -140,7 +172,7 @@ class Frame { const ADCTrajectory ¤t_frame_planned_trajectory() const { return current_frame_planned_trajectory_; } - + //设置当前frame的离散路径 轨迹trajectory=路径path + 速度规划 void set_current_frame_planned_path( DiscretizedPath current_frame_planned_path) { current_frame_planned_path_ = std::move(current_frame_planned_path); @@ -149,7 +181,7 @@ class Frame { const DiscretizedPath ¤t_frame_planned_path() const { return current_frame_planned_path_; } - + //离目标点是否足够近 const bool is_near_destination() const { return is_near_destination_; } /** @@ -158,26 +190,36 @@ class Frame { */ void UpdateReferenceLinePriority( const std::map &id_to_priority); - + //返回frame类的数据成员local_view_对象，其包含了所有的规划输入数据，是一个struct + //障碍物/底盘/交通灯/定位等信息 const LocalView &local_view() const { return local_view_; } - + //获取Frame类数据成员障碍物列表 ThreadSafeIndexedObstacles *GetObstacleList() { return &obstacles_; } - + //返回开放空间信息类对象 const OpenSpaceInfo &open_space_info() const { return open_space_info_; } OpenSpaceInfo *mutable_open_space_info() { return &open_space_info_; } - + //根据id获取交通灯对象，输入的是交通灯的id perception::TrafficLight GetSignal(const std::string &traffic_light_id) const; - + //获取frame类的数据成员存放人机交互动作对象 const PadMessage::DrivingAction &GetPadMsgDrivingAction() const { return pad_msg_driving_action_; } private: + /// @brief + /// @param vehicle_state_provider 车辆状态提供器类指针对象 + /// @param ego_info 自车信息类指针对象 + /// @return common::Status InitFrameData( const common::VehicleStateProvider *vehicle_state_provider, const EgoInfo *ego_info); - + /// @brief + /// @param reference_lines 参考线类对象列表reference_lines + /// @param segments 导航路径段类对象列表segments 导航route指全局路径规划 + /// @return + //其实这个函数就是将参数里的参考线列表reference_lines，导航路径段RouteSegments列表设 +//置到Frame类数据成员reference_line_info_参考线信息类对象列表里 bool CreateReferenceLineInfo(const std::list &reference_lines, const std::list &segments); @@ -192,47 +234,62 @@ class Frame { /** * @brief create a static virtual obstacle */ + //根据障碍物id和障碍物对应的边界盒对象作为参数去调用构建虚拟障碍物的函数， + //返回一个虚拟障碍物对象类 const Obstacle *CreateStaticVirtualObstacle(const std::string &id, const common::math::Box2d &box); - + //增加一个障碍物对象到类数据成员障碍物列表里 void AddObstacle(const Obstacle &obstacle); - + //读交通灯函数，从数据成员local_view_读到Frame类数据成员traffic_lights_交通灯列表中 +//local_view_包含规划的所有输入数据 void ReadTrafficLights(); - + //读取local_view_中的驾驶员交互操作行为 void ReadPadMsgDrivingAction(); + //复位人机交互动作对象 void ResetPadMsgDrivingAction(); private: + //人机交互动作，自驾模式选择等... static PadMessage::DrivingAction pad_msg_driving_action_; + // 序列号 uint32_t sequence_num_ = 0; + //LocalView类对象，包含规划所有输入数据 LocalView local_view_; + //高精度地图对象，初始化为空指针 const hdmap::HDMap *hdmap_ = nullptr; + //规划起始点 common::TrajectoryPoint planning_start_point_; + //车辆状态 common::VehicleState vehicle_state_; + //参考线列表 std::list reference_line_info_; - + //是否接近目标点？ bool is_near_destination_ = false; /** * the reference line info that the vehicle finally choose to drive on **/ + // 车辆最终选择驾驶的道路参考线 const ReferenceLineInfo *drive_reference_line_info_ = nullptr; - + //障碍物列表 ThreadSafeIndexedObstacles obstacles_; - + //无序map，存放id,和交通灯对象的映射关系 std::unordered_map traffic_lights_; // current frame published trajectory + // 当前帧的规划发布轨迹，也就是输出结果 ADCTrajectory current_frame_planned_trajectory_; // current frame path for future possible speed fallback + //当前帧的路径点，为了将来可能的speed fallback速度规划失败？ DiscretizedPath current_frame_planned_path_; - + //参考线提供器类对象 const ReferenceLineProvider *reference_line_provider_ = nullptr; - + //开放空间信息类对象？非结构化道路？ OpenSpaceInfo open_space_info_; - +//将来的路由点vector，用于rerouting时将自车在轨迹上最近点与之拼接 +//用于将之前设定的routing请求改为从自车位置出发 std::vector future_route_waypoints_; common::monitor::MonitorLogBuffer monitor_logger_buffer_; diff --git a/modules/planning/planning_base/common/local_view.h b/modules/planning/planning_base/common/local_view.h index 9adc1de6a03..1fa07f2bdf3 100644 --- a/modules/planning/planning_base/common/local_view.h +++ b/modules/planning/planning_base/common/local_view.h @@ -16,16 +16,27 @@ #pragma once +// C++ 标准库，提供了 智能指针（如 std::shared_ptr） 的支持，能自动管理内存，防止 内存泄漏 #include +// 以下都是proto生成的 +// 车辆底盘状态（如速度、档位等） #include "modules/common_msgs/chassis_msgs/chassis.pb.h" +// 车辆的定位信息（如经纬度、方向等） #include "modules/common_msgs/localization_msgs/localization.pb.h" +// 交通信号灯检测结果 #include "modules/common_msgs/perception_msgs/traffic_light_detection.pb.h" +// 导航相关信息 #include "modules/common_msgs/planning_msgs/navigation.pb.h" +// 用户的驾驶模式选择（如手动/自动驾驶） #include "modules/common_msgs/planning_msgs/pad_msg.pb.h" +// 规划模块下达的指令 #include "modules/common_msgs/planning_msgs/planning_command.pb.h" +// 预测的障碍物信息（如行人、车辆等动态目标） #include "modules/common_msgs/prediction_msgs/prediction_obstacle.pb.h" +// 路径规划的路由信息 #include "modules/common_msgs/routing_msgs/routing.pb.h" +// 叙事模块的内容 #include "modules/common_msgs/storytelling_msgs/story.pb.h" namespace apollo { @@ -33,18 +44,29 @@ namespace planning { /** * @struct local_view + // 存储规划所需的 所有输入数据 * @brief LocalView contains all necessary data as planning input */ +// LocalView 存储路径规划所需的传感器数据、地图信息和控制指令 struct LocalView { + // 预测模块输出的 障碍物运动轨迹 std::shared_ptr prediction_obstacles; + // 车辆底盘信息，如 速度、加速度、档位 等 std::shared_ptr chassis; + // 车辆的 位置信息（经纬度、方向等） std::shared_ptr localization_estimate; + // 交通灯识别结果 std::shared_ptr traffic_light; + // 相对地图信息 std::shared_ptr relative_map; + // 用户的驾驶模式选择（如 自动驾驶/手动驾驶） std::shared_ptr pad_msg; + // 叙事模块的状态 std::shared_ptr stories; + // 规划模块下达的 指令 std::shared_ptr planning_command; + // 终点车道的 路径信息 std::shared_ptr end_lane_way_point; }; diff --git a/modules/planning/planning_base/common/message_process.cc b/modules/planning/planning_base/common/message_process.cc index aab71172fc9..ac8aec5c8c7 100644 --- a/modules/planning/planning_base/common/message_process.cc +++ b/modules/planning/planning_base/common/message_process.cc @@ -58,8 +58,11 @@ using apollo::prediction::PredictionObstacles; using apollo::routing::RoutingResponse; using apollo::storytelling::CloseToJunction; using apollo::storytelling::Stories; - +/// @brief +/// @param planning_config 规划配置 +/// @return bool MessageProcess::Init(const PlanningConfig& planning_config) { + // 将 planning_config 的内容复制到 planning_config_ 中，确保 MessageProcess 持有最新的规划配置 planning_config_.CopyFrom(planning_config); map_m_["Sunnyvale"] = "sunnyvale"; @@ -68,23 +71,39 @@ bool MessageProcess::Init(const PlanningConfig& planning_config) { map_m_["Gomentum"] = "gomentum"; map_m_["Sunnyvale Loop"] = "sunnyvale_loop"; map_m_["San Mateo"] = "san_mateo"; + /* + FLAGS_map_dir = "/home/user/maps/sunnyvale"; + map_name_ = FLAGS_map_dir.substr(FLAGS_map_dir.find_last_of("/") + 1); + 结果：map_name_ = "sunnyvale" + */ + + // find_last_of("/") 找到最后一个 / 的位置，然后 substr() 获取路径的最后一部分，即当前地图的名称 + // 提取地图名称 map_name_ = FLAGS_map_dir.substr(FLAGS_map_dir.find_last_of("/") + 1); obstacle_history_map_.clear(); if (FLAGS_planning_offline_learning) { + // 以 追加模式 (std::ios_base::app) 打开 "learning_data.log" 日志文件，确保新日志不会覆盖旧日志 // offline process logging + // std::ofstream log_file_; log_file_.open(FLAGS_planning_data_dir + "/learning_data.log", std::ios_base::out | std::ios_base::app); + // 记录当前时间 start_time_，可能用于计算 日志记录的时间间隔 或 启动时间 start_time_ = std::chrono::system_clock::now(); + // std::time(nullptr) 获取当前时间（UTC 时间） std::time_t now = std::time(nullptr); + // std::asctime() 将时间转换为可读的字符串格式 log_file_ << "UTC date and time: " << std::asctime(std::gmtime(&now)) << "Local date and time: " << std::asctime(std::localtime(&now)); } return true; } - +/// @brief +/// @param planning_config PlanningConfig 类型的常量引用，表示规划配置参数 +/// @param injector 共享指针，指向 DependencyInjector 类的实例，通常用于依赖注入，提供其他对象的创建和管理 +/// @return bool MessageProcess::Init(const PlanningConfig& planning_config, const std::shared_ptr& injector) { injector_ = injector; diff --git a/modules/planning/planning_base/common/obstacle.cc b/modules/planning/planning_base/common/obstacle.cc index f5378bab317..a914d1d1fe8 100644 --- a/modules/planning/planning_base/common/obstacle.cc +++ b/modules/planning/planning_base/common/obstacle.cc @@ -193,11 +193,16 @@ bool Obstacle::IsValidPerceptionObstacle(const PerceptionObstacle& obstacle) { } return true; } - +/// @brief 将预测结果转换为规划模块障碍物表示的函数 +/// @param predictions +/// @return std::list> Obstacle::CreateObstacles( const prediction::PredictionObstacles& predictions) { +// 初始化空列表 std::list> obstacles; +// 遍历每一个预测障碍物 for (const auto& prediction_obstacle : predictions.prediction_obstacle()) { +// 检查感知障碍物合法性 if (!IsValidPerceptionObstacle(prediction_obstacle.perception_obstacle())) { AERROR << "Invalid perception obstacle: " << prediction_obstacle.perception_obstacle().DebugString(); @@ -205,6 +210,7 @@ std::list> Obstacle::CreateObstacles( } const auto perception_id = std::to_string(prediction_obstacle.perception_obstacle().id()); +// 如果没有预测轨迹（static） if (prediction_obstacle.trajectory().empty()) { obstacles.emplace_back( new Obstacle(perception_id, prediction_obstacle.perception_obstacle(), @@ -214,8 +220,10 @@ std::list> Obstacle::CreateObstacles( } int trajectory_index = 0; + // 有预测轨迹的情况 for (const auto& trajectory : prediction_obstacle.trajectory()) { bool is_valid_trajectory = true; + // 验证每个 trajectory 的点是否合法 for (const auto& point : trajectory.trajectory_point()) { if (!IsValidTrajectoryPoint(point)) { AERROR << "obj:" << perception_id @@ -228,9 +236,10 @@ std::list> Obstacle::CreateObstacles( if (!is_valid_trajectory) { continue; } - + // 用原始感知 id 加上轨迹编号来唯一标识这个障碍物轨迹 const std::string obstacle_id = absl::StrCat(perception_id, "_", trajectory_index); + // 创建新的 Obstacle 实例并加入返回列表 obstacles.emplace_back( new Obstacle(obstacle_id, prediction_obstacle.perception_obstacle(), trajectory, prediction_obstacle.priority().priority(), @@ -290,16 +299,23 @@ void Obstacle::SetPerceptionSlBoundary(const SLBoundary& sl_boundary) { sl_boundary_ = sl_boundary; } +/// @brief 根据当前障碍物的位置和车辆参数计算车辆在最小转弯半径下与障碍物的最小安全停车距离 +/// @param vehicle_param +/// @return double Obstacle::MinRadiusStopDistance( const common::VehicleParam& vehicle_param) const { if (min_radius_stop_distance_ > 0) { return min_radius_stop_distance_; } + // 定义一个静态常量缓冲区（单位：米），用于增加安全冗余，默认值为 0.5 米 static constexpr double stop_distance_buffer = 0.5; + // 从车辆配置中获取车辆的最小安全转弯半径，这是车辆在低速极限情况下能保持的最小转弯曲率 const double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius(); + // lateral_diff 表示车辆绕障碍物打最小方向盘时的横向最小绕行距离 double lateral_diff = vehicle_param.width() / 2.0 + std::max(std::fabs(sl_boundary_.start_l()), std::fabs(sl_boundary_.end_l())); + // 对 lateral_diff 做限制，确保不会超过最小转弯半径 const double kEpison = 1e-5; lateral_diff = std::min(lateral_diff, min_turn_radius - kEpison); double stop_distance = @@ -308,6 +324,7 @@ double Obstacle::MinRadiusStopDistance( (min_turn_radius - lateral_diff))) + stop_distance_buffer; stop_distance -= vehicle_param.front_edge_to_center(); + // 最后stop_distance： 把停车点设置为车辆前缘刚好到达该停止点 stop_distance = std::min(stop_distance, FLAGS_max_stop_distance_obstacle); stop_distance = std::max(stop_distance, FLAGS_min_stop_distance_obstacle); return stop_distance; @@ -541,16 +558,19 @@ bool Obstacle::IsLongitudinalDecision(const ObjectDecisionType& decision) { ObjectDecisionType Obstacle::MergeLongitudinalDecision( const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) { + // 1. 空决策处理（同横向） if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) { return rhs; } if (rhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) { return lhs; } + // 2. 安全优先级排序（通过 s_longitudinal_decision_safety_sorter_） const auto lhs_val = FindOrDie(s_longitudinal_decision_safety_sorter_, lhs.object_tag_case()); const auto rhs_val = FindOrDie(s_longitudinal_decision_safety_sorter_, rhs.object_tag_case()); + // 3. 同类型决策的细化合并规则（重点！） if (lhs_val < rhs_val) { return rhs; } else if (lhs_val > rhs_val) { @@ -596,21 +616,26 @@ bool Obstacle::IsLateralIgnore() const { ObjectDecisionType Obstacle::MergeLateralDecision( const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) { + // 1. 处理“空决策”（默认值） if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) { return rhs; } if (rhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) { return lhs; } + // 2. 安全优先级排序（核心机制） const auto lhs_val = FindOrDie(s_lateral_decision_safety_sorter_, lhs.object_tag_case()); const auto rhs_val = FindOrDie(s_lateral_decision_safety_sorter_, rhs.object_tag_case()); + // 数值越大，决策越“强”或越“主动” if (lhs_val < rhs_val) { return rhs; } else if (lhs_val > rhs_val) { return lhs; } else { + // 3. 同类型决策的细化合并 + // 情况1：都是 ignore if (lhs.has_ignore()) { return rhs; } else if (lhs.has_nudge()) { @@ -770,16 +795,22 @@ void Obstacle::SetLaneChangeBlocking(const bool is_distance_clear) { // input: obstacle trajectory point // ouput: obstacle polygon +/// @brief +/// @param point 障碍物路径点 +/// @return common::math::Polygon2d Obstacle::GetObstacleTrajectoryPolygon( const common::TrajectoryPoint& point) const { + // 从当前障碍物的姿态到目标路径点的姿态差异（旋转角度差） double delta_heading = point.path_point().theta() - perception_obstacle_.theta(); double cos_delta_heading = cos(delta_heading); double sin_delta_heading = sin(delta_heading); + // 存储障碍物轨迹多边形顶点 std::vector polygon_point; polygon_point.reserve(perception_polygon_.points().size()); - + // perception_polygon_.points() 获取障碍物的多边形顶点 for (auto& iter : perception_polygon_.points()) { + // 计算当前顶点相对于障碍物中心位置的偏移量 double relative_x = iter.x() - perception_obstacle_.position().x(); double relative_y = iter.y() - perception_obstacle_.position().y(); double x = relative_x * cos_delta_heading - relative_y * sin_delta_heading + diff --git a/modules/planning/planning_base/common/obstacle_blocking_analyzer.cc b/modules/planning/planning_base/common/obstacle_blocking_analyzer.cc index 17b13057988..9cebdfa6194 100644 --- a/modules/planning/planning_base/common/obstacle_blocking_analyzer.cc +++ b/modules/planning/planning_base/common/obstacle_blocking_analyzer.cc @@ -34,11 +34,16 @@ using apollo::hdmap::HDMapUtil; constexpr double kAdcDistanceThreshold = 35.0; // unit: m constexpr double kObstaclesDistanceThreshold = 15.0; - +/// @brief 判断一个障碍物（obstacle）是否为不可移动的障碍物 +/// @param reference_line_info 包含参考线信息的对象 +/// @param obstacle 当前障碍物的对象 +/// @return bool IsNonmovableObstacle(const ReferenceLineInfo& reference_line_info, const Obstacle& obstacle) { // Obstacle is far away. + // 自动驾驶车辆的当前边界，用于后续判断障碍物的位置 const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary(); + // 如果障碍物的起始位置（start_s()）距离参考线边界的结束位置（end_s()）加上一个阈值（kAdcDistanceThreshold）还远，那么认为该障碍物太远，系统无法确定其状态，返回 false if (obstacle.PerceptionSLBoundary().start_s() > adc_sl_boundary.end_s() + kAdcDistanceThreshold) { ADEBUG << " - It is too far ahead and we are not so sure of its status."; @@ -52,32 +57,41 @@ bool IsNonmovableObstacle(const ReferenceLineInfo& reference_line_info, } // Obstacle is blocked by others too. + // 遍历参考线信息中所有的障碍物，检查是否有其他障碍物可能阻挡当前障碍物 for (const auto* other_obstacle : reference_line_info.path_decision().obstacles().Items()) { + // 如果当前障碍物是正在检查的障碍物自身，跳过此次循环 if (other_obstacle->Id() == obstacle.Id()) { continue; } + // 如果其他障碍物是虚拟障碍物（如由算法模拟出来的障碍物），则跳过此次循环 if (other_obstacle->IsVirtual()) { continue; } + // 如果其他障碍物不是车辆类型的障碍物，则跳过此次循环 if (other_obstacle->Perception().type() != apollo::perception::PerceptionObstacle::VEHICLE) { continue; } + // 获取当前检查的障碍物和其他障碍物的感知边界 const auto& other_boundary = other_obstacle->PerceptionSLBoundary(); const auto& this_boundary = obstacle.PerceptionSLBoundary(); + // 如果其他障碍物的左右边界（start_l() 和 end_l()）没有与当前障碍物的边界重叠，即它们不在相邻的车道内，则认为它们不构成阻挡，跳过此次循环 if (other_boundary.start_l() > this_boundary.end_l() || other_boundary.end_l() < this_boundary.start_l()) { // not blocking the backside vehicle continue; } + // 计算当前障碍物与其他障碍物的纵向距离（delta_s）。如果这个距离小于零或大于一个预设的阈值（kObstaclesDistanceThreshold），则认为其他障碍物不构成阻挡，跳过此次循环 double delta_s = other_boundary.start_s() - this_boundary.end_s(); if (delta_s < 0.0 || delta_s > kObstaclesDistanceThreshold) { continue; } + // 如果以上条件都满足，说明当前障碍物被其他障碍物阻挡 return false; } ADEBUG << "IT IS NON-MOVABLE!"; + // 如果没有其他障碍物阻挡，且障碍物满足不可移动条件，输出日志并返回 true，表示当前障碍物不可移动 return true; } diff --git a/modules/planning/planning_base/common/path/discretized_path.cc b/modules/planning/planning_base/common/path/discretized_path.cc index e153dff0eed..e766fa48ee8 100644 --- a/modules/planning/planning_base/common/path/discretized_path.cc +++ b/modules/planning/planning_base/common/path/discretized_path.cc @@ -42,6 +42,7 @@ double DiscretizedPath::Length() const { PathPoint DiscretizedPath::Evaluate(const double path_s) const { ACHECK(!empty()); + // 返回第一个路径点 p，满足 p.s >= path_s auto it_lower = QueryLowerBound(path_s); if (it_lower == begin()) { return front(); diff --git a/modules/planning/planning_base/common/path_decision.cc b/modules/planning/planning_base/common/path_decision.cc index 105553ae923..373da51b6c4 100644 --- a/modules/planning/planning_base/common/path_decision.cc +++ b/modules/planning/planning_base/common/path_decision.cc @@ -64,7 +64,11 @@ void PathDecision::SetSTBoundary(const std::string &id, obstacle->set_path_st_boundary(boundary); } } - +/// @brief +/// @param tag 决策来源标识（如 "reference_line_filter"、"path_bounds_decider"），用于调试和追踪 +/// @param object_id 障碍物唯一 ID（来自感知/预测模块） +/// @param decision 具体的横向决策类型（protobuf 定义，可为 ignore、nudge、side_pass 等） +/// @return bool PathDecision::AddLateralDecision(const std::string &tag, const std::string &object_id, const ObjectDecisionType &decision) { diff --git a/modules/planning/planning_base/common/reference_line_info.cc b/modules/planning/planning_base/common/reference_line_info.cc index a80883d5680..27afa6fb41b 100644 --- a/modules/planning/planning_base/common/reference_line_info.cc +++ b/modules/planning/planning_base/common/reference_line_info.cc @@ -380,7 +380,7 @@ Obstacle* ReferenceLineInfo::AddObstacle(const Obstacle* obstacle) { AERROR << "failed to add obstacle " << obstacle->Id(); return nullptr; } - + // 使用障碍物的感知多边形（通常是凸包）投影到参考线，得到其在 SL 空间的占据范围 SLBoundary perception_sl; if (!reference_line_.GetSLBoundary(obstacle->PerceptionPolygon(), &perception_sl)) { @@ -388,13 +388,15 @@ Obstacle* ReferenceLineInfo::AddObstacle(const Obstacle* obstacle) { return mutable_obstacle; } mutable_obstacle->SetPerceptionSlBoundary(perception_sl); + // 判断障碍物是否物理上占据本车道（基于 SL 边界与车道边界的关系） mutable_obstacle->CheckLaneBlocking(reference_line_); if (mutable_obstacle->IsLaneBlocking()) { ADEBUG << "obstacle [" << obstacle->Id() << "] is lane blocking."; } else { ADEBUG << "obstacle [" << obstacle->Id() << "] is NOT lane blocking."; } - + + // 判断是否为“无关障碍物” if (IsIrrelevantObstacle(*mutable_obstacle)) { ObjectDecisionType ignore; ignore.mutable_ignore(); @@ -405,6 +407,7 @@ Obstacle* ReferenceLineInfo::AddObstacle(const Obstacle* obstacle) { ADEBUG << "NO build reference line st boundary. id:" << obstacle->Id(); } else { ADEBUG << "build reference line st boundary. id:" << obstacle->Id(); + // 构建 ST 边界（用于速度规划） mutable_obstacle->BuildReferenceLineStBoundary(reference_line_, adc_sl_boundary_.start_s()); @@ -444,14 +447,18 @@ bool ReferenceLineInfo::AddObstacles( } bool ReferenceLineInfo::IsIrrelevantObstacle(const Obstacle& obstacle) { + // 1.谨慎障碍物（Caution Level）永不忽略 if (obstacle.IsCautionLevelObstacle()) { return false; } + + // 2. 障碍物超出参考线长度 → 忽略 // if adc is on the road, and obstacle behind adc, ignore const auto& obstacle_boundary = obstacle.PerceptionSLBoundary(); if (obstacle_boundary.end_s() > reference_line_.Length()) { return true; } + // 3. 主要忽略逻辑：后方远处 + 在车道上 + 非变道 if (is_on_reference_line_ && !IsChangeLanePath() && adc_sl_boundary_.end_s() - obstacle_boundary.end_s() > FLAGS_obstacle_lon_ignore_buffer && @@ -513,9 +520,9 @@ bool ReferenceLineInfo::CombinePathAndSpeedProfile( ACHECK(ptr_discretized_trajectory != nullptr); // use varied resolution to reduce data load but also provide enough data // point for control module - const double kDenseTimeResoltuion = FLAGS_trajectory_time_min_interval; - const double kSparseTimeResolution = FLAGS_trajectory_time_max_interval; - const double kDenseTimeSec = FLAGS_trajectory_time_high_density_period; + const double kDenseTimeResoltuion = FLAGS_trajectory_time_min_interval; // 0.02 + const double kSparseTimeResolution = FLAGS_trajectory_time_max_interval; // 0.1 + const double kDenseTimeSec = FLAGS_trajectory_time_high_density_period; // 1.0 if (path_data_.discretized_path().empty()) { AERROR << "path data is empty"; @@ -526,19 +533,21 @@ bool ReferenceLineInfo::CombinePathAndSpeedProfile( AERROR << "speed profile is empty"; return false; } - + // 以时间遍历 speed profile for (double cur_rel_time = 0.0; cur_rel_time < speed_data_.TotalTime(); cur_rel_time += (cur_rel_time < kDenseTimeSec ? kDenseTimeResoltuion : kSparseTimeResolution)) { + // 1 根据时间找 speed_point common::SpeedPoint speed_point; if (!speed_data_.EvaluateByTime(cur_rel_time, &speed_point)) { AERROR << "Fail to get speed point with relative time " << cur_rel_time; return false; } - + // 2 s 超出 path 长度就停止 if (speed_point.s() > path_data_.discretized_path().Length()) { break; } + // 3 根据 s 找对应的 PathPoint common::PathPoint path_point = path_data_.GetPathPointWithPathS(speed_point.s()); path_point.set_s(path_point.s() + start_s); diff --git a/modules/planning/planning_base/common/speed/speed_data.cc b/modules/planning/planning_base/common/speed/speed_data.cc index 24ad55a7d15..8b28d122abe 100644 --- a/modules/planning/planning_base/common/speed/speed_data.cc +++ b/modules/planning/planning_base/common/speed/speed_data.cc @@ -59,13 +59,15 @@ void SpeedData::AppendSpeedPoint(const double s, const double time, bool SpeedData::EvaluateByTime(const double t, common::SpeedPoint* const speed_point) const { + // 1.边界检查 if (size() < 2) { return false; } if (!(front().t() < t + 1.0e-6 && t - 1.0e-6 < back().t())) { return false; } - + + // 2.使用 二分查找 (lower_bound) 找到第一个 sp.t() >= t 的点 auto comp = [](const common::SpeedPoint& sp, const double t) { return sp.t() < t; }; diff --git a/modules/planning/planning_base/common/speed/st_boundary.cc b/modules/planning/planning_base/common/speed/st_boundary.cc index f34a40c5e4c..520ab9163e6 100644 --- a/modules/planning/planning_base/common/speed/st_boundary.cc +++ b/modules/planning/planning_base/common/speed/st_boundary.cc @@ -68,7 +68,10 @@ STBoundary::STBoundary( obstacle_road_right_ending_t_ = std::numeric_limits::lowest(); } - +/// @brief +/// @param lower_points +/// @param upper_points +/// @return STBoundary STBoundary::CreateInstance( const std::vector& lower_points, const std::vector& upper_points) { @@ -259,7 +262,9 @@ bool STBoundary::IsPointInBoundary(const STPoint& st_point) const { return (check_upper * check_lower < 0); } - +/// @brief 根据输入的扩展值 s，调整 STBoundary 的上下边界点（lower_points_ 和 upper_points_），使得边界向左右各自扩展 s 单位，最终返回一个新的扩展后的 STBoundary 对象 +/// @param s +/// @return STBoundary STBoundary::ExpandByS(const double s) const { if (lower_points_.empty()) { return STBoundary(); diff --git a/modules/planning/planning_base/common/trajectory/discretized_trajectory.cc b/modules/planning/planning_base/common/trajectory/discretized_trajectory.cc index a81cf111563..74219464383 100644 --- a/modules/planning/planning_base/common/trajectory/discretized_trajectory.cc +++ b/modules/planning/planning_base/common/trajectory/discretized_trajectory.cc @@ -61,19 +61,30 @@ TrajectoryPoint DiscretizedTrajectory::Evaluate( return common::math::InterpolateUsingLinearApproximation( *(it_lower - 1), *it_lower, relative_time); } - +/// @brief +/// @param relative_time 表示查询的相对时间 +/// @param epsilon 表示容忍的误差，用于调整比较的精度 +/// @return size_t DiscretizedTrajectory::QueryLowerBoundPoint(const double relative_time, const double epsilon) const { + // 确保DiscretizedTrajectory对象不为空 ACHECK(!empty()); - + // 如果 relative_time 大于等于轨迹中的最后一个点的相对时间 (back().relative_time())，那么就直接返回最后一个轨迹点的索引 size() - 1 if (relative_time >= back().relative_time()) { + // 返回最后一个点的索引 return size() - 1; } + // 判断该轨迹点的相对时间加上epsilon是否小于查询的relative_time auto func = [&epsilon](const TrajectoryPoint& tp, const double relative_time) { return tp.relative_time() + epsilon < relative_time; }; + // 返回一个迭代器，指向第一个满足func条件的位置 + // std::lower_bound 是一个标准库算法，它在一个已排序的范围中查找第一个不小于目标值的元素。这里我们查找的是第一个相对时间大于或等于 relative_time 的轨迹点 auto it_lower = std::lower_bound(begin(), end(), relative_time, func); + // 返回从容器的开始位置到it_lower迭代器之间的距离。这个距离就是查询点relative_time的下界（lower bound）索引 + // 返回迭代器 it_lower 距离轨迹开头的元素的偏移量，也就是返回的索引值 + // 这个索引值对应于第一个相对时间不小于 relative_time 的轨迹点 return std::distance(begin(), it_lower); } @@ -93,21 +104,27 @@ size_t DiscretizedTrajectory::QueryNearestPoint( } return index_min; } - +/// @brief 给定的位置 position 查找距离该位置最近的轨迹点，并返回该点的索引 +/// @param position 二维向量（Vec2d），表示要查找的目标位置 +/// @param buffer 一个容忍误差，用来扩展查找范围，使得在距离目标位置较近的轨迹点也能被认为是最近的点 +/// @return size_t DiscretizedTrajectory::QueryNearestPointWithBuffer( const common::math::Vec2d& position, const double buffer) const { + // 初始化，保存最小的平方距离 double dist_sqr_min = std::numeric_limits::max(); + // 保存当前最小距离对应的轨迹点索引，初始为 0 size_t index_min = 0; for (size_t i = 0; i < size(); ++i) { const common::math::Vec2d curr_point(data()[i].path_point().x(), data()[i].path_point().y()); - +// 计算了当前轨迹点 curr_point 到目标位置 position 的平方距离 const double dist_sqr = curr_point.DistanceSquareTo(position); if (dist_sqr < dist_sqr_min + buffer) { dist_sqr_min = dist_sqr; index_min = i; } } + // 返回找到的最小距离点的索引 index_min return index_min; } @@ -118,10 +135,15 @@ void DiscretizedTrajectory::AppendTrajectoryPoint( } push_back(trajectory_point); } - +/// @brief 常量引用意味着该函数返回的对象不能被修改，并且通过引用返回的对象不会被复制 +// 从离散轨迹中获取某个特定点的函数 +/// @param index +/// @return const TrajectoryPoint& DiscretizedTrajectory::TrajectoryPointAt( const size_t index) const { + // 检查 index 是否小于 NumOfPoints() CHECK_LT(index, NumOfPoints()); + // 一个成员函数，它返回 DiscretizedTrajectory 类的一个数据结构（通常是一个数组或类似容器），其中存储了轨迹的所有点 return data()[index]; } diff --git a/modules/planning/planning_base/common/trajectory_stitcher.cc b/modules/planning/planning_base/common/trajectory_stitcher.cc index 72d060f7ca6..5f7ef69e563 100644 --- a/modules/planning/planning_base/common/trajectory_stitcher.cc +++ b/modules/planning/planning_base/common/trajectory_stitcher.cc @@ -39,10 +39,15 @@ using apollo::common::TrajectoryPoint; using apollo::common::VehicleModel; using apollo::common::VehicleState; using apollo::common::math::Vec2d; - +/// @brief 根据当前的车辆状态（vehicle_state）生成一个轨迹点（TrajectoryPoint） +/// @param planning_cycle_time 规划周期 100ms 10hz +/// @param vehicle_state +/// @return TrajectoryPoint TrajectoryStitcher::ComputeTrajectoryPointFromVehicleState( const double planning_cycle_time, const VehicleState& vehicle_state) { + // 保存生成的轨迹点 TrajectoryPoint point; + // 从路径起点累积的距离 point.mutable_path_point()->set_s(0.0); point.mutable_path_point()->set_x(vehicle_state.x()); point.mutable_path_point()->set_y(vehicle_state.y()); @@ -51,10 +56,14 @@ TrajectoryPoint TrajectoryStitcher::ComputeTrajectoryPointFromVehicleState( point.mutable_path_point()->set_kappa(vehicle_state.kappa()); point.set_v(vehicle_state.linear_velocity()); point.set_a(vehicle_state.linear_acceleration()); + // 表示此轨迹点在当前规划周期中的位置 point.set_relative_time(planning_cycle_time); return point; } - +/// @brief 根据车辆的当前状态（vehicle_state）和规划周期时间（planning_cycle_time），决定是使用当前车辆状态还是预测的车辆状态来生成新的轨迹点 +/// @param planning_cycle_time +/// @param vehicle_state +/// @return std::vector TrajectoryStitcher::ComputeReinitStitchingTrajectory( const double planning_cycle_time, const VehicleState& vehicle_state) { @@ -62,18 +71,23 @@ TrajectoryStitcher::ComputeReinitStitchingTrajectory( static constexpr double kEpsilon_v = 0.1; static constexpr double kEpsilon_a = 0.4; // TODO(Jinyun/Yu): adjust kEpsilon if corrected IMU acceleration provided + // 如果当前车速小于0.1，且加速度小于0.4，则直接当前车辆状态作为规划起点 + // 起步阶段，速度加速度很小，直接使用当前车辆状态作为规划起始点 if (std::abs(vehicle_state.linear_velocity()) < kEpsilon_v && std::abs(vehicle_state.linear_acceleration()) < kEpsilon_a) { reinit_point = ComputeTrajectoryPointFromVehicleState(planning_cycle_time, vehicle_state); } else { +// 预测车辆位置 +// 根据车辆运动学模型，往前推planning_cycle_time时间，预测下一规划周期的状态 VehicleState predicted_vehicle_state; predicted_vehicle_state = VehicleModel::Predict(planning_cycle_time, vehicle_state); +// 基于该预测状态生成轨迹点 reinit_point = ComputeTrajectoryPointFromVehicleState( planning_cycle_time, predicted_vehicle_state); } - +// 数返回值为size为1的数组，即该条拼接轨迹仅有一个轨迹点 return std::vector(1, reinit_point); } @@ -110,33 +124,47 @@ void TrajectoryStitcher::TransformLastPublishedTrajectory( p.mutable_path_point()->set_theta(theta_new); }); } - /* Planning from current vehicle state if: 1. the auto-driving mode is off (or) 2. we don't have the trajectory from last planning cycle (or) 3. the position deviation from actual and target is too high */ +/// @brief +/// @param vehicle_chassis 车辆底盘信息 +/// @param vehicle_state 车辆状态 +/// @param current_timestamp 当前时间戳 +/// @param planning_cycle_time 规划周期时间 100ms +/// @param preserved_points_num 保留的轨迹点数目 20 +/// @param replan_by_offset 是否根据偏移量重新规划 +/// @param prev_trajectory 先前的轨迹 +/// @param replan_reason 保存重新规划原因的字符串 +/// @return std::vector TrajectoryStitcher::ComputeStitchingTrajectory( const canbus::Chassis& vehicle_chassis, const VehicleState& vehicle_state, const double current_timestamp, const double planning_cycle_time, const size_t preserved_points_num, const bool replan_by_offset, const PublishableTrajectory* prev_trajectory, std::string* replan_reason) { + // 如果不使用轨迹拼接 if (!FLAGS_enable_trajectory_stitcher) { + // 重规划原因：通过配置文件关闭拼接 *replan_reason = "stitch is disabled by gflag."; + // 进入函数，主要利用自行车模型预测未来位置 return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } + // 如果没有上一帧轨迹， if (!prev_trajectory) { *replan_reason = "replan for no previous trajectory."; return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } - +// 如果当前驾驶模式不是自动驾驶 if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) { *replan_reason = "replan for manual mode."; return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } +// 验证先前的轨迹 size_t prev_trajectory_size = prev_trajectory->NumOfPoints(); - +// 如果先前轨迹的点数为零（即轨迹为空） if (prev_trajectory_size == 0) { ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time() << "] size is zero! Previous planning not exist or failed. Use " @@ -145,12 +173,23 @@ std::vector TrajectoryStitcher::ComputeStitchingTrajectory( return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } + // 第二种方法 +// 根据当前帧时间戳以及ADC实际位置信息，在上一帧轨迹寻找匹配点，将上一帧轨迹中匹配点向前看0.1s +// 所对应的轨迹点作为当前帧的规划起始状态点，待当前帧规划生成轨迹后，再和上一帧轨迹中所选择 +// 的规划起始点前一段距离的轨迹点进行拼接 + +// 3.计算时间匹配点：通过 veh_rel_time 查询先前轨迹中与当前时间最接近的轨迹点 +// 找到 当前时刻 在上一帧轨迹中的 理论执行位置（按时间推算） const double veh_rel_time = current_timestamp - prev_trajectory->header_time(); - + // 查询先前轨迹中时间最接近的点的索引 size_t time_matched_index = prev_trajectory->QueryLowerBoundPoint(veh_rel_time); +// 4. 检查时间与位置偏差 + // 如果当前时间比先前轨迹的起始时间还要早，则重新规划 + // 检查当前时间是否小于先前轨迹的起始时间 + // 情况1: 当前时间 < 轨迹起始时间（时钟回退 or 轨迹未来） if (time_matched_index == 0 && veh_rel_time < prev_trajectory->StartPoint().relative_time()) { AWARN << "current time smaller than the previous trajectory's first time"; @@ -159,29 +198,36 @@ std::vector TrajectoryStitcher::ComputeStitchingTrajectory( "time."; return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } + // 如果当前时间超出了先前轨迹的时间范围，则重新规划 + // 情况2: 当前时间 > 轨迹结束时间（轨迹已过期） if (time_matched_index + 1 >= prev_trajectory_size) { AWARN << "current time beyond the previous trajectory's last time"; *replan_reason = "replan for current time beyond the previous trajectory's last time"; return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } - + // 获取时间匹配的轨迹点 auto time_matched_point = prev_trajectory->TrajectoryPointAt( static_cast(time_matched_index)); +// 5. 检查路径点 +// 如果与当前时间匹配的轨迹点没有路径信息（has_path_point()），则重新规划 if (!time_matched_point.has_path_point()) { *replan_reason = "replan for previous trajectory missed path point"; return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } +// 6.检查位置匹配点 +// 查询与车辆当前位置最接近的轨迹点索引 size_t position_matched_index = prev_trajectory->QueryNearestPointWithBuffer( {vehicle_state.x(), vehicle_state.y()}, 1.0e-6); +// 计算车辆当前位置在轨迹坐标系中的位置投影（Frenet坐标系） auto frenet_sd = ComputePositionProjection( vehicle_state.x(), vehicle_state.y(), prev_trajectory->TrajectoryPointAt( static_cast(position_matched_index))); - +// 如果启用了基于偏移量的重新规划（replan_by_offset 为 true），并且停车制动器已释放（车辆开始移动），则触发重新规划 if (replan_by_offset) { if (vehicle_chassis.has_parking_brake()) { static bool parking_brake = true; @@ -196,7 +242,7 @@ std::vector TrajectoryStitcher::ComputeStitchingTrajectory( } parking_brake = vehicle_chassis.parking_brake(); } - +// 计算轨迹点与实际位置在纵向（lon_diff）、横向（lat_diff）和时间上的差异 auto lon_diff = time_matched_point.path_point().s() - frenet_sd.first; auto lat_diff = frenet_sd.second; double time_diff = @@ -208,8 +254,8 @@ std::vector TrajectoryStitcher::ComputeStitchingTrajectory( ADEBUG << "Control lateral diff: " << lat_diff << ", longitudinal diff: " << lon_diff << ", time diff: " << time_diff; - - if (std::fabs(lat_diff) > FLAGS_replan_lateral_distance_threshold) { +// 如果横向误差（lat_diff）超过预设阈值，则重新规划 + if (std::fabs(lat_diff) > FLAGS_replan_lateral_distance_threshold) { // 0.5 const std::string msg = absl::StrCat( "the distance between matched point and actual position is too " "large. Replan is triggered. lat_diff = ", @@ -219,8 +265,8 @@ std::vector TrajectoryStitcher::ComputeStitchingTrajectory( return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } - - if (std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold) { +// 如果纵向误差（lon_diff）超过预设阈值，则重新规划 + if (std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold) { // 2.5 const std::string msg = absl::StrCat( "the distance between matched point and actual position is too " "large. Replan is triggered. lon_diff = ", @@ -230,8 +276,8 @@ std::vector TrajectoryStitcher::ComputeStitchingTrajectory( return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } - - if (std::fabs(time_diff) > FLAGS_replan_time_threshold) { +// 如果时间差异（time_diff）超过预设阈值，则重新规划 + if (std::fabs(time_diff) > FLAGS_replan_time_threshold) { // 7.0 const std::string msg = absl::StrCat( "the difference between time matched point relative time and " "actual position corresponding relative time is too " @@ -247,43 +293,66 @@ std::vector TrajectoryStitcher::ComputeStitchingTrajectory( << "lat、lon and time offset is disabled"; } - double forward_rel_time = veh_rel_time + planning_cycle_time; +// 如果 所有重规划条件都不满足，进入正常缝合 +// 计算前进方向上的相对时间 +// 保留到 未来一个规划周期（如当前 10.1s，规划周期 0.1s → 保留到 10.2s） + double forward_rel_time = veh_rel_time + planning_cycle_time; +// 查找与前进方向时间最接近的轨迹点 size_t forward_time_index = prev_trajectory->QueryLowerBoundPoint(forward_rel_time); - +// 输出位置匹配和时间匹配的索引 ADEBUG << "Position matched index:\t" << position_matched_index; ADEBUG << "Time matched index:\t" << time_matched_index; - +// 选择时间匹配和位置匹配中较小的索引作为最终的匹配点 +// 确定起始索引:取 更保守（靠前） 的匹配点，避免跳过当前位置 auto matched_index = std::min(time_matched_index, position_matched_index); - +// 基于匹配点和保留点数，从先前轨迹中提取出拼接的轨迹部分 +// 从 prev_trajectory 中提取一段子轨迹，起始点为 matched_index - preserved_points_num（确保不会越界），结束点为 forward_time_index +// preserved_points_num：额外保留的历史点（如 20 点 ≈ 2 秒） std::vector stitching_trajectory( prev_trajectory->begin() + std::max(0, static_cast(matched_index - preserved_points_num)), prev_trajectory->begin() + forward_time_index + 1); ADEBUG << "stitching_trajectory size: " << stitching_trajectory.size(); - +// 获取拼接轨迹最后一个点的路径坐标（s值），用于后续归一化 const double zero_s = stitching_trajectory.back().path_point().s(); +// 遍历拼接轨迹，对每个轨迹点进行处理 for (auto& tp : stitching_trajectory) { +// 如果轨迹点缺少路径点，则重新规划 if (!tp.has_path_point()) { *replan_reason = "replan for previous trajectory missed path point"; return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state); } + // 调整每个点的相对时间，并将路径坐标归一化 + // 时间归一化：使当前时刻 ≈ 0 tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() - current_timestamp); + // 路程归一化：使当前点 s = 0 tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s); + // 归一化后，Planner 在 以当前车辆为原点的局部坐标系 中工作 } + // 返回拼接后的轨迹 return stitching_trajectory; } - +/// @brief +/// @param x 需要投影的目标位置的x坐标 +/// @param y 需要投影的目标位置的y坐标 +/// @param p 包含一个路径点的信息，这个路径点用于计算目标位置相对于路径的投影 +/// @return std::pair TrajectoryStitcher::ComputePositionProjection( const double x, const double y, const TrajectoryPoint& p) { + // 计算从路径点p到目标点(x,y)的向量v Vec2d v(x - p.path_point().x(), y - p.path_point().y()); + // 计算路径方向的单位向量 n，即路径点的切线方向 Vec2d n(std::cos(p.path_point().theta()), std::sin(p.path_point().theta())); std::pair frenet_sd; + // v.InnerProd(n) 计算向量 v 和单位向量 n 的内积（点积）。这表示目标点在路径的切线方向上的投影长度 frenet_sd.first = v.InnerProd(n) + p.path_point().s(); + // 计算了 v 和 n 的叉积，得到目标点相对于路径的正交坐标 d + // 对于二维向量，叉积的结果是一个标量，表示向量 v 在垂直于 n 的方向上的分量 frenet_sd.second = v.CrossProd(n); return frenet_sd; } diff --git a/modules/planning/planning_base/common/util/common.cc b/modules/planning/planning_base/common/util/common.cc index 8af59aeca4c..2dcd7943de4 100644 --- a/modules/planning/planning_base/common/util/common.cc +++ b/modules/planning/planning_base/common/util/common.cc @@ -25,50 +25,72 @@ using apollo::common::util::WithinBound; /* * @brief: build virtual obstacle of stop wall, and add STOP decision */ +/// @brief 根在参考线上 stop_line_s 位置创建一个虚拟障碍物（代表车辆应该停止的位置），并基于此障碍物添加一个带有距离和原因的 stop 决策 +/// @param stop_wall_id 停车围栏的标识符 +/// @param stop_line_s 停车线的位置（在参考线上的 s 坐标） +/// @param stop_distance 停车距离，即车辆从停车线到目标停车点的距离 +/// @param stop_reason_code 停车的原因代码（例如，停车以避免碰撞等） +/// @param wait_for_obstacles 一个字符串数组，表示在停车前需要等待的障碍物标识符 +/// @param decision_tag 决策标签，用于标识这个停车决策 +/// @param frame 当前的帧对象，包含车辆的状态和其他信息 +/// @param reference_line_info 参考线信息，包含路径和决策 +/// @return int BuildStopDecision(const std::string& stop_wall_id, const double stop_line_s, const double stop_distance, const StopReasonCode& stop_reason_code, const std::vector& wait_for_obstacles, const std::string& decision_tag, Frame* const frame, ReferenceLineInfo* const reference_line_info) { + // 空指针检查 CHECK_NOTNULL(frame); CHECK_NOTNULL(reference_line_info); // check const auto& reference_line = reference_line_info->reference_line(); + // 检查 stop_line_s 是否在参考线的有效范围内 if (!WithinBound(0.0, reference_line.Length(), stop_line_s)) { AERROR << "stop_line_s[" << stop_line_s << "] is not on reference line"; return 0; } // create virtual stop wall + // 此障碍物位于 stop_line_s 位置，并使用 stop_wall_id 作为障碍物的标识符 const auto* obstacle = frame->CreateStopObstacle(reference_line_info, stop_wall_id, stop_line_s); + // 判断创建的障碍物是否成功 if (!obstacle) { AERROR << "Failed to create obstacle [" << stop_wall_id << "]"; return -1; } + // 将创建的停车围栏障碍物 obstacle 添加到参考线的障碍物列表中，并返回添加后的障碍物对象指针 stop_wall const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle); + // 判断添加障碍物是否成功 if (!stop_wall) { AERROR << "Failed to add obstacle[" << stop_wall_id << "]"; return -1; } // build stop decision + // 计算停车点的位置 stop_s，它位于停车线 stop_line_s 前方 stop_distance 的位置 const double stop_s = stop_line_s - stop_distance; + // 该位置的参考点（包含坐标和方向），并计算停车点的朝向 stop_heading const auto& stop_point = reference_line.GetReferencePoint(stop_s); const double stop_heading = reference_line.GetReferencePoint(stop_s).heading(); - + // 创建一个 ObjectDecisionType 类型的停车决策对象 stop ObjectDecisionType stop; auto* stop_decision = stop.mutable_stop(); + // 设置停车原因代码 stop_decision->set_reason_code(stop_reason_code); + // 设置停车距离，-stop_distance 表示停车点在停车线前方 stop_decision->set_distance_s(-stop_distance); + // 设置停车点的朝向 stop_decision->set_stop_heading(stop_heading); + // 设置停车点的坐标 stop_decision->mutable_stop_point()->set_x(stop_point.x()); stop_decision->mutable_stop_point()->set_y(stop_point.y()); stop_decision->mutable_stop_point()->set_z(0.0); - + // 遍历 wait_for_obstacles 列表，将需要等待的障碍物标识符添加到停车决策中 for (size_t i = 0; i < wait_for_obstacles.size(); ++i) { stop_decision->add_wait_for_obstacle(wait_for_obstacles[i]); } diff --git a/modules/planning/planning_base/common/util/config_util.cc b/modules/planning/planning_base/common/util/config_util.cc index eb8435c3587..4225c1573fa 100644 --- a/modules/planning/planning_base/common/util/config_util.cc +++ b/modules/planning/planning_base/common/util/config_util.cc @@ -29,10 +29,15 @@ std::string ConfigUtil::TransformToPathName(const std::string& name) { return output; } +/// @brief ConfigUtil 类的一个静态成员函数，用于获取完整的规划器类名 +/// @param class_name +/// @return std::string ConfigUtil::GetFullPlanningClassName( const std::string& class_name) { + // 静态常量字符串 static const std::string kNameSpace = "apollo::planning::"; // If the class name already has namespace in it, do nothing. + // 如果 class_name 里面已经有 ::，说明它是完整的类名，则直接返回，不做任何修改 if (class_name.find("::") != std::string::npos) { return class_name; } diff --git a/modules/planning/planning_base/common/util/util.cc b/modules/planning/planning_base/common/util/util.cc index 2329a4d295f..3e53bf391a7 100644 --- a/modules/planning/planning_base/common/util/util.cc +++ b/modules/planning/planning_base/common/util/util.cc @@ -32,6 +32,9 @@ namespace util { using apollo::common::VehicleState; using apollo::hdmap::PathOverlap; +/// @brief +/// @param vehicle_state +/// @return bool IsVehicleStateValid(const VehicleState& vehicle_state) { if (std::isnan(vehicle_state.x()) || std::isnan(vehicle_state.y()) || std::isnan(vehicle_state.z()) || std::isnan(vehicle_state.heading()) || @@ -43,14 +46,21 @@ bool IsVehicleStateValid(const VehicleState& vehicle_state) { return true; } +/// @brief 判断两个 PlanningCommand 对象在路线规划方面是否有差异，主要是通过比较它们的头部信息（header）来实现 +/// @param first +/// @param second +/// @return bool IsDifferentRouting(const PlanningCommand& first, const PlanningCommand& second) { +// 检查两个 PlanningCommand 对象是否都包含头部信息 if (first.has_header() && second.has_header()) { const auto& first_header = first.header(); const auto& second_header = second.header(); + // 比较头部的序列号和模块名称，判断是否不同 return (first_header.sequence_num() != second_header.sequence_num() || first_header.module_name() != second_header.module_name()); } + // 如果任何一个命令缺少头部信息，则认为它们不同 return true; } diff --git a/modules/planning/planning_base/math/constraint_checker/constraint_checker.cc b/modules/planning/planning_base/math/constraint_checker/constraint_checker.cc index c6e810c71d7..95f2e6a5f6f 100644 --- a/modules/planning/planning_base/math/constraint_checker/constraint_checker.cc +++ b/modules/planning/planning_base/math/constraint_checker/constraint_checker.cc @@ -27,21 +27,26 @@ namespace apollo { namespace planning { namespace { +// 判断值 v 是否在 [lower, upper] 闭区间内 template bool WithinRange(const T v, const T lower, const T upper) { return lower <= v && v <= upper; } } // namespace +//检查输入轨迹是否满足所有预设的物理和舒适性约束 ConstraintChecker::Result ConstraintChecker::ValidTrajectory( const DiscretizedTrajectory& trajectory) { const double kMaxCheckRelativeTime = FLAGS_trajectory_time_length; + // 逐点检查基本状态量（速度、加速度、曲率） for (const auto& p : trajectory) { double t = p.relative_time(); + // 只检查轨迹中 相对时间 ≤ FLAGS_trajectory_time_length 的部分（通常为 8 秒） if (t > kMaxCheckRelativeTime) { break; } double lon_v = p.v(); + // -0.1 40 if (!WithinRange(lon_v, FLAGS_speed_lower_bound, FLAGS_speed_upper_bound)) { ADEBUG << "Velocity at relative time " << t << " exceeds bound, value: " << lon_v << ", bound [" @@ -51,6 +56,7 @@ ConstraintChecker::Result ConstraintChecker::ValidTrajectory( } double lon_a = p.a(); + // -6 4 if (!WithinRange(lon_a, FLAGS_longitudinal_acceleration_lower_bound, FLAGS_longitudinal_acceleration_upper_bound)) { ADEBUG << "Longitudinal acceleration at relative time " << t @@ -61,6 +67,7 @@ ConstraintChecker::Result ConstraintChecker::ValidTrajectory( } double kappa = p.path_point().kappa(); + // 0.1979 if (!WithinRange(kappa, -FLAGS_kappa_bound, FLAGS_kappa_bound)) { ADEBUG << "Kappa at relative time " << t << " exceeds bound, value: " << kappa << ", bound [" @@ -68,7 +75,7 @@ ConstraintChecker::Result ConstraintChecker::ValidTrajectory( return Result::CURVATURE_OUT_OF_BOUND; } } - + // 逐段检查高阶导数（加加速度 jerk、横向加速度） for (size_t i = 1; i < trajectory.NumOfPoints(); ++i) { const auto& p0 = trajectory.TrajectoryPointAt(static_cast(i - 1)); const auto& p1 = trajectory.TrajectoryPointAt(static_cast(i)); @@ -78,10 +85,12 @@ ConstraintChecker::Result ConstraintChecker::ValidTrajectory( } double t = p0.relative_time(); - + // 从第 1 个点开始，与前一个点组成线段，计算变化率（需要时间差 dt） double dt = p1.relative_time() - p0.relative_time(); double d_lon_a = p1.a() - p0.a(); + // Jerk（加加速度） = 加速度变化率 = (a1 - a0) / dt（单位：m/s³） double lon_jerk = d_lon_a / dt; + // -4 2 if (!WithinRange(lon_jerk, FLAGS_longitudinal_jerk_lower_bound, FLAGS_longitudinal_jerk_upper_bound)) { ADEBUG << "Longitudinal jerk at relative time " << t @@ -90,8 +99,9 @@ ConstraintChecker::Result ConstraintChecker::ValidTrajectory( << FLAGS_longitudinal_jerk_upper_bound << "]."; return Result::LON_JERK_OUT_OF_BOUND; } - + // 横向加速度公式：a_lat = v² × κ（由圆周运动公式 a = v² / r，且 κ = 1/r 推导） double lat_a = p1.v() * p1.v() * p1.path_point().kappa(); + // 4 if (!WithinRange(lat_a, -FLAGS_lateral_acceleration_bound, FLAGS_lateral_acceleration_bound)) { ADEBUG << "Lateral acceleration at relative time " << t @@ -106,7 +116,10 @@ ConstraintChecker::Result ConstraintChecker::ValidTrajectory( /** double d_lat_a = p1.v() * p1.v() * p1.path_point().kappa() - p0.v() * p0.v() * p0.path_point().kappa(); + // 横向 jerk = (a_lat1 - a_lat0) / dt + // 当前被禁用，原因：参考线质量不高，导致 kappa 抖动大，误报 jerk 超限 double lat_jerk = d_lat_a / dt; + // 4 if (!WithinRange(lat_jerk, -FLAGS_lateral_jerk_bound, FLAGS_lateral_jerk_bound)) { ADEBUG << "Lateral jerk at relative time " << t diff --git a/modules/planning/planning_base/reference_line/reference_line.cc b/modules/planning/planning_base/reference_line/reference_line.cc index dc4582210b3..23af8ec829c 100644 --- a/modules/planning/planning_base/reference_line/reference_line.cc +++ b/modules/planning/planning_base/reference_line/reference_line.cc @@ -141,6 +141,11 @@ ReferencePoint ReferenceLine::GetNearestReferencePoint( return reference_points_[min_index]; } +/// @brief +/// @param point +/// @param look_backward +/// @param look_forward +/// @return bool ReferenceLine::Segment(const common::math::Vec2d& point, const double look_backward, const double look_forward) { @@ -213,18 +218,44 @@ common::FrenetFramePoint ReferenceLine::GetFrenetPoint( frenet_frame_point.set_ddl(ddl); return frenet_frame_point; } - +/// @brief 将一个给定的轨迹点（traj_point）转换为 Frenet 坐标系下的 s 和 l 条件， +//并返回一个 std::pair, std::array> 类型的值 +/// @param traj_point +/// @return +// 返回值： std::pair, std::array> +// s_condition：沿参考线方向的【位置 s，速度 s'，加速度 s''】； +// l_condition：垂直参考线方向的【偏移量 l，速度 l'，加速度 l''】。 std::pair, std::array> ReferenceLine::ToFrenetFrame(const common::TrajectoryPoint& traj_point) const { + // 验证参考点非空 ACHECK(!reference_points_.empty()); common::SLPoint sl_point; + // 使用 XYToSL 函数，将 traj_point 对应的 (x, y) 坐标转换成 Frenet 系的 (s, l) + // s：点在线上的投影点离参考线起点的距离 + // l：点到参考线的垂直距离 + // theta 用于辅助判断偏移方向（左侧为正 l，右侧为负 l） + // 第一步： XYToSL(traj_point.path_point().theta(), {traj_point.path_point().x(), traj_point.path_point().y()}, &sl_point); + // 结果变量 std::array s_condition; std::array l_condition; + + // 根据上一步获得的 s 值，在参考线上获取对应的 ReferencePoint（插值或查表） + // 包含：x, y, heading（朝向）, kappa（曲率）, dkappa（曲率变化率）等信息 + // 这个点是 Frenet 转换的“基准点” + // 第二步：进行插值查找 ReferencePoint ref_point = GetReferencePoint(sl_point.s()); + + // 将轨迹点的笛卡尔坐标转换为 Frenet 坐标 + // s：路径上投影点的弧长位置 + // s'：在参考线方向上的速度 + // s''：在参考线方向上的加速度 + // l：车辆相对于参考线的横向偏移 + // l'：横向速度 + // l''：横向加速度 CartesianFrenetConverter::cartesian_to_frenet( sl_point.s(), ref_point.x(), ref_point.y(), ref_point.heading(), ref_point.kappa(), ref_point.dkappa(), traj_point.path_point().x(), @@ -299,8 +330,11 @@ std::vector ReferenceLine::GetReferencePoints( } return ref_points; } - +/// @brief +/// @param s 路径上某一点的累积距离值，即路径上的 s 坐标 +/// @return ReferencePoint ReferenceLine::GetReferencePoint(const double s) const { + // 检查给定的 s 是否在路径的有效范围内 const auto& accumulated_s = map_path_.accumulated_s(); if (s < accumulated_s.front() - 1e-2) { AWARN << "The requested s: " << s << " < 0."; @@ -311,7 +345,9 @@ ReferencePoint ReferenceLine::GetReferencePoint(const double s) const { << " > reference line length: " << accumulated_s.back(); return reference_points_.back(); } - + // 获取 s 所在路径段的索引 + // 返回一个结构体，里面包含了 id（路径段的索引）。然后，index 和 next_index 分别表示当前路径段和下一个路径段的索引。 + // 如果下一个索引超出了参考点数组的范围，next_index 被设置为最后一个参考点的索引 auto interpolate_index = map_path_.GetIndexFromS(s); size_t index = interpolate_index.id; @@ -319,12 +355,13 @@ ReferencePoint ReferenceLine::GetReferencePoint(const double s) const { if (next_index >= reference_points_.size()) { next_index = reference_points_.size() - 1; } - + // 获取当前路径段的参考点 p0 和下一个路径段的参考点 p1 const auto& p0 = reference_points_[index]; const auto& p1 = reference_points_[next_index]; - + // 获取当前参考点和下一个参考点的累计弧长值 s0 和 s1 const double s0 = accumulated_s[index]; const double s1 = accumulated_s[next_index]; + // 通过已知的两个参考点 p0 和 p1，以及它们的累计弧长 s0 和 s1，根据 s 值插值得到对应的参考点 return InterpolateWithMatchedIndex(p0, s0, p1, s1, interpolate_index); } @@ -398,7 +435,11 @@ bool ReferenceLine::SLToXY(const SLPoint& sl_point, xy_point->set_y(matched_point.y() + common::math::cos(angle) * sl_point.l()); return true; } - +/// @brief +/// @param xy_point +/// @param sl_point +/// @param warm_start_s +/// @return bool ReferenceLine::XYToSL(const common::math::Vec2d& xy_point, common::SLPoint* const sl_point, double warm_start_s) const { @@ -421,14 +462,24 @@ bool ReferenceLine::XYToSL(const common::math::Vec2d& xy_point, sl_point->set_l(l); return true; } - +/// @brief +/// @param heading 当前点的航向角 +/// @param xy_point 当前点在笛卡尔坐标系中的坐标 +/// @param sl_point 输出的 Frenet 坐标点，包含 s 和 l 值 +/// @param warm_start_s 提供的起始位置 s，如果小于 0 会进行路径的重新计算 +/// @return bool ReferenceLine::XYToSL(const double heading, const common::math::Vec2d& xy_point, common::SLPoint* const sl_point, double warm_start_s) const { + // 如果 warm_start_s 小于 0，则通过路径投影计算 double s = warm_start_s; double l = 0.0; if (warm_start_s < 0.0) { + // 调用 `map_path_` 的方法在参考线上**搜索离 `(x, y)` 最近的投影点**，返回： + // `s`：投影点在参考线上的弧长 + // `l`：输入点到投影点的横向偏移（垂直于参考线方向，左正右负） + // heading: 右方 x轴 0 前方 y轴 90 逆时针 if (!map_path_.GetProjection(heading, xy_point, &s, &l)) { AERROR << "Cannot get nearest point from path."; return false; @@ -445,22 +496,32 @@ bool ReferenceLine::XYToSL(const double heading, sl_point->set_l(l); return true; } - +/// @brief +/// @param p0 路径段的起始参考点 +/// @param s0 路径段的起始参考点对应的累积 s 值 +/// @param p1 路径段的终止参考点 +/// @param s1 路径段的终止参考点对应的累积 s 值 +/// @param index 包含路径段的索引和偏移量的结构体，用于确定插值位置 +/// @return ReferencePoint ReferenceLine::InterpolateWithMatchedIndex( const ReferencePoint& p0, const double s0, const ReferencePoint& p1, const double s1, const InterpolatedIndex& index) const { + // 两个点太近 if (std::fabs(s0 - s1) < common::math::kMathEpsilon) { return p0; } + // 使用传入的 index.offset 计算当前目标 s 值。offset 是从起始点 s0 开始的相对偏移，表示目标位置在路径段上的位置 double s = s0 + index.offset; + // 使用 DCHECK_LE 进行断言检查，确保目标 s 值在 s0 和 s1 之间（稍微容忍误差）。如果 s 值超出了该范围，将会触发断言错误 DCHECK_LE(s0 - 1.0e-6, s) << "s: " << s << " is less than s0 : " << s0; DCHECK_LE(s, s1 + 1.0e-6) << "s: " << s << " is larger than s1: " << s1; - + // 根据插值位置的 index 获取平滑路径点。这通常是通过对路径进行插值计算得到的连续的路径点 auto map_path_point = map_path_.GetSmoothPoint(index); + // 使用 lerp（线性插值）计算路径段上目标位置的曲率 (kappa) 和曲率变化率 (dkappa) const double kappa = common::math::lerp(p0.kappa(), s0, p1.kappa(), s1, s); const double dkappa = common::math::lerp(p0.dkappa(), s0, p1.dkappa(), s1, s); - - return ReferencePoint(map_path_point, kappa, dkappa); + // 使用获取的平滑路径点 map_path_point、计算得到的曲率 kappa 和曲率变化率 dkappa，创建并返回一个新的 ReferencePoint，表示路径上目标位置的参考点 + return ReferencePoint(map_path_point, kappa, dkappa); } ReferencePoint ReferenceLine::Interpolate(const ReferencePoint& p0, @@ -518,12 +579,15 @@ bool ReferenceLine::GetLaneWidth(const double s, double* const lane_left_width, } return true; } - +/// @brief 根据给定的 s 坐标，计算并返回相应的 l 偏移量（即车道中心到参考线的偏移量） +/// @param s +/// @param l_offset +/// @return bool ReferenceLine::GetOffsetToMap(const double s, double* l_offset) const { if (map_path_.path_points().empty()) { return false; } - +// 获取距离给定 s 坐标最近的参考点 auto ref_point = GetNearestReferencePoint(s); if (ref_point.lane_waypoints().empty()) { return false; @@ -611,6 +675,9 @@ bool ReferenceLine::IsOnLane(const SLBoundary& sl_boundary) const { sl_boundary.end_l() >= -lane_right_width; } +/// @brief 判断sl点是否在车道内 +/// @param sl_point +/// @return bool ReferenceLine::IsOnLane(const SLPoint& sl_point) const { if (sl_point.s() <= 0 || sl_point.s() > map_path_.length()) { return false; diff --git a/modules/planning/planning_base/reference_line/reference_line_provider.cc b/modules/planning/planning_base/reference_line/reference_line_provider.cc index a7019e29dbd..37e59c6f2ad 100644 --- a/modules/planning/planning_base/reference_line/reference_line_provider.cc +++ b/modules/planning/planning_base/reference_line/reference_line_provider.cc @@ -57,6 +57,10 @@ using apollo::hdmap::RouteSegments; ReferenceLineProvider::~ReferenceLineProvider() {} +/// @brief +/// @param vehicle_state_provider 一个指向 common::VehicleStateProvider 的指针，用于提供车辆状态信息 +/// @param reference_line_config 参考线的配置，包含参考线的各种参数 +/// @param relative_map 用于导航模式下的相对地图信息 ReferenceLineProvider::ReferenceLineProvider( const common::VehicleStateProvider *vehicle_state_provider, const ReferenceLineConfig *reference_line_config, @@ -69,10 +73,14 @@ ReferenceLineProvider::ReferenceLineProvider( relative_map_ = relative_map; } + // modules/planning/conf/planning.conf中定义smoother_config_filename + // 或者使用 modules/planning/common/planning_gflags.cc ACHECK(cyber::common::GetProtoFromFile(FLAGS_smoother_config_filename, &smoother_config_)) << "Failed to load smoother config file " << FLAGS_smoother_config_filename; + + // 根据配置，创建参考线平滑器。 多态: 父类指针指向子类对象 if (smoother_config_.has_qp_spline()) { smoother_.reset(new QpSplineReferenceLineSmoother(smoother_config_)); } else if (smoother_config_.has_spiral()) { @@ -106,36 +114,48 @@ ReferenceLineProvider::ReferenceLineProvider( is_initialized_ = true; } +/// @brief 更新参考路线的规划命令，并确保当前的规划命令能正确处理并匹配相应的 PNC map（路径导航计算图） +/// @param command +/// @return bool ReferenceLineProvider::UpdatePlanningCommand( const planning::PlanningCommand &command) { + // 使用路由锁保护对 pnc_map_list_ 的访问 std::lock_guard routing_lock(routing_mutex_); bool find_matched_pnc_map = false; + // 遍历 pnc_map_list_ 查找能够处理当前命令的 pnc_map for (const auto &pnc_map : pnc_map_list_) { if (pnc_map->CanProcess(command)) { - current_pnc_map_ = pnc_map; + current_pnc_map_ = pnc_map; // 找到匹配的 pnc_map，更新 current_pnc_map_ find_matched_pnc_map = true; break; } } + // 如果没有找到合适的 pnc_map，返回失败 if (nullptr == current_pnc_map_) { AERROR << "Cannot find pnc map to process input command!" << command.DebugString(); return false; } + // 如果没有找到匹配的 pnc_map，警告使用旧的 pnc_map if (!find_matched_pnc_map) { AWARN << "Find no pnc map for the input command and the old one will be " "used!"; } + // 锁住 pnc_map_mutex_ 来更新当前 pnc_map 的路由 // Update routing in pnc_map std::lock_guard lock(pnc_map_mutex_); + // 如果当前 pnc_map 需要更新规划命令 if (current_pnc_map_->IsNewPlanningCommand(command)) { is_new_command_ = true; + + // 更新 pnc_map 路由信息 if (!current_pnc_map_->UpdatePlanningCommand(command)) { AERROR << "Failed to update routing in pnc map: " << command.DebugString(); return false; } } + // 更新当前的规划命令和状态 planning_command_ = command; has_planning_command_ = true; return true; @@ -152,6 +172,8 @@ ReferenceLineProvider::FutureRouteWaypoints() { return std::vector(); } +/// @brief 获取参考车道的结束航路点，并将其通过引用传递给 end_point +/// @param end_point void ReferenceLineProvider::GetEndLaneWayPoint( std::shared_ptr &end_point) const { if (nullptr == current_pnc_map_) { @@ -175,6 +197,8 @@ void ReferenceLineProvider::UpdateVehicleState( vehicle_state_ = vehicle_state; } +/// @brief +/// @return bool ReferenceLineProvider::Start() { if (FLAGS_use_navigation_mode) { return true; @@ -183,7 +207,8 @@ bool ReferenceLineProvider::Start() { AERROR << "ReferenceLineProvider has NOT been initiated."; return false; } - + // 异步启动 + // 创建参考线生成线程：FLAGS_enable_reference_line_provider_thread定义在modules/planning/common/pkanning_gflags.cc中 if (FLAGS_enable_reference_line_provider_thread) { task_future_ = cyber::Async(&ReferenceLineProvider::GenerateThread, this); } @@ -223,6 +248,8 @@ void ReferenceLineProvider::UpdateReferenceLine( return; } std::lock_guard lock(reference_lines_mutex_); + + // 将最新计算好的参考线拷贝给成员变量 if (reference_lines_.size() != reference_lines.size()) { reference_lines_ = reference_lines; route_segments_ = route_segments; @@ -237,7 +264,7 @@ void ReferenceLineProvider::UpdateReferenceLine( internal_segment_iter != route_segments_.end(); ++iter, ++segment_iter, ++internal_iter, ++internal_segment_iter) { if (iter->reference_points().empty()) { - *internal_iter = *iter; + *internal_iter = *iter; // 将iter所指向的数据复制到internal_iter 所指向的位置 *internal_segment_iter = *segment_iter; continue; } @@ -264,25 +291,37 @@ void ReferenceLineProvider::UpdateReferenceLine( } } +// 多线程的回调函数 void ReferenceLineProvider::GenerateThread() { - while (!is_stop_) { + // is_stop_ 是一个 原子变量（std::atomic），用于标识线程是否应终止 + // 只要 is_stop_ 为 false，线程就会一直运行 + while (!is_stop_) { // 执行的定时任务，每隔50ms提供一次参考线 static constexpr int32_t kSleepTime = 50; // milliseconds + // 线程休眠50ms cyber::SleepFor(std::chrono::milliseconds(kSleepTime)); const double start_time = Clock::NowInSeconds(); + // 如果没有收到新的规划命令，则 跳过本次循环，等待下一次执行 if (!has_planning_command_) { continue; } - std::list reference_lines; - std::list segments; + + // 生成参考线和短期路由 + std::list reference_lines; // 用于存储生成的参考线 + std::list segments; // 用于存储对应的路径段信息 if (!CreateReferenceLine(&reference_lines, &segments)) { is_reference_line_updated_ = false; AERROR << "Fail to get reference line"; continue; } + + + // 将 reference_lines 和 segments 更新到类成员变量中，以便规划模块使用 UpdateReferenceLine(reference_lines, segments); const double end_time = Clock::NowInSeconds(); + // 使用 互斥锁 reference_lines_mutex_ 保护 last_calculation_time_，防止并发访问导致数据异常 std::lock_guard lock(reference_lines_mutex_); last_calculation_time_ = end_time - start_time; + // 新的参考线已更新 is_reference_line_updated_ = true; } } @@ -296,17 +335,23 @@ double ReferenceLineProvider::LastTimeDelay() { return last_calculation_time_; } } - +/// @brief 是否成功获取了参考线（reference_lines）和路段段（segments） +/// @param reference_lines +/// @param segments +/// @return bool ReferenceLineProvider::GetReferenceLines( std::list *reference_lines, std::list *segments) { +// 确保 reference_lines 和 segments 这两个指针不为空 CHECK_NOTNULL(reference_lines); CHECK_NOTNULL(segments); if (!has_planning_command_) { + // 没有必要更新参考线 return true; } if (FLAGS_use_navigation_mode) { double start_time = Clock::NowInSeconds(); + // 尝试从“相对地图”中获取参考线和路段 bool result = GetReferenceLinesFromRelativeMap(reference_lines, segments); if (!result) { AERROR << "Failed to get reference line from relative map"; @@ -359,7 +404,10 @@ void ReferenceLineProvider::PrioritizeChangeLane( ++iter; } } - +/// @brief 从relative_map（一个描述相对位置的地图）中获取参考路线，并填充到reference_lines和segments列表中 +/// @param reference_lines 存储返回的参考线 +/// @param segments 存储路径段信息 +/// @return bool ReferenceLineProvider::GetReferenceLinesFromRelativeMap( std::list *reference_lines, std::list *segments) { @@ -379,11 +427,13 @@ bool ReferenceLineProvider::GetReferenceLinesFromRelativeMap( } // 1.get adc current lane info ,such as lane_id,lane_priority,neighbor lanes + //获取当前adc（自动驾驶车辆）的车道信息，首先通过遍历relative_map_中的导航路径，将所有车道ID存入navigation_lane_ids集合 std::unordered_set navigation_lane_ids; for (const auto &path_pair : relative_map_->navigation_path()) { const auto lane_id = path_pair.first; navigation_lane_ids.insert(lane_id); } + // 如果导航路径中的车道ID集合为空，则输出错误日志 if (navigation_lane_ids.empty()) { AERROR << "navigation path ids is empty"; return false; @@ -391,6 +441,7 @@ bool ReferenceLineProvider::GetReferenceLinesFromRelativeMap( // get current adc lane info by vehicle state common::VehicleState vehicle_state = vehicle_state_provider_->vehicle_state(); hdmap::LaneWaypoint adc_lane_way_point; + // 根据车辆状态和导航路径获取与当前车道最近的路点（adc_lane_way_point if (!GetNearestWayPointFromNavigationPath(vehicle_state, navigation_lane_ids, &adc_lane_way_point)) { return false; @@ -608,16 +659,23 @@ bool ReferenceLineProvider::GetNearestWayPointFromNavigationPath( return waypoint->lane != nullptr; } +/// @brief 是否成功创建了路线段（Route Segments） +/// @param vehicle_state common::VehicleState 类型，表示车辆的当前状态（如位置、速度、朝向等） +/// @param segments 指向 std::list 的指针，用于存储生成的路线段 +/// @return bool ReferenceLineProvider::CreateRouteSegments( const common::VehicleState &vehicle_state, std::list *segments) { { + // 使用 std::lock_guard 加锁 pnc_map_mutex_ 互斥量，防止多线程并发访问 pnc_map_ 对象导致数据竞争 std::lock_guard lock(pnc_map_mutex_); + // 根据 vehicle_state 获取当前车辆对应的路线段，并存入 segments if (!current_pnc_map_->GetRouteSegments(vehicle_state, segments)) { AERROR << "Failed to extract segments from routing"; return false; } } + // 遍历 segments 中的每个 RouteSegment，并调用其 DebugString() 方法，打印调试信息 for (auto &seg : *segments) { ADEBUG << seg.DebugString(); } @@ -627,18 +685,24 @@ bool ReferenceLineProvider::CreateRouteSegments( return !segments->empty(); } +/// @brief 根据车辆状态和路由信息生成参考线，并对参考线进行平滑处理或拼接 +/// @param reference_lines 用于存储生成的参考线 +/// @param segments 用于存储 路由段信息（路径片段） +/// @return bool ReferenceLineProvider::CreateReferenceLine( std::list *reference_lines, std::list *segments) { CHECK_NOTNULL(reference_lines); CHECK_NOTNULL(segments); - common::VehicleState vehicle_state; + // vehicle_state_ 是 ReferenceLineProvider 类的成员变量，存储车辆的 当前位置、速度、航向角等信息 + common::VehicleState vehicle_state; // .pb.h { std::lock_guard lock(vehicle_state_mutex_); vehicle_state = vehicle_state_; } + // planning_command_ 存储了规划模块的指令，可能包含 变道、停止、加速等决策信息 planning::PlanningCommand command; { std::lock_guard lock(routing_mutex_); @@ -648,20 +712,31 @@ bool ReferenceLineProvider::CreateReferenceLine( AERROR << "Current pnc map is null! " << command.DebugString(); return false; } - + + // 根据 当前车辆状态 生成 路径段信息（Route Segments） if (!CreateRouteSegments(vehicle_state, segments)) { AERROR << "Failed to create reference line from routing"; return false; } - if (is_new_command_ || !FLAGS_enable_reference_line_stitching) { + + // 判断是否需要进行参考线拼接 + // 如果 收到新的规划命令 或 禁用拼接模式，则执行 重新生成参考线 逻辑；否则执行 参考线拼接 逻辑 + if (is_new_command_ || !FLAGS_enable_reference_line_stitching) { // 首次运行或者新路由，不拼接参考线 + // 重新生成参考线 + // 遍历路径段 segments，为每个路径段创建一个参考线 for (auto iter = segments->begin(); iter != segments->end();) { reference_lines->emplace_back(); + + // 参考线平滑 + // 平滑各路由片段列表segments，并将平滑后的路由片段存储到参考线reference_lines中，同时将不能平滑的路由片段从segments中删除 if (!SmoothRouteSegment(*iter, &reference_lines->back())) { AERROR << "Failed to create reference line from route segments"; + // 失败，删除该路径段 reference_lines->pop_back(); iter = segments->erase(iter); } else { common::SLPoint sl; + // 车辆坐标投影到参考线的 Frenet 坐标系 if (!reference_lines->back().XYToSL( vehicle_state.heading(), common::math::Vec2d(vehicle_state.x(), vehicle_state.y()), @@ -669,15 +744,22 @@ bool ReferenceLineProvider::CreateReferenceLine( AWARN << "Failed to project point: {" << vehicle_state.x() << "," << vehicle_state.y() << "} to stitched reference line"; } - Shrink(sl, &reference_lines->back(), &(*iter)); + // 成功，保留该参考线 + // 用于裁剪参考线，确保它符合当前规划需求 + Shrink(sl, &reference_lines->back(), &(*iter)); // 收缩参考线 ++iter; } } is_new_command_ = false; return true; } else { // stitching reference line + // 启用了参考线拼接（stitching） + // 先遍历 segments，为每个路径段创建新的参考线 for (auto iter = segments->begin(); iter != segments->end();) { reference_lines->emplace_back(); + +// 合并不同参考片段的重合部分，并将拼接后的路由片段保存到参考线列表reference_lines中，同时将不能拼接的路由片段从segments中删除 + // 负责将新路径段拼接到现有参考线上 if (!ExtendReferenceLine(vehicle_state, &(*iter), &reference_lines->back())) { AERROR << "Failed to extend reference line"; @@ -798,6 +880,7 @@ bool ReferenceLineProvider::Shrink(const common::SLPoint &sl, const auto &ref_points = reference_line->reference_points(); const double cur_heading = ref_points[index].heading(); auto last_index = index; + // 主车航向与参考线航向偏差超过一定阈值，对参考线进行收缩 while (last_index < ref_points.size() && std::fabs(AngleDiff(cur_heading, ref_points[last_index].heading())) < FLAGS_referfece_line_max_forward_heading_diff) { @@ -838,20 +921,23 @@ bool ReferenceLineProvider::Shrink(const common::SLPoint &sl, return true; } +// 确保路径平滑算法没有“过度扭曲”原始参考线，从而保证导航一致性与安全性 bool ReferenceLineProvider::IsReferenceLineSmoothValid( const ReferenceLine &raw, const ReferenceLine &smoothed) const { + // 1.每隔 10 米沿平滑后的参考线采样一个点进行检查 static constexpr double kReferenceLineDiffCheckStep = 10.0; + // 2. 沿平滑参考线遍历采样点 for (double s = 0.0; s < smoothed.Length(); s += kReferenceLineDiffCheckStep) { auto xy_new = smoothed.GetReferencePoint(s); - + // 3. 将平滑点投影到原始参考线的 Frenet 坐标系 common::SLPoint sl_new; if (!raw.XYToSL(xy_new, &sl_new)) { AERROR << "Fail to change xy point on smoothed reference line to sl " "point respect to raw reference line."; return false; } - + // 4. 检查横向偏差是否超限 const double diff = std::fabs(sl_new.l()); if (diff > FLAGS_smoothed_reference_line_max_diff) { AERROR << "Fail to provide reference line because too large diff " @@ -865,17 +951,23 @@ bool ReferenceLineProvider::IsReferenceLineSmoothValid( AnchorPoint ReferenceLineProvider::GetAnchorPoint( const ReferenceLine &reference_line, double s) const { + // 1.设置纵向边界限制 AnchorPoint anchor; anchor.longitudinal_bound = smoother_config_.longitudinal_boundary_bound(); + + // 2. 获取参考点并处理无车道信息的情况 auto ref_point = reference_line.GetReferencePoint(s); if (ref_point.lane_waypoints().empty()) { + // 路径点由参考点转换而来 anchor.path_point = ref_point.ToPathPoint(s); + // 直接使用最大横向边界（最宽松的约束） anchor.lateral_bound = smoother_config_.max_lateral_boundary_bound(); return anchor; } - + // 3.获取车辆宽度和车道左右边界信息 const double adc_width = VehicleConfigHelper::GetConfig().vehicle_param().width(); + // 垂直于行驶方向的单位向量（指向左侧） const Vec2d left_vec = Vec2d::CreateUnitVec2d(ref_point.heading() + M_PI / 2.0); auto waypoint = ref_point.lane_waypoints().front(); @@ -884,20 +976,22 @@ AnchorPoint ReferenceLineProvider::GetAnchorPoint( waypoint.lane->GetWidth(waypoint.s, &left_width, &right_width); const double kEpislon = 1e-8; double effective_width = 0.0; - + // 4. 初始安全车道宽度计算 // shrink width by vehicle width, curb double safe_lane_width = left_width + right_width; + // 安全车道宽度 = 车道总宽 - 车辆宽度 safe_lane_width -= adc_width; bool is_lane_width_safe = true; - + // 如果剩余空间太小（< ε），说明车道太窄，无法安全通行，标记为不安全，并设置极小的有效宽度 if (safe_lane_width < kEpislon) { ADEBUG << "lane width [" << left_width + right_width << "] " << "is smaller than adc width [" << adc_width << "]"; effective_width = kEpislon; is_lane_width_safe = false; } - + // 5. 考虑路缘（Curb）的影响 double center_shift = 0.0; + // 如果右侧/左侧是路缘（CURB），车辆应主动避让，因此 if (hdmap::RightBoundaryType(waypoint) == hdmap::LaneBoundaryType::CURB) { safe_lane_width -= smoother_config_.curb_shift(); if (safe_lane_width < kEpislon) { @@ -905,6 +999,7 @@ AnchorPoint ReferenceLineProvider::GetAnchorPoint( effective_width = kEpislon; is_lane_width_safe = false; } else { + // 将参考点向远离路缘的方向平移半个 curb_shift（即 center_shift 调整） center_shift += 0.5 * smoother_config_.curb_shift(); } } @@ -918,18 +1013,22 @@ AnchorPoint ReferenceLineProvider::GetAnchorPoint( center_shift -= 0.5 * smoother_config_.curb_shift(); } } - + // 6. 应用横向缓冲（Lateral Buffer） // apply buffer if possible + // 在安全宽度基础上，左右各扣除一个 lateral_buffer（如 0.2m），作为额外的安全余量 const double buffered_width = safe_lane_width - 2.0 * smoother_config_.lateral_buffer(); safe_lane_width = buffered_width < kEpislon ? safe_lane_width : buffered_width; // shift center depending on the road width + // 7. 确定最终横向边界（effective_width） + // 车道宽度足够安全，则横向边界设为 可用宽度的一半（因为车辆可在中心线左右摆动） if (is_lane_width_safe) { effective_width = 0.5 * safe_lane_width; } - + // 8. 应用中心线偏移并构造锚点 + // 将原始参考点按 center_shift 平移（避开路缘） ref_point += left_vec * center_shift; anchor.path_point = ref_point.ToPathPoint(s); anchor.lateral_bound = common::math::Clamp( @@ -942,16 +1041,25 @@ void ReferenceLineProvider::GetAnchorPoints( const ReferenceLine &reference_line, std::vector *anchor_points) const { CHECK_NOTNULL(anchor_points); +// interval为采样间隔，默认max_constraint_interval=0.25，即路径累积距离0.25m采样一个点 const double interval = smoother_config_.max_constraint_interval(); + +// 路径采样点数量计算 int num_of_anchors = std::max(2, static_cast(reference_line.Length() / interval + 0.5)); + +// uniform_slice函数就是对[0, len]区间等间隔采样，每两个点之间距离为(length_ - 0.0)/(num_of_anchors - 1) std::vector anchor_s; common::util::uniform_slice(0.0, reference_line.Length(), num_of_anchors - 1, &anchor_s); + +// 计算采样点的坐标(x,y),并进行矫正 for (const double s : anchor_s) { AnchorPoint anchor = GetAnchorPoint(reference_line, s); anchor_points->emplace_back(anchor); } + + // 参考线首位两点设置强约束 anchor_points->front().longitudinal_bound = 1e-6; anchor_points->front().lateral_bound = 1e-6; anchor_points->front().enforced = true; @@ -1018,6 +1126,7 @@ bool ReferenceLineProvider::SmoothReferenceLine( std::vector anchor_points; GetAnchorPoints(raw_reference_line, &anchor_points); smoother_->SetAnchorPoints(anchor_points); + // 调用具体的平滑算法进行参考线平滑 if (!smoother_->Smooth(raw_reference_line, reference_line)) { AERROR << "Failed to smooth reference line with anchor points"; return false; diff --git a/modules/planning/planning_base/reference_line/reference_line_provider.h b/modules/planning/planning_base/reference_line/reference_line_provider.h index caa4b6db82a..9b6001b194d 100644 --- a/modules/planning/planning_base/reference_line/reference_line_provider.h +++ b/modules/planning/planning_base/reference_line/reference_line_provider.h @@ -34,17 +34,17 @@ #include "modules/common_msgs/routing_msgs/routing.pb.h" #include "modules/planning/planning_base/proto/planning_config.pb.h" -#include "cyber/cyber.h" +#include "cyber/cyber.h" // 用于异步任务管理 #include "modules/common/util/factory.h" #include "modules/common/util/util.h" -#include "modules/common/vehicle_state/vehicle_state_provider.h" -#include "modules/map/pnc_map/pnc_map_base.h" +#include "modules/common/vehicle_state/vehicle_state_provider.h" // 提供车辆状态数据 +#include "modules/map/pnc_map/pnc_map_base.h" // 处理地图相关信息 #include "modules/planning/planning_base/common/indexed_queue.h" #include "modules/planning/planning_base/math/smoothing_spline/spline_2d_solver.h" -#include "modules/planning/planning_base/reference_line/discrete_points_reference_line_smoother.h" -#include "modules/planning/planning_base/reference_line/qp_spline_reference_line_smoother.h" -#include "modules/planning/planning_base/reference_line/reference_line.h" -#include "modules/planning/planning_base/reference_line/spiral_reference_line_smoother.h" +#include "modules/planning/planning_base/reference_line/discrete_points_reference_line_smoother.h" // 离散点参考线平滑器 +#include "modules/planning/planning_base/reference_line/qp_spline_reference_line_smoother.h" // QP 样条曲线平滑器 +#include "modules/planning/planning_base/reference_line/reference_line.h" // 参考线数据结构 +#include "modules/planning/planning_base/reference_line/spiral_reference_line_smoother.h" // 螺旋线参考线平滑器 /** * @namespace apollo::planning @@ -62,6 +62,10 @@ class ReferenceLineProvider { public: ReferenceLineProvider() = default; + /// @brief + /// @param vehicle_state_provider 车辆状态提供器 + /// @param reference_line_config 参考线配置 + /// @param relative_map 可选：相对地图数据（用于导航模式） ReferenceLineProvider( const common::VehicleStateProvider* vehicle_state_provider, const ReferenceLineConfig* reference_line_config, @@ -80,25 +84,34 @@ class ReferenceLineProvider { bool UpdatePlanningCommand(const planning::PlanningCommand& command); void UpdateVehicleState(const common::VehicleState& vehicle_state); - + + // 启动参考线提供器（如果 FLAGS_enable_reference_line_provider_thread 为 true，则会创建一个线程） bool Start(); - + + // 停止参考线提供器 void Stop(); - + + // 重置参考线提供器 void Reset(); - + + // 获取参考线路和路由段 bool GetReferenceLines(std::list* reference_lines, std::list* segments); - + + // 获取上次计算的时间延迟 double LastTimeDelay(); - + + // 获取未来的路由航点 std::vector FutureRouteWaypoints(); - + + // 检查参考线是否更新 bool UpdatedReferenceLine() { return is_reference_line_updated_.load(); } - + + // 获取终点车道信息 void GetEndLaneWayPoint( std::shared_ptr& end_point) const; - + + // 根据车道 ID 获取车道信息 hdmap::LaneInfoConstPtr GetLaneById(const hdmap::Id& id) const; private: @@ -116,12 +129,16 @@ class ReferenceLineProvider { * @brief store the computed reference line. This function can avoid * unnecessary copy if the reference lines are the same. */ + // 更新参考线（如果数据未发生变化，则避免不必要的拷贝） void UpdateReferenceLine( const std::list& reference_lines, const std::list& route_segments); - + + // 生成参考线的后台线程 void GenerateThread(); + // 检查参考线是否有效 void IsValidReferenceLine(); + // 优先处理换道 void PrioritizeChangeLane(std::list* route_segments); bool CreateRouteSegments(const common::VehicleState& vehicle_state, @@ -173,14 +190,18 @@ class ReferenceLineProvider { private: bool is_initialized_ = false; std::atomic is_stop_{false}; - + + // 参考线平滑器 std::unique_ptr smoother_; + // 平滑器配置 ReferenceLineSmootherConfig smoother_config_; std::mutex pnc_map_mutex_; // The loaded pnc map plugin which can create referene line from // PlanningCommand. + // 多个 PncMapBase 实例 std::vector> pnc_map_list_; + // 当前使用的 PncMapBase std::shared_ptr current_pnc_map_; // Used in Navigation mode @@ -199,13 +220,18 @@ class ReferenceLineProvider { std::list route_segments_; double last_calculation_time_ = 0.0; + // 存储历史参考线 std::queue> reference_line_history_; + // 存储历史路由段 std::queue> route_segments_history_; - + + // task_future_：用于异步任务执行 std::future task_future_; - + + // is_reference_line_updated_：标志参考线是否已更新 std::atomic is_reference_line_updated_{true}; - + + // vehicle_state_provider_：车辆状态提供器指针 const common::VehicleStateProvider* vehicle_state_provider_ = nullptr; }; diff --git a/modules/planning/planning_base/reference_line/reference_point.cc b/modules/planning/planning_base/reference_line/reference_point.cc index bfa451f6b6f..cf1396fbf85 100644 --- a/modules/planning/planning_base/reference_line/reference_point.cc +++ b/modules/planning/planning_base/reference_line/reference_point.cc @@ -33,7 +33,7 @@ const double kDuplicatedPointsEpsilon = 1e-7; ReferencePoint::ReferencePoint(const MapPathPoint& map_path_point, const double kappa, const double dkappa) - : hdmap::MapPathPoint(map_path_point), kappa_(kappa), dkappa_(dkappa) {} + : hdmap::MapPathPoint(map_path_point), kappa_(kappa), dkappa_(dkappa) {} // 调用拷贝 common::PathPoint ReferencePoint::ToPathPoint(double s) const { return common::util::PointFactory::ToPathPoint(x(), y(), 0.0, s, heading(), diff --git a/modules/planning/planning_component/on_lane_planning.cc b/modules/planning/planning_component/on_lane_planning.cc index 25d3929775b..e7bfdc183d0 100644 --- a/modules/planning/planning_component/on_lane_planning.cc +++ b/modules/planning/planning_component/on_lane_planning.cc @@ -16,34 +16,59 @@ #include "modules/planning/planning_component/on_lane_planning.h" -#include -#include -#include -#include - +#include // 如 std::sort, std::find 等，用于轨迹点排序、参考线选择等 +#include // 获取数值类型极值（如 std::numeric_limits::max()），常用于初始化最小/最大距离 +#include // 可能用于存储历史轨迹或障碍物列表（虽然 Apollo 更常用 vector） +#include // 提供 std::pair, std::move 等，用于高效数据传递（如移动语义） +// 作用：允许 Google Test 框架访问 OnLanePlanning 的 私有成员函数 +// 机制：通过 FRIEND_TEST 宏，测试代码可调用内部逻辑进行白盒测试 +// 注意：此头文件 不影响运行时行为，仅在编译测试时生效 #include "gtest/gtest_prod.h" - +// 作用：使用 absl::StrCat() 高效拼接字符串（比 std::string + 更快、更安全） #include "absl/strings/str_cat.h" - +// 包含调试用的内部数据结构（如 ST 图、SL 图、参考线信息） #include "modules/common_msgs/planning_msgs/planning_internal.pb.h" +// 接收来自 Routing 模块的导航路径（RoutingResponse），是规划的起点 #include "modules/common_msgs/routing_msgs/routing.pb.h" +// 定义语义地图相关配置（如路口放大区域、虚拟车道等） #include "modules/planning/planning_base/proto/planning_semantic_map_config.pb.h" - +// Cyber RT 基础设施（Apollo 的中间件 +// 文件操作（如读取配置文件、保存调试数据） #include "cyber/common/file.h" +// 定义 AINFO, AWARN, AERROR, ADEBUG 等日志宏 #include "cyber/common/log.h" +// 高精度时间获取（Clock::Now()），用于时间戳对齐、性能分析 #include "cyber/time/clock.h" +// 提供四元数运算，用于处理车辆姿态（从定位消息中提取 heading） #include "modules/common/math/quaternion.h" +// 封装车辆状态（位置、速度、加速度、航向、角速度等） #include "modules/common/vehicle_state/vehicle_state_provider.h" +// 提供高精地图查询工具 +// 根据 GPS 坐标查找最近车道 +// 获取车道边界、限速、交通灯关联关系 +// 判断是否在交叉口、匝道等 #include "modules/map/hdmap/hdmap_util.h" +// 规划核心公共组件 +// 封装自车（Ego Vehicle）相关信息，如前方净空距离、是否在路口等 #include "modules/planning/planning_base/common/ego_info.h" +// 管理历史帧缓存（FrameHistory），用于轨迹平滑、预测等 #include "modules/planning/planning_base/common/history.h" +// 全局规划上下文，存储跨帧状态（如变道意图、红灯等待状态） #include "modules/planning/planning_base/common/planning_context.h" +// 实现轨迹“缝合”逻辑，保证新旧轨迹平滑衔接（避免跳变） #include "modules/planning/planning_base/common/trajectory_stitcher.h" +// 通用工具函数，如坐标转换、插值、角度归一化等 #include "modules/planning/planning_base/common/util/util.h" #include "modules/planning/planning_base/gflags/planning_gflags.h" #include "modules/planning/planning_base/learning_based/img_feature_renderer/birdview_img_feature_renderer.h" +// 动态生成参考线（Reference Line），是 Frenet 坐标系规划的基础 +// 支持多参考线（如直行、变道）并行评估 #include "modules/planning/planning_base/reference_line/reference_line_provider.h" +// 定义规划器抽象接口（Planner），具体实现如 PublicRoadPlanner, LatticePlanner 等 +// 采用 策略模式（Strategy Pattern），支持插件化替换 #include "modules/planning/planning_interface_base/planner_base/planner.h" +// 应用交通规则（Traffic Rules）对每条参考线进行可行性判断 +// 包括：红绿灯、让行、限速、行人避让等子规则 #include "modules/planning/planning_interface_base/traffic_rules_base/traffic_decider.h" namespace apollo { @@ -62,13 +87,19 @@ using apollo::hdmap::HDMapUtil; using apollo::planning_internal::SLFrameDebug; using apollo::planning_internal::SpeedPlan; using apollo::planning_internal::STGraphDebug; +/// @brief 自动设置 Dreamview 图表的 X/Y 轴范围和标签 +/// @param chart 图表指针 +/// @param label_name_x X轴标签名 +/// @param label_name_y Y轴标签名 void SetChartminmax(apollo::dreamview::Chart* chart, std::string label_name_x, std::string label_name_y) { auto* options = chart->mutable_options(); + // 初始化极值变量，用于遍历所有数据点后确定坐标轴范围 double xmin(std::numeric_limits::max()), xmax(std::numeric_limits::lowest()), ymin(std::numeric_limits::max()), ymax(std::numeric_limits::lowest()); + // 遍历图表中所有折线（line）的所有点（point），更新 X/Y 的最小/最大值 for (int i = 0; i < chart->line_size(); i++) { auto* line = chart->mutable_line(i); for (auto& pt : line->point()) { @@ -77,6 +108,8 @@ void SetChartminmax(apollo::dreamview::Chart* chart, std::string label_name_x, xmax = std::max(xmax, pt.x()); ymax = std::max(ymax, pt.y()); } + // 设置折线样式：线宽 2px、无点标记、直线连接（无线条张力）、不填充区域、显示线条 + // 这些是 Chart.js（Dreamview 前端图表库）的配置项 auto* properties = line->mutable_properties(); (*properties)["borderWidth"] = "2"; (*properties)["pointRadius"] = "0"; @@ -84,6 +117,7 @@ void SetChartminmax(apollo::dreamview::Chart* chart, std::string label_name_x, (*properties)["fill"] = "false"; (*properties)["showLine"] = "true"; } + // 将计算出的范围和标签应用到图表的 X/Y 轴 options->mutable_x()->set_min(xmin); options->mutable_x()->set_max(xmax); options->mutable_x()->set_label_string(label_name_x); @@ -92,11 +126,13 @@ void SetChartminmax(apollo::dreamview::Chart* chart, std::string label_name_x, options->mutable_y()->set_label_string(label_name_y); // Set chartJS's dataset properties } +/// @brief 释放资源，停止后台线程，清空状态 OnLanePlanning::~OnLanePlanning() { if (reference_line_provider_) { reference_line_provider_->Stop(); } planner_->Stop(); + // 清空历史缓存、规划上下文、自车信息等，防止内存泄漏或状态残留 injector_->frame_history()->Clear(); injector_->history()->Clear(); injector_->planning_context()->mutable_planning_status()->Clear(); @@ -105,42 +141,68 @@ OnLanePlanning::~OnLanePlanning() { } std::string OnLanePlanning::Name() const { return "on_lane_planning"; } - +/// @brief 分配具体的Planner，启动参考线提供器(reference_line_provider_) +/// @param config +/// @return +/* +1.检查 PlanningConfig，确保配置正确。 +2.清理历史数据，避免干扰。 +3.加载高精地图，为规划提供地图支持。 +4.初始化 ReferenceLineProvider，计算参考线。 +5.创建 planner_（路径规划器），加载不同的规划算法。 +6.启用强化学习（如果配置允许），用于训练自动驾驶模型。 +7.初始化 traffic_decider_，处理交通规则。 +8.记录启动时间，初始化 planner_ +*/ +// planning_config.pb.txt Status OnLanePlanning::Init(const PlanningConfig& config) { if (!CheckPlanningConfig(config)) { return Status(ErrorCode::PLANNING_ERROR, - "planning config error: " + config.DebugString()); + "planning config error: " + config_.DebugString()); } - - PlanningBase::Init(config); + // 调用父类的Init()函数 + // 调用基类初始化，并清空前序状态 + PlanningBase::Init(config_); // 会将config_赋值给PlanningBase中的成员变量 // clear planning history + // 清除先前的规划历史数据 + // OnLanePlanning公有继承PlanningBase， 且injecteor_是PlanningBase的protocted成员变量 injector_->history()->Clear(); - +// // clear planning status + // 清除当前的规划状态 injector_->planning_context()->mutable_planning_status()->Clear(); // load map + // 使用 HDMapUtil::BaseMapPtr() 获取地图指针，并检查地图是否成功加载。如果地图加载失败，会触发断言并输出错误信息 + // 加载高精地图，失败则终止（ACHECK 是 fatal check） hdmap_ = HDMapUtil::BaseMapPtr(); ACHECK(hdmap_) << "Failed to load map"; // instantiate reference line provider + // 创建 ReferenceLineProvider 实例并启动它，提供参考路线。如果配置中包含参考路线配置，将其传递给提供者 + // 创建并启动参考线提供器（内部可能有独立线程预计算参考线） const ReferenceLineConfig* reference_line_config = nullptr; if (config_.has_reference_line_config()) { reference_line_config = &config_.reference_line_config(); } reference_line_provider_ = std::make_unique( injector_->vehicle_state(), reference_line_config); + // 调用 Start() 开始提供参考线数据 reference_line_provider_->Start(); - + + // 加载路径规划器并检查其是否成功初始化。如果没有成功初始化，返回错误状态 // dispatch planner - LoadPlanner(); + // 加载规划器 + // 根据配置加载具体 Planner（如 PublicRoadPlanner） + LoadPlanner(); // 在父类中定义，选择EM 或者 Lattice if (!planner_) { return Status( ErrorCode::PLANNING_ERROR, "planning is not initialized with config : " + config_.DebugString()); } - + // 如果启用了学习模式，加载并初始化图像特征渲染器配置。如果加载失败，输出错误信息 + // 若启用学习模式，则初始化图像特征渲染器 if (config_.learning_mode() != PlanningConfig::NO_LEARNING) { PlanningSemanticMapConfig renderer_config; ACHECK(apollo::cyber::common::GetProtoFromFile( @@ -151,34 +213,54 @@ Status OnLanePlanning::Init(const PlanningConfig& config) { BirdviewImgFeatureRenderer::Instance()->Init(renderer_config); } - + // 初始化交通规则决策器，并调用 Planner 的 Init 完成最终初始化 + // 初始化交通决策模块 + // traffic_decider_ 负责处理交通信号灯、行人、让行规则等 traffic_decider_.Init(injector_); - + // 获取当前时间并保存为 start_time_，以记录路径规划的开始时间 start_time_ = Clock::NowInSeconds(); - return planner_->Init(injector_, FLAGS_planner_config_path); + // 初始化路径规划器，并使用配置路径 FLAGS_planner_config_path 来进行初始化 + // 从LoadPlanner中得到的 + return planner_->Init(injector_, FLAGS_planner_config_path); // public_road_planner_config.pb.txt } - +/// @brief +/// @param sequence_num 标识这一帧的序列号 +/// @param planning_start_point 表示路径规划开始的点 +/// @param vehicle_state 当前车辆的状态，包括位置、速度、加速度等信息 +/// @return Status OnLanePlanning::InitFrame(const uint32_t sequence_num, const TrajectoryPoint& planning_start_point, const VehicleState& vehicle_state) { + // planning_base.h: std::unique_ptr frame_; ?????? + // reset 方法确保 frame_ 的指针是有效的 + // 创建新的 Frame 对象，封装当前帧所有上下文（车辆状态、参考线、障碍物等） frame_.reset(new Frame(sequence_num, local_view_, planning_start_point, vehicle_state, reference_line_provider_.get())); - + if (frame_ == nullptr) { return Status(ErrorCode::PLANNING_ERROR, "Fail to init frame: nullptr."); } + // reference_lines 用来存储参考线 std::list reference_lines; + // segments 用来存储路段信息 std::list segments; + // 由routing给出的route_segments信息生成reference_lines + // 会填充这两个列表，获取当前的参考线和路段 reference_line_provider_->GetReferenceLines(&reference_lines, &segments); + // 验证参考线和路段的数量一致 DCHECK_EQ(reference_lines.size(), segments.size()); - + + // 静态方法，用于计算车辆基于当前速度的前向可视距离 auto forward_limit = planning::PncMapBase::LookForwardDistance( vehicle_state.linear_velocity()); - + + // 遍历所有参考线，调用 ref_line.Segment 方法进行收缩。收缩操作是根据车辆当前位置和前向/后向限制来进行的 + // planning::FLAGS_look_backward_distance：后向可视距离 + // forward_limit：前向可视距离 for (auto& ref_line : reference_lines) { if (!ref_line.Segment(Vec2d(vehicle_state.x(), vehicle_state.y()), - planning::FLAGS_look_backward_distance, + planning::FLAGS_look_backward_distance, // 50 forward_limit)) { const std::string msg = "Fail to shrink reference line."; AERROR << msg; @@ -190,6 +272,7 @@ Status OnLanePlanning::InitFrame(const uint32_t sequence_num, } ref_print_curve.PrintToLog(); } + // 遍历所有路段并对每个路段调用 seg.Shrink 方法进行收缩，确保它们在车辆当前状态下是有效的。如果收缩失败，则返回错误 for (auto& seg : segments) { if (!seg.Shrink(Vec2d(vehicle_state.x(), vehicle_state.y()), planning::FLAGS_look_backward_distance, forward_limit)) { @@ -207,6 +290,12 @@ Status OnLanePlanning::InitFrame(const uint32_t sequence_num, segment_print_curve.PrintToLog(); } + // 初始化帧 + // injector_->vehicle_state()：注入的车辆状态 + // reference_lines：参考线列表 + // segments：路段列表 + // reference_line_provider_->FutureRouteWaypoints()：未来的路线点 + // injector_->ego_info()：车辆的自我信息 auto status = frame_->Init( injector_->vehicle_state(), reference_lines, segments, reference_line_provider_->FutureRouteWaypoints(), injector_->ego_info()); @@ -219,55 +308,85 @@ Status OnLanePlanning::InitFrame(const uint32_t sequence_num, // TODO(all): fix this! this will cause unexpected behavior from controller void OnLanePlanning::GenerateStopTrajectory(ADCTrajectory* ptr_trajectory_pb) { + // 确保在生成新轨迹点之前，轨迹数组为空，以避免附加到现有的轨迹点数据上 ptr_trajectory_pb->clear_trajectory_point(); const auto& vehicle_state = injector_->vehicle_state()->vehicle_state(); - const double max_t = FLAGS_fallback_total_time; - const double unit_t = FLAGS_fallback_time_unit; + // apollo/modules/planning/planning_base/gflags/planning_gflags.cc + // 轨迹时间范围（如 3 秒），表示这个轨迹要持续多久 + const double max_t = FLAGS_fallback_total_time; // 3 + // 轨迹时间步长（如 0.1 秒），表示每个轨迹点的间隔 + const double unit_t = FLAGS_fallback_time_unit; // 0.1 TrajectoryPoint tp; - auto* path_point = tp.mutable_path_point(); + auto* path_point = tp.mutable_path_point(); // 返回的是一个指向 path_point 的指针，允许我们修改轨迹点的位置信息 path_point->set_x(vehicle_state.x()); path_point->set_y(vehicle_state.y()); path_point->set_theta(vehicle_state.heading()); - path_point->set_s(0.0); + path_point->set_s(0.0); // 轨迹点的初始位置 tp.set_v(0.0); tp.set_a(0.0); + // 未来3s内，每个0.1s取一个轨迹点，这些点的信息和tp一样 for (double t = 0.0; t < max_t; t += unit_t) { tp.set_relative_time(t); auto next_point = ptr_trajectory_pb->add_trajectory_point(); next_point->CopyFrom(tp); } } - +/* +private：只有父类自己可以访问，子类不能访问。 +protected：父类自己可以访问，子类也可以访问，但外部无法访问。 +public：所有地方都可以访问 +*/ +// 这部分主要做以下工作： +/* +1.更新传感器数据 +2.检查车辆状态 +3.更新参考线 +4.轨迹拼接 +5.生成规划轨迹 +6.轨迹平滑 & 输出 +*/ void OnLanePlanning::RunOnce(const LocalView& local_view, ADCTrajectory* const ptr_trajectory_pb) { + // 1.0 // when rerouting, reference line might not be updated. In this case, planning // module maintains not-ready until be restarted. local_view_ = local_view; - const double start_timestamp = Clock::NowInSeconds(); + // 获取路径规划起始时间戳 s + // 受 ROS 2 use_sim_time 影响，如果 use_sim_time:=true，那么 Clock::NowInSeconds() 取的是 仿真时间 而不是系统时间 + const double start_timestamp = Clock::NowInSeconds(); // start_timestamp 用来计算轨迹的时间戳，确保和 ROS 2 其他模块同步 + // 路径规划算法耗时起始时间 s const double start_system_timestamp = std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()) - .count(); + .count(); // start_system_timestamp 用来计算 规划耗时（单位：秒），避免受 ROS 2 use_sim_time 影响 +// 2.0 // localization ADEBUG << "Get localization:" << local_view_.localization_estimate->DebugString(); // chassis ADEBUG << "Get chassis:" << local_view_.chassis->DebugString(); - + // 更新车辆状态---> 进入Update函数查看 + // apollo/modules/common/vehicle_state/vehicle_state_provoder.cc + // 融合定位（位置/姿态）和底盘（速度/档位）数据，生成统一的 VehicleState Status status = injector_->vehicle_state()->Update( *local_view_.localization_estimate, *local_view_.chassis); - + // 获取车辆状态 + // 获取并检查车辆状态的时间戳是否有效，确保 start_timestamp 大于等于车辆状态时间戳 VehicleState vehicle_state = injector_->vehicle_state()->vehicle_state(); const double vehicle_state_timestamp = vehicle_state.timestamp(); + // 确保 start_timestamp 不会早于 vehicle_state_timestamp，并在不符合要求时打印出详细的错误信息 + // 检查时间合理性：规划开始时间不应早于车辆状态时间（否则说明时钟异常） DCHECK_GE(start_timestamp, vehicle_state_timestamp) << "start_timestamp is behind vehicle_state_timestamp by " << start_timestamp - vehicle_state_timestamp << " secs"; - +// 如果车辆状态无效，生成刹车轨迹 +// status.ok()： Status status if (!status.ok() || !util::IsVehicleStateValid(vehicle_state)) { + // 车辆状态无效 → 输出 NotReady + 停车轨迹 const std::string msg = "Update VehicleStateProvider failed " "or the vehicle state is out dated."; @@ -283,39 +402,62 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, GenerateStopTrajectory(ptr_trajectory_pb); return; } - + // 检查系统时间是否滞后于GPS时间，并输出警告 if (start_timestamp + 1e-6 < vehicle_state_timestamp) { common::monitor::MonitorLogBuffer monitor_logger_buffer( common::monitor::MonitorMessageItem::PLANNING); monitor_logger_buffer.ERROR("ego system time is behind GPS time"); } - +// 对齐时间戳 vehicle_state_timestamp为车辆状态时间戳 +// 如果时间差小于某个阈值，对齐车辆状态的时间戳 +// 大于它就完犊子了 +// 时间对齐：若延迟小（如 < 100ms），用运动学模型预测车辆在 start_timestamp 时刻的状态，提升规划精度 if (start_timestamp - vehicle_state_timestamp < - FLAGS_message_latency_threshold) { + FLAGS_message_latency_threshold) { // 0.02s 消息延时阈值 vehicle_state = AlignTimeStamp(vehicle_state, start_timestamp); } +// 参考线更新车辆状态 // Update reference line provider and reset scenario if new routing - reference_line_provider_->UpdateVehicleState(vehicle_state); + reference_line_provider_->UpdateVehicleState(vehicle_state); // 车辆状态单纯的赋值 + // // 如果是两个不同的routing,则重新初始化 + // 当收到新的导航指令（如重新规划路线）时 if (local_view_.planning_command->is_motion_command() && util::IsDifferentRouting(last_command_, *local_view_.planning_command)) { + // 如果发现新的规划命令，更新 last_command_ 为当前的规划命令 last_command_ = *local_view_.planning_command; AINFO << "new_command:" << last_command_.DebugString(); + // 重置参考路线提供者，即清除当前的参考路线信息 + // 重置参考线提供器（ReferenceLineProvider） reference_line_provider_->Reset(); + // 通过 injector_ 调用 history() 获取历史数据的对象，并清除历史记录（Clear()）。这意味着在接收到新命令后，之前的历史数据（如轨迹、状态等）可能已经不再适用 + // 清空历史缓存（FrameHistory）和规划上下文（PlanningContext） injector_->history()->Clear(); +// 通过 injector_ 获取规划上下文（planning_context()）并清除其中的规划状态（Clear()）。这表明与当前规划相关的状态信息也需要重置，可能是为了清除过时的规划状态 injector_->planning_context()->mutable_planning_status()->Clear(); reference_line_provider_->UpdatePlanningCommand( *(local_view_.planning_command)); +// 重置规划器，并使用 frame_ 中的相关信息来重新初始化或更新规划器的状态 +// 通知 Planner 重置状态（如取消正在进行的变道） planner_->Reset(frame_.get()); } // Get end lane way point. + // 获取当前车道的结束点 + // 更新终点车道信息（用于泊车或终点减速） reference_line_provider_->GetEndLaneWayPoint(local_view_.end_lane_way_point); // planning is triggered by prediction data, but we can still use an estimated // cycle time for stitching + // 3.0 + // 进行轨迹拼接 +// 在apollo规划算法中，在每一帧规划开始时会调用一个轨迹拼接函数，返回一段拼接轨迹点集 +// 并告诉我们是否重新规划以及重规划的原因 +// 将上一帧发布的轨迹末尾与当前车辆状态拼接，保证轨迹连续性 const double planning_cycle_time = - 1.0 / static_cast(FLAGS_planning_loop_rate); + 1.0 / static_cast(FLAGS_planning_loop_rate); // 10 hz +// 使用轨迹拼接器计算拼接后的轨迹 +// 轨迹拼接保留长度 std::string replan_reason; std::vector stitching_trajectory = TrajectoryStitcher::ComputeStitchingTrajectory( @@ -323,23 +465,36 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, planning_cycle_time, FLAGS_trajectory_stitching_preserved_length, true, last_publishable_trajectory_.get(), &replan_reason); +// 4.0 +// 更新ego信息，进入EgoInfo类查看私有成员变量释义，进入Update查看 +// 规划起点设置、车辆状态、包围盒 +// 更新自车信息（如前方净空距离），供后续决策使用 injector_->ego_info()->Update(stitching_trajectory.back(), vehicle_state); const uint32_t frame_num = static_cast(seq_num_++); AINFO << "Planning start frame sequence id = [" << frame_num << "]"; - status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state); + + + // 初始化frame,重要信息，将拼接轨迹最后一个点作为路径规划起点 + status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);// frame类存储了一次规划循环中所需的所有数据 180 + // 计算前方障碍物距离、当前路段信息（如是否在交叉口） if (status.ok()) { injector_->ego_info()->CalculateFrontObstacleClearDistance( frame_->obstacles()); + // injector_->ego_info()->CalculateCurrentRouteInfo( + // reference_line_provider_.get()); } +// 如果启用了调试记录，则记录调试信息 if (FLAGS_enable_record_debug) { frame_->RecordInputDebug(ptr_trajectory_pb->mutable_debug()); } + // 记录 Frame 初始化耗时 ptr_trajectory_pb->mutable_latency_stats()->set_init_frame_time_ms( Clock::NowInSeconds() - start_timestamp); if (!status.ok()) { AERROR << status.ToString(); + // ESTOP：最高优先级，控制模块会立即刹车 if (FLAGS_publish_estop) { // "estop" signal check in function "Control::ProduceControlCommand()" // estop_ = estop_ || local_view_.trajectory.estop().is_estop(); @@ -351,6 +506,7 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, status.Save(estop_trajectory.mutable_header()->mutable_status()); ptr_trajectory_pb->CopyFrom(estop_trajectory); } else { + // NotReady + Stop Trajectory：温和降级，缓慢停车 ptr_trajectory_pb->mutable_decision() ->mutable_main_decision() ->mutable_not_ready() @@ -362,11 +518,12 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, ptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE); FillPlanningPb(start_timestamp, ptr_trajectory_pb); frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb); + // 结束当前帧的规划并将帧添加到历史中 const uint32_t n = frame_->SequenceNum(); injector_->frame_history()->Add(n, std::move(frame_)); return; } - + // 交通规则决策 for (auto& ref_line_info : *frame_->mutable_reference_line_info()) { auto traffic_status = traffic_decider_.Execute(frame_.get(), &ref_line_info); @@ -376,10 +533,11 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, << " traffic decider failed"; } } - + // 开始正在的规划 planner 开始规划 status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb); // print trajxy + // 打印轨迹 XY 曲线 PrintCurves trajectory_print_curve; for (const auto& p : ptr_trajectory_pb->trajectory_point()) { trajectory_print_curve.AddPoint("trajxy", p.path_point().x(), @@ -388,15 +546,19 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, trajectory_print_curve.PrintToLog(); // print obstacle polygon + // 打印障碍物多边形 for (const auto& obstacle : frame_->obstacles()) { obstacle->PrintPolygonCurve(); } // print ego box + // 打印自车包围盒 PrintBox print_box("ego_box"); print_box.AddAdcBox(vehicle_state.x(), vehicle_state.y(), vehicle_state.heading(), true); print_box.PrintToLog(); + // 计算总的规划时间并输出 + // 记录总耗时（通常要求 < 100ms） const auto end_system_timestamp = std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()) @@ -408,7 +570,8 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, ptr_trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms); ADEBUG << "Planning latency: " << ptr_trajectory_pb->latency_stats().DebugString(); - + + // 如果规划失败，输出错误信息并根据设置发布 estop 信号 if (!status.ok()) { status.Save(ptr_trajectory_pb->mutable_header()->mutable_status()); AERROR << "Planning failed:" << status.ToString(); @@ -423,25 +586,30 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, } } +// 若轨迹缝合失败（只保留一个点），说明是重新规划，记录原因 ptr_trajectory_pb->set_is_replan(stitching_trajectory.size() == 1); if (ptr_trajectory_pb->is_replan()) { ptr_trajectory_pb->set_replan_reason(replan_reason); } +// 根据是否在开放空间轨迹上，填充并保存规划结果 if (frame_->open_space_info().is_on_open_space_trajectory()) { + // 泊车模式：直接输出 FillPlanningPb(start_timestamp, ptr_trajectory_pb); ADEBUG << "Planning pb:" << ptr_trajectory_pb->header().DebugString(); frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb); } else { + // 处理不同场景的规划结果 + // 主干道模式：可能进行轨迹平滑 auto* ref_line_task = ptr_trajectory_pb->mutable_latency_stats()->add_task_stats(); ref_line_task->set_time_ms(reference_line_provider_->LastTimeDelay() * 1000.0); ref_line_task->set_name("ReferenceLineProvider"); - + // 填充header信息 FillPlanningPb(start_timestamp, ptr_trajectory_pb); ADEBUG << "Planning pb:" << ptr_trajectory_pb->header().DebugString(); - + // 将当前帧加入历史缓存，供下一帧缝合使用 frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb); if (FLAGS_enable_planning_smoother) { planning_smoother_.Smooth(injector_->frame_history(), frame_.get(), @@ -458,6 +626,7 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, AINFO << "Planning Perf: planning name [" << Name() << "], " << plnning_perf_ms << " ms."; AINFO << "Planning end frame sequence id = [" << frame_num << "]"; + injector_->frame_history()->Add(frame_num, std::move(frame_)); } @@ -537,6 +706,7 @@ Status OnLanePlanning::Plan( const double current_time_stamp, const std::vector& stitching_trajectory, ADCTrajectory* const ptr_trajectory_pb) { + // 记录调试起点（Init Point） auto* ptr_debug = ptr_trajectory_pb->mutable_debug(); if (FLAGS_enable_record_debug) { ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom( @@ -557,10 +727,12 @@ Status OnLanePlanning::Plan( frame_->open_space_info().publishable_trajectory_data().first; const auto& publishable_trajectory_gear = frame_->open_space_info().publishable_trajectory_data().second; + // 直接使用 OpenSpace 生成的轨迹 publishable_trajectory.PopulateTrajectoryProtobuf(ptr_trajectory_pb); ptr_trajectory_pb->set_gear(publishable_trajectory_gear); ptr_trajectory_pb->set_trajectory_type(ADCTrajectory::OPEN_SPACE); // TODO(QiL): refine engage advice in open space trajectory optimizer. + // 设置 Engage Advice（接管建议） auto* engage_advice = ptr_trajectory_pb->mutable_engage_advice(); // enable start auto from open_space planner. @@ -574,11 +746,12 @@ Status OnLanePlanning::Plan( engage_advice->set_reason("Keep engage while in parking"); } // TODO(QiL): refine the export decision in open space info + // 设置决策状态 ptr_trajectory_pb->mutable_decision() ->mutable_main_decision() ->mutable_parking() ->set_status(MainParking::IN_PARKING); - + // 记录 Debug & 导出图表 if (FLAGS_enable_record_debug) { // ptr_debug->MergeFrom(frame_->open_space_info().debug_instance()); frame_->mutable_open_space_info()->RecordDebug(ptr_debug); @@ -589,11 +762,13 @@ Status OnLanePlanning::Plan( ptr_debug); } } else { + // On-Lane 规划（正常行驶） const auto* best_ref_info = frame_->FindDriveReferenceLineInfo(); const auto* target_ref_info = frame_->FindTargetReferenceLineInfo(); if (!best_ref_info) { const std::string msg = "planner failed to make a driving plan"; AERROR << msg; + // 参考线空,清空last_publishable_trajectory_ if (last_publishable_trajectory_) { last_publishable_trajectory_->Clear(); } @@ -601,13 +776,17 @@ Status OnLanePlanning::Plan( } // Store current frame stitched path for possible speed fallback in next // frames + // 构建 current_frame_planned_path（用于速度回退） DiscretizedPath current_frame_planned_path; + // 先加入 stitching_trajectory 的所有 path_point for (const auto& trajectory_point : stitching_trajectory) { current_frame_planned_path.push_back(trajectory_point.path_point()); } const auto& best_ref_path = best_ref_info->path_data().discretized_path(); + // 再追加 best_ref_info 规划的新路径（跳过第一个点，避免重复） std::copy(best_ref_path.begin() + 1, best_ref_path.end(), std::back_inserter(current_frame_planned_path)); + // Speed Fallback 机制: 如果下一帧速度规划失败，可沿用本帧的路径，只重新规划速度 frame_->set_current_frame_planned_path(current_frame_planned_path); ptr_debug->MergeFrom(best_ref_info->debug()); @@ -621,12 +800,14 @@ Status OnLanePlanning::Plan( ExportFailedLaneChangeSTChart(failed_ref_info->debug(), ptr_debug); } } + // 各子模块耗时（ST图生成、DP、QP等） ptr_trajectory_pb->mutable_latency_stats()->MergeFrom( best_ref_info->latency_stats()); // set right of way status + // 是否拥有路权（如通过无保护左转） ptr_trajectory_pb->set_right_of_way_status( best_ref_info->GetRightOfWayStatus()); - + // 当前车道 & 目标车道 ID for (const auto& id : best_ref_info->TargetLaneId()) { ptr_trajectory_pb->add_lane_id()->CopyFrom(id); } @@ -636,11 +817,11 @@ Status OnLanePlanning::Plan( } ptr_trajectory_pb->set_trajectory_type(best_ref_info->trajectory_type()); - + // Responsibility-Sensitive Safety 信息 if (FLAGS_enable_rss_info) { *ptr_trajectory_pb->mutable_rss_info() = best_ref_info->rss_info(); } - + // 包含 yield、overtake、stop 等决策 best_ref_info->ExportDecision(ptr_trajectory_pb->mutable_decision(), injector_->planning_context()); @@ -674,7 +855,7 @@ Status OnLanePlanning::Plan( prev_y = reference_point.y(); } } - + // (1) 创建新轨迹（仅含新规划部分） last_publishable_trajectory_.reset(new PublishableTrajectory( current_time_stamp, best_ref_info->trajectory())); PrintCurves debug_traj; @@ -687,13 +868,13 @@ Status OnLanePlanning::Plan( } // debug_traj.PrintToLog(); ADEBUG << "current_time_stamp: " << current_time_stamp; - + // (2) 前置拼接历史轨迹（stitching） last_publishable_trajectory_->PrependTrajectoryPoints( std::vector(stitching_trajectory.begin(), stitching_trajectory.end() - 1)); - + // 最终发布的轨迹 = 历史缝合点 + 新规划轨迹，保证连续性 last_publishable_trajectory_->PopulateTrajectoryProtobuf(ptr_trajectory_pb); - + // 设置 Engage Advice（接管建议） best_ref_info->ExportEngageAdvice( ptr_trajectory_pb->mutable_engage_advice(), injector_->planning_context()); @@ -746,6 +927,7 @@ void AddSTGraph(const STGraphDebug& st_graph, Chart* chart) { if (type == "DRIVABLE_REGION") { (*properties)["color"] = "\"rgba(0, 255, 0, 0.5)\""; + // (*properties)["color"] = "\"rgba(255, 145, 0, 0.5)\""; } else { (*properties)["color"] = "\"rgba(255, 0, 0, 0.8)\""; } @@ -1201,10 +1383,16 @@ void OnLanePlanning::AddPublishedSpeed(const ADCTrajectory& trajectory_pb, (*sliding_line_properties)["showLine"] = "true"; } +/// @brief 对齐车辆状态的时间戳，即根据时间差 (curr_timestamp - vehicle_state.timestamp()) +// 预测 curr_timestamp 时刻的车辆位置 (x, y)，然后返回更新后的 VehicleState +/// @param vehicle_state 当前的车辆状态 +/// @param curr_timestamp 目标时间戳，通常是规划器希望对齐的轨迹起点时间 +/// @return VehicleState OnLanePlanning::AlignTimeStamp(const VehicleState& vehicle_state, const double curr_timestamp) const { // TODO(Jinyun): use the same method in trajectory stitching // for forward prediction + // 预测 time_delta 秒后 车辆的位置 auto future_xy = injector_->vehicle_state()->EstimateFuturePosition( curr_timestamp - vehicle_state.timestamp()); diff --git a/modules/planning/planning_component/on_lane_planning.h b/modules/planning/planning_component/on_lane_planning.h index 9c12dc2bccd..ace6ecfda16 100644 --- a/modules/planning/planning_component/on_lane_planning.h +++ b/modules/planning/planning_component/on_lane_planning.h @@ -40,63 +40,75 @@ class OnLanePlanning : public PlanningBase { public: explicit OnLanePlanning(const std::shared_ptr& injector) : PlanningBase(injector) {} - +// 虚析构函数，确保当通过基类指针删除派生类对象时，能够正确调用派生类的析构函数。 virtual ~OnLanePlanning(); /** * @brief Planning name. */ + // 返回规划模块的名称，覆盖基类中的同名函数，即"OnLanePlanning"" std::string Name() const override; /** * @brief module initialization function * @return initialization status */ + // 模块的初始化函数，接收配置并返回初始化状态 common::Status Init(const PlanningConfig& config) override; /** * @brief main logic of the planning module, runs periodically triggered by * timer. */ + // 主规划逻辑函数，周期性地被定时器触发，处理输入数据并生成路径 void RunOnce(const LocalView& local_view, ADCTrajectory* const ptr_trajectory_pb) override; + // 执行路径规划的主要函数，接受时间戳和路径点进行规划 common::Status Plan( const double current_time_stamp, const std::vector& stitching_trajectory, ADCTrajectory* const trajectory) override; private: + // 初始化规划帧，包括设置序列号、规划起点和车辆状态 common::Status InitFrame(const uint32_t sequence_num, const common::TrajectoryPoint& planning_start_point, const common::VehicleState& vehicle_state); - +// 对齐车辆状态的时间戳，确保与当前时间一致 common::VehicleState AlignTimeStamp(const common::VehicleState& vehicle_state, const double curr_timestamp) const; - +// 导出参考线的调试信息 void ExportReferenceLineDebug(planning_internal::Debug* debug); +// 检查规划配置的有效性 bool CheckPlanningConfig(const PlanningConfig& config); +// 生成停车轨迹 void GenerateStopTrajectory(ADCTrajectory* ptr_trajectory_pb); +// 导出车道变换失败的调试图表 void ExportFailedLaneChangeSTChart(const planning_internal::Debug& debug_info, planning_internal::Debug* debug_chart); +// 导出在车道上行驶的调试图表 void ExportOnLaneChart(const planning_internal::Debug& debug_info, planning_internal::Debug* debug_chart); +// 导出开放空间的调试图表 void ExportOpenSpaceChart(const planning_internal::Debug& debug_info, const ADCTrajectory& trajectory_pb, planning_internal::Debug* debug_chart); +// 向调试图表中添加开放空间优化结果 void AddOpenSpaceOptimizerResult(const planning_internal::Debug& debug_info, planning_internal::Debug* debug_chart); +// 向调试图表中添加分区轨迹 void AddPartitionedTrajectory(const planning_internal::Debug& debug_info, planning_internal::Debug* debug_chart); - +// 向调试图表中添加拼接速度曲线 void AddStitchSpeedProfile(planning_internal::Debug* debug_chart); - +// 向调试图表中添加发布的速度 void AddPublishedSpeed(const ADCTrajectory& trajectory_pb, planning_internal::Debug* debug_chart); - +// 向调试图表中添加发布的加速度 void AddPublishedAcceleration(const ADCTrajectory& trajectory_pb, planning_internal::Debug* debug); - +// 向调试图表中添加备用轨迹 void AddFallbackTrajectory(const planning_internal::Debug& debug_info, planning_internal::Debug* debug_chart); diff --git a/modules/planning/planning_component/planning_base.cc b/modules/planning/planning_component/planning_base.cc index 51436669f94..ae329ff87db 100644 --- a/modules/planning/planning_component/planning_base.cc +++ b/modules/planning/planning_component/planning_base.cc @@ -40,7 +40,7 @@ PlanningBase::PlanningBase(const std::shared_ptr& injector) : injector_(injector) {} PlanningBase::~PlanningBase() {} - +// 就是一个赋值 Status PlanningBase::Init(const PlanningConfig& config) { injector_->planning_context()->Init(); config_ = config; @@ -95,9 +95,14 @@ bool PlanningBase::IsPlanningFinished( } } +/// @brief +/// @param timestamp 当前规划周期的时间戳，用于同步不同模块的数据 +/// @param trajectory_pb void PlanningBase::FillPlanningPb(const double timestamp, ADCTrajectory* const trajectory_pb) { trajectory_pb->mutable_header()->set_timestamp_sec(timestamp); + // 将感知模块提供的激光雷达、摄像头、雷达时间戳同步到 trajectory_pb 中 + // 保证轨迹生成时，使用的感知数据是最新的，避免时间不匹配导致的问题 if (local_view_.prediction_obstacles->has_header()) { trajectory_pb->mutable_header()->set_lidar_timestamp( local_view_.prediction_obstacles->header().lidar_timestamp()); @@ -106,6 +111,11 @@ void PlanningBase::FillPlanningPb(const double timestamp, trajectory_pb->mutable_header()->set_radar_timestamp( local_view_.prediction_obstacles->header().radar_timestamp()); } + // 直接拷贝 planning_command 里的头部信息，确保轨迹和路径规划模块的最新指令对齐 + // planning_command 可能存储 + // 当前的全局路径 + // 目标点信息 + // 规划模式（如巡航、变道、紧急停车等） trajectory_pb->mutable_routing_header()->CopyFrom( local_view_.planning_command->header()); } @@ -116,7 +126,8 @@ void PlanningBase::LoadPlanner() { if ("" != config_.planner()) { planner_name = config_.planner(); planner_name = ConfigUtil::GetFullPlanningClassName(planner_name); - } + } // 单例模式 + // 使用 PluginManager 创建规划器实例 ？？？？？？？？？？？？？？？？？ planner_ = cyber::plugin_manager::PluginManager::Instance()->CreateInstance( planner_name); diff --git a/modules/planning/planning_component/planning_component.cc b/modules/planning/planning_component/planning_component.cc index ad7850c3d16..fbeb1ae070c 100644 --- a/modules/planning/planning_component/planning_component.cc +++ b/modules/planning/planning_component/planning_component.cc @@ -13,22 +13,36 @@ * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ -#include "modules/planning/planning_component/planning_component.h" - +#include "modules/planning/planning_component/planning_component.h" // 定义了这个组件的所有功能 +// 包含 Cyber 框架和 Apollo 公共模块的配置，用于处理 gflags 配置参数 #include "cyber/common/file.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/configs/config_gflags.h" +// 导入工具函数和 HDMap 相关功能，用于读取地图信息和处理消息 #include "modules/common/util/message_util.h" #include "modules/common/util/util.h" #include "modules/map/hdmap/hdmap_util.h" +// 包含规划模块的通用组件，如历史记录 (history)、规划上下文 (planning_context) #include "modules/planning/planning_base/common/history.h" #include "modules/planning/planning_base/common/planning_context.h" #include "modules/planning/planning_base/common/util/util.h" -#include "modules/planning/planning_component/navi_planning.h" -#include "modules/planning/planning_component/on_lane_planning.h" +// 包含两种规划模式的实现 +#include "modules/planning/planning_component/navi_planning.h" // 导航模式 +#include "modules/planning/planning_component/on_lane_planning.h" // 基于车道的规划模式 namespace apollo { namespace planning { +//planning 支持两种规划模式：OnLanePlanning: 基于高精地图的轨迹规划，也是默认的规划模式； +// NaviPlanning: 相对地图导航规划，主要用于交通规则较简单的高速公路 + +// 每种规划模式可以通过 PlannerDispatcher 选择使用的 Planner ，目前 planning 模块中共有 4 种 Planner +/* +apollo::planning::PublicRoadPlanner: 于高精地图的规划器； +apollo::planning::NaviPlanner: 于实时相对地图的规划器； +apollo::planning::LatticePlanner: 于网格算法的规划器 +apollo::planning::RTKReplayPlanner: 于录制轨迹的规划器 +*/ + using apollo::cyber::ComponentBase; using apollo::hdmap::HDMapUtil; using apollo::perception::TrafficLightDetection; @@ -36,30 +50,43 @@ using apollo::relative_map::MapMsg; using apollo::routing::RoutingRequest; using apollo::routing::RoutingResponse; using apollo::storytelling::Stories; - +/// @brief 初始化组件 +/// @return bool PlanningComponent::Init() { +// 依赖注入器，本质就是一个数据缓存中心，以便于规划任务前后帧之间的承接，以及异常处理的回溯 +// 创建 DependencyInjector 对象，用于依赖注入：用于在不同模块间共享状态（如规划上下文、历史轨迹等） injector_ = std::make_shared(); - - if (FLAGS_use_navigation_mode) { - planning_base_ = std::make_unique(injector_); +// 规划模式的选择: apollo/modules/common/configs/config_gflags.cc +// 创建PlanningBase对象（默认OnLanePlanning），它是轨迹规划的主体 + if (FLAGS_use_navigation_mode) { // 默认是false + planning_base_ = std::make_unique(injector_); // 相对地图规划器 } else { - planning_base_ = std::make_unique(injector_); + planning_base_ = std::make_unique(injector_); // 默认规划器 } - +// 加载config文件 proto +// ACHECK 检查配置文件是否成功加载，配置文件包含路径规划的相关参数 +// 配置文件 planning_config.pb.txt ？？？？？？？？？？？？？？？？？？？？？？？？ +// 从 .conf 或 .pb.txt 配置文件中加载 PlanningConfig ACHECK(ComponentBase::GetProtoConfig(&config_)) << "failed to load planning config file " << ComponentBase::ConfigFilePath(); - +// 消息处理初始化: +// 初始化消息处理模块，如果启用了离线学习或非零学习模式 if (FLAGS_planning_offline_learning || config_.learning_mode() != PlanningConfig::NO_LEARNING) { + // 初始化 MessageProcess 用于收集训练数据 if (!message_process_.Init(config_, injector_)) { AERROR << "failed to init MessageProcess"; return false; } } - - planning_base_->Init(config_); - +// 初始化规划子模块 +// 在这里执行的是OnLanePlanning::Init的初始化 + planning_base_->Init(config_); // 参考线优化主要在这部分 +// 订阅ROS2话题 +// 订阅 PlanningCommand 话题，收到消息后存入 planning_command_ +// 输入导航命令订阅 +// 来自 Routing 或人机交互的规划指令 planning_command_reader_ = node_->CreateReader( config_.topic_config().planning_command_topic(), [this](const std::shared_ptr& planning_command) { @@ -68,7 +95,8 @@ bool PlanningComponent::Init() { std::lock_guard lock(mutex_); planning_command_.CopyFrom(*planning_command); }); - +// 订阅交通信号灯检测数据，存入 traffic_light_ +// 交通灯状态 traffic_light_reader_ = node_->CreateReader( config_.topic_config().traffic_light_detection_topic(), [this](const std::shared_ptr& traffic_light) { @@ -76,7 +104,9 @@ bool PlanningComponent::Init() { std::lock_guard lock(mutex_); traffic_light_.CopyFrom(*traffic_light); }); - +// 订阅 PadMessage（手动输入指令） +// planning操作命令（start，stop）消息订阅 +// 车内物理按钮（如“清除规划”） pad_msg_reader_ = node_->CreateReader( config_.topic_config().planning_pad_topic(), [this](const std::shared_ptr& pad_msg) { @@ -84,7 +114,8 @@ bool PlanningComponent::Init() { std::lock_guard lock(mutex_); pad_msg_.CopyFrom(*pad_msg); }); - +// storytelling消息订阅 +// 场景描述（用于仿真或日志） story_telling_reader_ = node_->CreateReader( config_.topic_config().story_telling_topic(), [this](const std::shared_ptr& stories) { @@ -92,7 +123,9 @@ bool PlanningComponent::Init() { std::lock_guard lock(mutex_); stories_.CopyFrom(*stories); }); - +// 如果启用了导航模式，则订阅 relative_map（相对地图） +// 实时相对地图消息订阅（用于NaviPlanning） +// 仅导航模式使用，相对地图 if (FLAGS_use_navigation_mode) { relative_map_reader_ = node_->CreateReader( config_.topic_config().relative_map_topic(), @@ -102,73 +135,113 @@ bool PlanningComponent::Init() { relative_map_.CopyFrom(*map_message); }); } + // 创建消息的发布者（Writer） +// planning发布话题部分: 话题也是写在配置中 +// 发布规划路径给control模块 +// planning输出轨迹消息发布 planning_writer_ = node_->CreateWriter( config_.topic_config().planning_trajectory_topic()); - +// planning阻塞时需要重新路由的请求 服务客户端：用于在需要时主动发起 重新路径规划（Rerouting） 请求 rerouting_client_ = node_->CreateClient( config_.topic_config().routing_request_topic()); + // 发布学习用数据（RL/IL） planning_learning_data_writer_ = node_->CreateWriter( config_.topic_config().planning_learning_data_topic()); +// planning实时任务状态消息发布 +// 回传命令执行状态（RUNNING / FINISHED / ERROR） command_status_writer_ = node_->CreateWriter( FLAGS_planning_command_status); return true; } - +/// @brief 检查数据，并且执行注册好的Planning，生成路线并且发布 +/// @param prediction_obstacles 预测模块 +/// @param chassis 来自总线CanBus,主要是汽车底盘所给出的机械状态信息 +/// @param localization_estimate 定位信息 +/// @return bool PlanningComponent::Proc( - const std::shared_ptr& + const std::shared_ptr& // // modules/common_msgs/prediction_msgs/prediction_obstacle.proto prediction_obstacles, const std::shared_ptr& chassis, const std::shared_ptr& localization_estimate) { + // 确保预测障碍物数据 prediction_obstacles 非空，否则会触发 ACHECK 终止程序 ACHECK(prediction_obstacles != nullptr); // check and process possible rerouting request + // 1.检查是否需要重新规划线路 + // 如果路径不通或环境变化，可能需要重新规划路径，调用 CheckRerouting() 进行检查 + // 如果规划上下文中标记了 need_rerouting()，则发送 LaneFollowCommand 触发新路径规划 CheckRerouting(); // process fused input data + // 2.数据放入local_view_中，并且检查输入数据 + // std::mutex: 互斥锁，用于保护共享资源的访问 + // 当多个线程需要同时访问共享资源时，为了避免出现数据竞争等问题，需要使用互斥锁进行同步控制 + // 存储传感器数据，用于规划计算 + // 将所有输入（包括缓存的 pad、交通灯、故事等）整合成一个结构体 local_view_，供规划器使用 + // 对每个缓存变量都加锁读取，并检查是否为新消息（通过 IsProtoEqual 判断 header 是否变化） + + // 填充local_view_ local_view_.prediction_obstacles = prediction_obstacles; local_view_.chassis = chassis; local_view_.localization_estimate = localization_estimate; + + // yk: std::mutex mutex_ + // 处理 planning_command_（规划指令） { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); // 互斥锁 if (!local_view_.planning_command || !common::util::IsProtoEqual(local_view_.planning_command->header(), planning_command_.header())) { + // 如果不相同，说明规划指令有更新，拷贝到 local_view_.planning_command local_view_.planning_command = std::make_shared(planning_command_); } } + + // 存储交通灯检测数据 (traffic_light_) 和 相对地图信息 (relative_map_) { std::lock_guard lock(mutex_); local_view_.traffic_light = - std::make_shared(traffic_light_); + std::make_shared(traffic_light_); // 需要看一下 local_view_.relative_map = std::make_shared(relative_map_); } + + // 处理用户控制指令 { std::lock_guard lock(mutex_); if (!local_view_.pad_msg || !common::util::IsProtoEqual(local_view_.pad_msg->header(), pad_msg_.header())) { + + // 特殊情况： 如果 PadMessage 指令是 "CLEAR_PLANNING"，则清除 planning_command_ + // 清除当前规划命令，通常用于紧急停止或重置 // Check if "CLEAR_PLANNING" PadMessage is received and process. if (pad_msg_.action() == PadMessage::CLEAR_PLANNING) { local_view_.planning_command = nullptr; planning_command_.Clear(); } + // 如果 PadMessage 发生变化，则更新 local_view_.pad_msg local_view_.pad_msg = std::make_shared(pad_msg_); } } + { std::lock_guard lock(mutex_); local_view_.stories = std::make_shared(stories_); } - +// 3.0 +// 输入性检查，如果检查失败，则退出 if (!CheckInput()) { AINFO << "Input check failed"; return false; } +// 4.0 处理强化学习数据 +// 如果配置文件为深度学习模式，非主流方法，跳过 +// 如果当前处于 Learning Mode，将输入数据存入 message_process_，用于强化学习训练 if (config_.learning_mode() != PlanningConfig::NO_LEARNING) { // data process for online training message_process_.OnChassis(*local_view_.chassis); @@ -181,9 +254,12 @@ bool PlanningComponent::Proc( message_process_.OnTrafficLightDetection(*local_view_.traffic_light); message_process_.OnLocalization(*local_view_.localization_estimate); } - +// 5.0 强化学习数据发布 // publish learning data frame for RL test + // 非主流 + // 如果处于 RL_TEST 模式，记录学习数据，并 写入 planning_learning_data_writer_ if (config_.learning_mode() == PlanningConfig::RL_TEST) { + // 发布 LearningDataFrame，不执行实际规划 PlanningLearningData planning_learning_data; LearningDataFrame* learning_data_frame = injector_->learning_based_data()->GetLatestLearningDataFrame(); @@ -198,27 +274,42 @@ bool PlanningComponent::Proc( } return true; } - +// 6.0 生成并发布轨迹 +// adc_trajectory_pb为最终获取的路径信息 ADCTrajectory adc_trajectory_pb; + // 在这里配置的是on_lane_planning方法，路径规划函数入口 + // planning逻辑主循环 + // 输入:local_view_ 输出:adc_trajectory_pb 轨迹 + // planning_base_: OnLanePlanning实例化的对象 planning_base_->RunOnce(local_view_, &adc_trajectory_pb); - auto start_time = adc_trajectory_pb.header().timestamp_sec(); + // 7.0 此时路径规划已经完成，获取路径规划计算最开始的时间戳，参看RunOnce时间戳赋值 + // 获取计算完成后的时间戳，参看FillHeader时间戳赋值 + auto start_time = adc_trajectory_pb.header().timestamp_sec(); // 从 adc_trajectory_pb 中获取起始时间（start_time），即该消息头中的时间戳 + // 填充 header，包含时间戳 common::util::FillHeader(node_->Name(), &adc_trajectory_pb); // modify trajectory relative time due to the timestamp change in header + // 修正轨迹点的相对时间，确保时间戳一致 + // 修正 relative_time 因 header timestamp 变化导致的偏移 const double dt = start_time - adc_trajectory_pb.header().timestamp_sec(); for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) { p.set_relative_time(p.relative_time() + dt); } + // 8.0 + // 发布路径信息 planning_writer_->Write(adc_trajectory_pb); // Send command execution feedback. // Error occured while executing the command. + // 创建 CommandStatus 反馈规划执行状态 + // 根据轨迹生成状态（OK / ERROR / FINISHED / RUNNING），通过 command_status_writer_ 回传 external_command::CommandStatus command_status; common::util::FillHeader(node_->Name(), &command_status); if (nullptr != local_view_.planning_command) { command_status.set_command_id(local_view_.planning_command->command_id()); } + // 根据 adc_trajectory_pb 的 error_code，判断状态是 ERROR、FINISHED 还是 RUNNING ADCTrajectory::TrajectoryType current_trajectory_type = adc_trajectory_pb.trajectory_type(); if (adc_trajectory_pb.header().status().error_code() != @@ -235,34 +326,48 @@ bool PlanningComponent::Proc( command_status_writer_->Write(command_status); // record in history + // 记录历史轨迹信息 + // record in history + // 将轨迹添加到历史记录 auto* history = injector_->history(); history->Add(adc_trajectory_pb); return true; } - +/// @brief 检查是否需要进行路径重新规划。如果需要，填充相关命令并发送请求，然后将标志位need_rerouting置为false +// 检查是否需要重新规划路径（rerouting），如果需要，则发送新的车道跟随（LaneFollowCommand）请求， +// 并更新 rerouting 状态 void PlanningComponent::CheckRerouting() { auto* rerouting = injector_->planning_context() ->mutable_planning_status() ->mutable_rerouting(); + // 检查是否需要重新规划 if (!rerouting->need_rerouting()) { return; } + // 填充 lane_follow_command 的 header + // FillHeader() 方法会添加 当前节点名称 和 时间戳 common::util::FillHeader(node_->Name(), rerouting->mutable_lane_follow_command()); + // 从 rerouting 提取 lane_follow_command 并包装为 shared_ptr auto lane_follow_command_ptr = std::make_shared( rerouting->lane_follow_command()); - rerouting_client_->SendRequest(lane_follow_command_ptr); + // 向 rerouting_client_ 发送新的 rerouting 请求 + // rerouting_client_ 可能是一个 gRPC 或 ROS 2 Service Client，向路径规划模块发送新的 LaneFollowCommand + // 发送新的规划请求 + rerouting_client_->SendRequest(lane_follow_command_ptr); // 需要看一下 + // 重置 need_rerouting 标志，防止重复请求 rerouting rerouting->set_need_rerouting(false); } - +/// @brief 检查输入数据是否完整，如定位数据、底盘数据、地图数据是否准备好。如果数据不完整，则跳过当前的规划周期 +/// @return bool PlanningComponent::CheckInput() { ADCTrajectory trajectory_pb; auto* not_ready = trajectory_pb.mutable_decision() ->mutable_main_decision() ->mutable_not_ready(); - +// 检查定位，底盘，地图数据 if (local_view_.localization_estimate == nullptr) { not_ready->set_reason("localization not ready"); } else if (local_view_.chassis == nullptr) { @@ -274,19 +379,23 @@ bool PlanningComponent::CheckInput() { } if (FLAGS_use_navigation_mode) { + // 如果开启了导航模式，则需要 relative_map 数据 if (!local_view_.relative_map->has_header()) { not_ready->set_reason("relative map not ready"); } } else { + // 如果 planning_command 为空，或不含 header，则说明规划指令未准备好 if (!local_view_.planning_command || !local_view_.planning_command->has_header()) { not_ready->set_reason("planning_command not ready"); } } - + // 若任一缺失，发布一个空轨迹并返回 false if (not_ready->has_reason()) { AINFO << not_ready->reason() << "; skip the planning cycle."; + // 填充 trajectory_pb 的 header common::util::FillHeader(node_->Name(), &trajectory_pb); + // 发布空轨迹 planning_writer_->Write(trajectory_pb); return false; } diff --git a/modules/planning/planning_component/planning_component.h b/modules/planning/planning_component/planning_component.h index a4a13e4a835..22331466947 100644 --- a/modules/planning/planning_component/planning_component.h +++ b/modules/planning/planning_component/planning_component.h @@ -18,28 +18,31 @@ #include -#include "modules/common_msgs/chassis_msgs/chassis.pb.h" +#include "modules/common_msgs/chassis_msgs/chassis.pb.h" // 车辆底盘数据 (Chassis) #include "modules/common_msgs/external_command_msgs/command_status.pb.h" #include "modules/common_msgs/external_command_msgs/lane_follow_command.pb.h" -#include "modules/common_msgs/localization_msgs/localization.pb.h" -#include "modules/common_msgs/perception_msgs/traffic_light_detection.pb.h" +#include "modules/common_msgs/localization_msgs/localization.pb.h" // 车辆定位数据 (LocalizationEstimate) +#include "modules/common_msgs/perception_msgs/traffic_light_detection.pb.h" // 交通信号灯检测数据 (TrafficLightDetection) #include "modules/common_msgs/planning_msgs/pad_msg.pb.h" -#include "modules/common_msgs/planning_msgs/planning.pb.h" +#include "modules/common_msgs/planning_msgs/planning.pb.h" // 规划轨迹 (ADCTrajectory) #include "modules/common_msgs/prediction_msgs/prediction_obstacle.pb.h" #include "modules/common_msgs/storytelling_msgs/story.pb.h" -#include "modules/planning/planning_base/proto/learning_data.pb.h" -#include "modules/planning/planning_base/proto/planning_config.pb.h" +#include "modules/planning/planning_base/proto/learning_data.pb.h" // 机器学习数据 (PlanningLearningData) +#include "modules/planning/planning_base/proto/planning_config.pb.h" // 规划模块的参数配置 (PlanningConfig) -#include "cyber/class_loader/class_loader.h" -#include "cyber/component/component.h" -#include "cyber/message/raw_message.h" -#include "modules/planning/planning_base/common/message_process.h" -#include "modules/planning/planning_base/gflags/planning_gflags.h" -#include "modules/planning/planning_component/planning_base.h" +#include "cyber/class_loader/class_loader.h" // 用于加载 Cyber 组件 +#include "cyber/component/component.h" // 使 PlanningComponent 继承 cyber::Component，从而成为 Apollo Cyber RT 组件 +#include "cyber/message/raw_message.h" // 处理原始消息 +#include "modules/planning/planning_base/common/message_process.h" // 消息处理逻辑 +#include "modules/planning/planning_base/gflags/planning_gflags.h" // 规划模块的全局标志变量 +#include "modules/planning/planning_component/planning_base.h" // 规划模块的基类 namespace apollo { namespace planning { - +/// @brief PlanningComponent 继承自 cyber::Component，表示它是 Apollo Cyber 组件，可以订阅和发布消息 +// 预测模块的障碍物信息 +// 车辆底盘信息 +// 车辆定位信息 class PlanningComponent final : public cyber::Component { @@ -49,6 +52,7 @@ class PlanningComponent final ~PlanningComponent() = default; public: + // 在组件启动时调用，用于初始化 订阅者、发布者、参数配置 等 bool Init() override; bool Proc(const std::shared_ptr& @@ -58,25 +62,35 @@ class PlanningComponent final localization_estimate) override; private: + // 检查是否需要重新规划路径 void CheckRerouting(); + // 检查输入数据是否有效 bool CheckInput(); private: + // 订阅交通信号灯检测数据 std::shared_ptr> traffic_light_reader_; + // Apollo Cyber 服务客户端，用于向 rerouting 服务发送车道跟随命令并接收状态反馈 std::shared_ptr< apollo::cyber::Client> rerouting_client_; + // 其他订阅者 std::shared_ptr> pad_msg_reader_; std::shared_ptr> relative_map_reader_; std::shared_ptr> story_telling_reader_; std::shared_ptr> planning_command_reader_; + // 发布者 + // 规划轨迹 (ADCTrajectory) std::shared_ptr> planning_writer_; + // 重新规划请求 (RoutingRequest) std::shared_ptr> rerouting_writer_; + // 学习数据 (PlanningLearningData) std::shared_ptr> planning_learning_data_writer_; + // 指令状态 (CommandStatus) std::shared_ptr> command_status_writer_; @@ -86,7 +100,7 @@ class PlanningComponent final planning::PadMessage pad_msg_; relative_map::MapMsg relative_map_; storytelling::Stories stories_; - PlanningCommand planning_command_; + PlanningCommand planning_command_; // proto生成的pb.h LocalView local_view_; std::unique_ptr planning_base_; @@ -96,6 +110,8 @@ class PlanningComponent final MessageProcess message_process_; }; +// 组件注册 +// CYBER_REGISTER_COMPONENT() 注册 PlanningComponent 组件，使其可以在 Apollo Cyber 框架 中运行 CYBER_REGISTER_COMPONENT(PlanningComponent) } // namespace planning diff --git a/modules/planning/planning_interface_base/planner_base/planner.h b/modules/planning/planning_interface_base/planner_base/planner.h index 9e4e33469fb..9faf72838c8 100644 --- a/modules/planning/planning_interface_base/planner_base/planner.h +++ b/modules/planning/planning_interface_base/planner_base/planner.h @@ -79,6 +79,12 @@ class Planner { std::shared_ptr injector_ = nullptr; }; +/// @brief +/// @tparam T +/// @param custom_config_path 表示自定义配置文件路径 +/// @param config 指向类型 T 的指针 config（用于存放加载的配置） +/// @return +// 如果提供了自定义的配置文件路径，就加载它。如果没有提供，则通过反射机制获取当前类的名字，并根据类名查找默认的配置文件路径，最终加载配置文件并填充配置对象 template bool Planner::LoadConfig(const std::string& custom_config_path, T* config) { std::string config_path = custom_config_path; @@ -86,12 +92,15 @@ bool Planner::LoadConfig(const std::string& custom_config_path, T* config) { if ("" == config_path) { int status; // Get the name of this class. + //通过 typeid(*this).name() 获取当前对象的类型信息，并使用 abi::__cxa_demangle 函数将类型名称解码为更可读的格式 + // （例如，从 typeid 返回的名称通常是编译器特定的符号，需要解码）。最终，解码后的类名存储在 class_name 字符串中 std::string class_name = abi::__cxa_demangle(typeid(*this).name(), 0, 0, &status); config_path = apollo::cyber::plugin_manager::PluginManager::Instance() ->GetPluginConfPath( class_name, "conf/planner_config.pb.txt"); } + // 读取配置文件并将内容填充到 config 指向的对象中 return apollo::cyber::common::LoadConfig(config_path, config); } diff --git a/modules/planning/planning_interface_base/scenario_base/scenario.cc b/modules/planning/planning_interface_base/scenario_base/scenario.cc index 29ee79fd215..a990b4f9ee2 100644 --- a/modules/planning/planning_interface_base/scenario_base/scenario.cc +++ b/modules/planning/planning_interface_base/scenario_base/scenario.cc @@ -19,13 +19,18 @@ **/ #include "modules/planning/planning_interface_base/scenario_base/scenario.h" - +/* +引入 C++ ABI（Application Binary Interface）工具，用于 将编译器修饰的类名还原为可读字符串（如 typeid(*this).name() 返回的是 _ZN5apollo7planning8ScenarioE，需 demangle 成 "apollo::planning::Scenario"） +*/ #include #include "cyber/class_loader/class_loader_manager.h" #include "cyber/plugin_manager/plugin_manager.h" +// 引入 Frame 类，包含当前规划周期所需的全部上下文（感知、定位、地图、交通规则等） #include "modules/planning/planning_base/common/frame.h" +// 引入配置工具，如 TransformToPathName（将场景名如 "LANE_FOLLOW" 转为路径名 "lane_follow"） #include "modules/planning/planning_base/common/util/config_util.h" +// 引入 Stage 基类。每个 Scenario 由多个 Stage（阶段）组成，例如“减速”、“停车”、“起步”。 #include "modules/planning/planning_interface_base/scenario_base/stage.h" namespace apollo { @@ -40,32 +45,42 @@ Scenario::Scenario() config_dir_(""), name_("") {} +/// @brief +/// @param injector 依赖注入器（提供共享服务如地图、定位） +/// @param name 场景名称（如 "LANE_FOLLOW"） +/// @return bool Scenario::Init(std::shared_ptr injector, const std::string& name) { name_ = name; injector_ = injector; // set scenario_type in PlanningContext + // 1.设置 PlanningContext 中当前 Scenario 类型 + // 将当前场景类型写入全局 PlanningContext（供其他模块读取，如监控、可视化） auto* scenario = injector_->planning_context() ->mutable_planning_status() ->mutable_scenario(); scenario->Clear(); scenario->set_scenario_type(name_); + // 2. 获取当前 Scenario 类的名称 // Generate the default config path. + // 获取当前对象的实际类名（如 "apollo::planning::LaneFollowScenario"） int status; // Get the name of this class. std::string class_name = abi::__cxa_demangle(typeid(*this).name(), 0, 0, &status); - + // 3. 找到该 Scenario 插件所在目录（配置目录） config_dir_ = apollo::cyber::plugin_manager::PluginManager::Instance() ->GetPluginClassHomePath(class_name); config_dir_ += "/conf"; AINFO << "config_dir : " << config_dir_; // Generate the default task config path from PluginManager. + // 4. 获取 scenario_conf.pb.txt 的路径 config_path_ = apollo::cyber::plugin_manager::PluginManager::Instance() ->GetPluginConfPath(class_name, "conf/scenario_conf.pb.txt"); - + + // 5.加载 pipeline（流水线）配置 // Load the pipeline config. std::string pipeline_config_path = apollo::cyber::plugin_manager::PluginManager::Instance() @@ -77,17 +92,27 @@ bool Scenario::Init(std::shared_ptr injector, AERROR << "Load pipeline of " << name_ << " failed!"; return false; } + // 6. 将 pipeline 中的 stage 映射进 map + // 将配置中的每个 Stage 按名称存入 stage_pipeline_map_（std::map），便于后续查找 for (const auto& stage : scenario_pipeline_config_.stage()) { stage_pipeline_map_[stage.name()] = &stage; } return true; } - +/// @brief 执行特定场景中各个阶段（stage）的核心函数，涉及到场景状态的管理以及阶段的处理和切换 +/// @param planning_init_point 规划起点（自车当前位置/速度） +/// @param frame 当前帧上下文 +/// @return ScenarioResult Scenario::Process( const common::TrajectoryPoint& planning_init_point, Frame* frame) { +// 如果当前阶段为空 +// 如果尚未创建第一阶段，则根据配置的第一个 Stage 创建它 if (current_stage_ == nullptr) { + // 从scenario_pipeline_config_ 中获取第一个阶段的配置信息,并通过 CreateStage 创建该阶段 current_stage_ = CreateStage( *stage_pipeline_map_[scenario_pipeline_config_.stage(0).name()]); + // 创建失败,打印错误信息并将阶段状态设置为 ERROR，然后返回场景结果 + // 创建失败则报错并返回错误状态 if (nullptr == current_stage_) { AERROR << "Create stage " << scenario_pipeline_config_.stage(0).name() << " failed!"; @@ -96,37 +121,54 @@ ScenarioResult Scenario::Process( } AINFO << "Create stage " << current_stage_->Name(); } + // 如果当前阶段的名称为空，表示当前场景已经完成，不再需要处理，直接将场景的状态设置为 STATUS_DONE（已完成），然后返回场景结 + // 如果当前阶段名称为空，视为场景已完成 if (current_stage_->Name().empty()) { scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_DONE); return scenario_result_; } - auto ret = current_stage_->Process(planning_init_point, frame); + + // 调用当前阶段的 Process 方法，获取其执行结果 + auto ret = current_stage_->Process(planning_init_point, frame); // lane_follow_stage.cc scenario_result_.SetStageResult(ret); + + // 根据阶段返回状态决定下一步 switch (ret.GetStageStatus()) { + // 情况 1：阶段出错 case StageStatusType::ERROR: { AERROR << "Stage '" << current_stage_->Name() << "' returns error"; scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_UNKNOWN); break; } + // 情况 2：阶段仍在运行 case StageStatusType::RUNNING: { scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_PROCESSING); break; } + // 情况 3：阶段完成 + // 如果阶段返回 FINISHED，表示当前阶段已经完成，接下来要判断是否需要切换到下一个阶段 case StageStatusType::FINISHED: { + // 获取下一阶段名称（由当前阶段决定，如 "STOP_SIGN_APPROACH" → "STOP_SIGN_WAIT"） auto next_stage = current_stage_->NextStage(); + // 如果下一阶段 不同于当前阶段（即需要切换） if (next_stage != current_stage_->Name()) { AINFO << "switch stage from " << current_stage_->Name() << " to " << next_stage; + // 若下一阶段为空，表示整个场景结束 if (next_stage.empty()) { scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_DONE); return scenario_result_; } + // 如果下一个阶段的配置找不到，打印错误并返回 STATUS_UNKNOWN + // 检查下一阶段是否在配置中存在 if (stage_pipeline_map_.find(next_stage) == stage_pipeline_map_.end()) { AERROR << "Failed to find config for stage: " << next_stage; scenario_result_.SetScenarioStatus( ScenarioStatusType::STATUS_UNKNOWN); return scenario_result_; } + // 如果下一个阶段的配置存在，尝试创建下一个阶段。如果创建失败，则打印警告并返回 STATUS_UNKNOWN + // 创建新阶段实例 current_stage_ = CreateStage(*stage_pipeline_map_[next_stage]); if (current_stage_ == nullptr) { AWARN << "Current stage is a null pointer."; @@ -135,6 +177,8 @@ ScenarioResult Scenario::Process( return scenario_result_; } } + // 如果当前阶段继续有效，设置场景状态为 STATUS_PROCESSING（处理中） + // 设置场景整体状态 if (current_stage_ != nullptr && !current_stage_->Name().empty()) { scenario_result_.SetScenarioStatus( ScenarioStatusType::STATUS_PROCESSING); @@ -143,6 +187,7 @@ ScenarioResult Scenario::Process( } break; } + // 默认情况（异常） default: { AWARN << "Unexpected Stage return value: " << static_cast(ret.GetStageStatus()); diff --git a/modules/planning/planning_interface_base/scenario_base/stage.cc b/modules/planning/planning_interface_base/scenario_base/stage.cc index 304298d2e1f..a2c78c04d35 100644 --- a/modules/planning/planning_interface_base/scenario_base/stage.cc +++ b/modules/planning/planning_interface_base/scenario_base/stage.cc @@ -40,6 +40,7 @@ using apollo::cyber::Clock; Stage::Stage() : next_stage_(""), context_(nullptr), injector_(nullptr), name_("") {} +// 初始化一个 Stage 并加载其 Task 列表 + fallback task bool Stage::Init(const StagePipeline& config, const std::shared_ptr& injector, const std::string& config_dir, void* context) { diff --git a/modules/planning/planning_interface_base/task_base/common/path_generation.cc b/modules/planning/planning_interface_base/task_base/common/path_generation.cc index e3517143b89..796df662061 100644 --- a/modules/planning/planning_interface_base/task_base/common/path_generation.cc +++ b/modules/planning/planning_interface_base/task_base/common/path_generation.cc @@ -105,13 +105,19 @@ void PathGeneration::RecordDebugInfo( {path_points.begin(), path_points.end()}); } void PathGeneration::GetStartPointSLState() { + // 获取当前正在处理的参考线对象 reference_line const ReferenceLine& reference_line = reference_line_info_->reference_line(); + // 获取规划起点的世界坐标（x, y, heading, speed 等），通常是车辆当前位置 common::TrajectoryPoint planning_start_point = frame_->PlanningStartPoint(); + // 使用车辆前轮中心作为路径起点 if (FLAGS_use_front_axe_center_in_path_planning) { + // 获取车辆前轴到后轴之间的距离（wheel_base，轴距），单位为米 + // 用来从车辆重心位置推算前轴中心位置的关键参数 double front_to_rear_axe_distance = apollo::common::VehicleConfigHelper::GetConfig() .vehicle_param() .wheel_base(); + // 根据车辆朝向 theta，将路径起点向前（车辆前轴方向）移动一个“轴距”长度 planning_start_point.mutable_path_point()->set_x( planning_start_point.path_point().x() + front_to_rear_axe_distance * @@ -120,6 +126,7 @@ void PathGeneration::GetStartPointSLState() { planning_start_point.path_point().y() + front_to_rear_axe_distance * std::sin(planning_start_point.path_point().theta())); + // 最终得到的点位于车辆前轮轴中心 } ADEBUG << std::fixed << "Plan at the starting point: x = " << planning_start_point.path_point().x() @@ -128,6 +135,7 @@ void PathGeneration::GetStartPointSLState() { // Initialize some private variables. // ADC s/l info. + // 将世界坐标点 planning_start_point 转换成参考线下的 Frenet 坐标（S, L） init_sl_state_ = reference_line.ToFrenetFrame(planning_start_point); } diff --git a/modules/planning/planning_interface_base/task_base/common/path_util/path_assessment_decider_util.cc b/modules/planning/planning_interface_base/task_base/common/path_util/path_assessment_decider_util.cc index a4b9a2f8b26..4af12bfe201 100644 --- a/modules/planning/planning_interface_base/task_base/common/path_util/path_assessment_decider_util.cc +++ b/modules/planning/planning_interface_base/task_base/common/path_util/path_assessment_decider_util.cc @@ -37,21 +37,24 @@ bool PathAssessmentDeciderUtil::IsValidRegularPath( return false; } // Check if the path is greatly off the reference line. + // 判断该路径是否严重偏离参考线（比如横向偏移过大 if (IsGreatlyOffReferenceLine(path_data)) { ADEBUG << path_data.path_label() << ": ADC is greatly off reference line."; return false; } // Check if the path is greatly off the road. + // 判断路径是否严重偏离道路边界（即路径不在车道或道路允许范围内） if (IsGreatlyOffRoad(reference_line_info, path_data)) { ADEBUG << path_data.path_label() << ": ADC is greatly off road."; return false; } // Check if there is any collision. + // 判断路径是否与静态障碍物发生碰撞（比如停在路边的车辆、交通设施等） if (IsCollidingWithStaticObstacles(reference_line_info, path_data)) { ADEBUG << path_data.path_label() << ": ADC has collision."; return false; } - + // 判断路径终点是否停在了逆向邻接车道上（即车头朝向和车道方向相反） if (IsStopOnReverseNeighborLane(reference_line_info, path_data)) { ADEBUG << path_data.path_label() << ": stop at reverse neighbor lane"; return false; @@ -65,6 +68,7 @@ bool PathAssessmentDeciderUtil::IsGreatlyOffReferenceLine( static constexpr double kOffReferenceLineThreshold = 20.0; const auto& frenet_path = path_data.frenet_frame_path(); for (const auto& frenet_path_point : frenet_path) { + // 判断当前路径点的横向偏移 l 值的绝对值是否大于 20 米 if (std::fabs(frenet_path_point.l()) > kOffReferenceLineThreshold) { ADEBUG << "Greatly off reference line at s = " << frenet_path_point.s() << ", with l = " << frenet_path_point.l(); @@ -83,6 +87,7 @@ bool PathAssessmentDeciderUtil::IsGreatlyOffRoad( double road_right_width = 0.0; if (reference_line_info.reference_line().GetRoadWidth( frenet_path_point.s(), &road_left_width, &road_right_width)) { + // 判断当前路径点的横向位置（l 值）是否严重超出道路边缘 if (frenet_path_point.l() > road_left_width + kOffRoadThreshold || frenet_path_point.l() < -road_right_width - kOffRoadThreshold) { ADEBUG << "Greatly off-road at s = " << frenet_path_point.s() @@ -104,14 +109,18 @@ bool PathAssessmentDeciderUtil::IsCollidingWithStaticObstacles( common::VehicleConfigHelper::GetConfig().vehicle_param(); double front_edge_to_center = vehicle_param.front_edge_to_center(); double back_edge_to_center = vehicle_param.back_edge_to_center(); + // 计算路径点的横向缓冲区（用于包裹车辆边界的矩形碰撞体积） double path_point_lateral_buffer = std::max(vehicle_param.width() / 2.0, vehicle_param.length() / 2.0); for (const auto* obstacle : indexed_obstacles.Items()) { // Filter out unrelated obstacles. + // 障碍物筛选 + // 是否在路径规划考虑的范围内（例如远处无关障碍物直接忽略） if (!PathBoundsDeciderUtil::IsWithinPathDeciderScopeObstacle(*obstacle)) { continue; } // Ignore too small obstacles. + // 是否面积太小，比如小碎石、标志杆等，不构成严重影响 const auto& obstacle_sl = obstacle->PerceptionSLBoundary(); if ((obstacle_sl.end_s() - obstacle_sl.start_s()) * (obstacle_sl.end_l() - obstacle_sl.start_l()) < @@ -120,16 +129,21 @@ bool PathAssessmentDeciderUtil::IsCollidingWithStaticObstacles( } obstacles.push_back(obstacle); } + + // 遍历路径点进行碰撞检测 // Go through all the four corner points at every path pt, check collision. const auto& frenet_path = path_data.frenet_frame_path(); for (size_t i = 0; i < path_data.discretized_path().size(); ++i) { // Skip the point after end point. + // 忽略尾部的冗余路径点 + // 判断逻辑：从当前点到路径尾部的 s 距离是否太短，如果短于设定阈值则退出 if (path_data.frenet_frame_path().back().s() - path_data.frenet_frame_path()[i].s() < (FLAGS_num_extra_tail_bound_point + 1) * FLAGS_path_bounds_decider_resolution) { break; } + // 构建路径点的边界框（SL） double path_point_start_s = frenet_path[i].s() - back_edge_to_center; double path_point_end_s = frenet_path[i].s() + front_edge_to_center; double path_point_start_l = frenet_path[i].l() - path_point_lateral_buffer; @@ -138,6 +152,7 @@ bool PathAssessmentDeciderUtil::IsCollidingWithStaticObstacles( for (const auto* obstacle : obstacles) { const auto& obstacle_sl = obstacle->PerceptionSLBoundary(); // Filter the path points by s range. + // 快速判断当前障碍物是否可能和车辆包围盒有重叠（以SL边界为基础） if (obstacle_sl.start_s() > path_point_end_s || obstacle_sl.end_s() < path_point_start_s) { continue; @@ -146,10 +161,14 @@ bool PathAssessmentDeciderUtil::IsCollidingWithStaticObstacles( obstacle_sl.end_l() < path_point_start_l) { continue; } + + // 准确地用矩形多边形碰撞检测 const auto& path_point = path_data.discretized_path()[i]; + // 获取当前路径点对应的车辆矩形包络框（通过 vehicle_box） const auto& vehicle_box = common::VehicleConfigHelper::Instance()->GetBoundingBox(path_point); const std::vector& ABCDpoints = vehicle_box.GetAllCorners(); + // 使用障碍物的真实多边形轮廓（obstacle_polygon）进行点是否在障碍物内部的判断 const common::math::Polygon2d& obstacle_polygon = obstacle->PerceptionPolygon(); for (const auto& corner_point : ABCDpoints) { diff --git a/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.cc b/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.cc index 7f64c4feef5..71729f72ce0 100644 --- a/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.cc +++ b/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.cc @@ -34,24 +34,35 @@ namespace apollo { namespace planning { using apollo::common::VehicleConfigHelper; - +/// @brief 初始化一个空的路径边界（PathBoundary），从当前 s 开始，按固定步长（FLAGS_path_bounds_decider_resolution）向前生成一系列点，初始上下界设为 ±∞ +/// @param reference_line_info 包含参考线的信息（如参考线的路径、速度等） +/// @param path_bound 指向 PathBoundary 对象的指针，用来存储路径边界 +/// @param init_sl_state 起始状态，表示路径坐标系中的起始位置和状态 +/// @return bool PathBoundsDeciderUtil::InitPathBoundary( const ReferenceLineInfo& reference_line_info, PathBoundary* const path_bound, SLState init_sl_state) { // Sanity checks. + // 确保 path_bound 指针不为空 CHECK_NOTNULL(path_bound); + // 清空路径边界，准备重新初始化 path_bound->clear(); const auto& reference_line = reference_line_info.reference_line(); + // 设置路径边界的分辨率 delta_s ： 0.5 path_bound->set_delta_s(FLAGS_path_bounds_decider_resolution); - + // 获取车辆配置 vehicle_config，从中获取与车辆相关的参数 const auto& vehicle_config = common::VehicleConfigHelper::Instance()->GetConfig(); + // 车辆前端到后轴中心的距离 const double ego_front_to_center = vehicle_config.vehicle_param().front_edge_to_center(); - +// reference_line.Length() - ego_front_to_center 确保了在路径的末端，车辆的前端不会超出路径的边界，确保了路径的有效性，避免了车辆行驶出路径范围 +// 起始 s = init_sl_state.first[0]（即 ego 车当前位置 s） +// 终止 s = min(起始s + horizon, 参考线长度 - 车头到中心距离) +// horizon = max(配置的 horizon, 巡航速度 × 时间长度) for (double curr_s = init_sl_state.first[0]; curr_s < std::fmin(init_sl_state.first[0] + - std::fmax(FLAGS_path_bounds_horizon, + std::fmax(FLAGS_path_bounds_horizon, // 100 reference_line_info.GetCruiseSpeed() * FLAGS_trajectory_time_length), reference_line.Length() - ego_front_to_center); @@ -68,6 +79,7 @@ bool PathBoundsDeciderUtil::InitPathBoundary( return true; } +// 将规划起点（通常是后轴中心）转换为 Frenet 坐标（s, l, l', l''） void PathBoundsDeciderUtil::GetStartPoint( common::TrajectoryPoint planning_start_point, const ReferenceLine& reference_line, SLState* init_sl_state) { @@ -85,6 +97,7 @@ void PathBoundsDeciderUtil::GetStartPoint( *init_sl_state = reference_line.ToFrenetFrame(planning_start_point); } +// 获取 ego 车所在位置的车道总宽度（左宽 + 右宽） double PathBoundsDeciderUtil::GetADCLaneWidth( const ReferenceLine& reference_line, const double adc_s) { double lane_left_width = 0.0; @@ -103,6 +116,7 @@ bool PathBoundsDeciderUtil::UpdatePathBoundaryWithBuffer( double left_bound, double right_bound, BoundType left_type, BoundType right_type, std::string left_id, std::string right_id, PathBoundPoint* const bound_point) { + // 将输入的 left_bound 减去 半车宽（因为边界是车道边缘，而路径是车辆中心轨迹 if (!UpdateLeftPathBoundaryWithBuffer(left_bound, left_type, left_id, bound_point)) { return false; @@ -156,32 +170,66 @@ bool PathBoundsDeciderUtil::UpdateRightPathBoundaryWithBuffer( return true; } +/// @brief 根据路径被阻塞的位置修剪路径边界 +/// @param path_blocked_idx 路径被阻塞的索引位置 +/// @param path_boundaries void PathBoundsDeciderUtil::TrimPathBounds( const int path_blocked_idx, PathBoundary* const path_boundaries) { if (path_blocked_idx != -1) { if (path_blocked_idx == 0) { ADEBUG << "Completely blocked. Cannot move at all."; } + // 计算从阻塞位置到路径边界的剩余部分的长度 int range = static_cast(path_boundaries->size()) - path_blocked_idx; + // 移除所有在阻塞索引之后的元素 for (int i = 0; i < range; ++i) { path_boundaries->pop_back(); } } } +// 如果在某点 i 被障碍物完全堵死，则截断路径边界，只保留到 i 之前的部分（再减去车头长度） +// 从尾部 pop_back() 直到 s <= blocked_s - front_edge_to_center +void PathBoundsDeciderUtil::TrimPathBounds( + const int path_blocked_idx, PathBoundary* const path_boundaries) { + if (path_blocked_idx != -1) { + if (path_blocked_idx == 0) { + AINFO << "Completely blocked. Cannot move at all."; + } + double front_edge_to_center = + VehicleConfigHelper::GetConfig().vehicle_param().front_edge_to_center(); + double trimmed_s = + path_boundaries->at(path_blocked_idx).s - front_edge_to_center; + AINFO << "Trimmed from " << path_boundaries->back().s << " to " + << path_boundaries->at(path_blocked_idx).s; + while (path_boundaries->size() > 1 && + path_boundaries->back().s > trimmed_s) { + path_boundaries->pop_back(); + } + } +} + +/// @brief 找到起始 s 值最大的障碍物 ID，也就是最靠前（沿路径最远）开始阻挡自车的障碍物 +/// @param obs_id_to_start_s 键是障碍物的 ID，值是该障碍物在参考线上开始影响路径的 s 值 +/// @return std::string PathBoundsDeciderUtil::FindFarthestBlockObstaclesId( const std::unordered_map& obs_id_to_start_s) { std::string nearest_obstcles_id = ""; double max_start_s = std::numeric_limits::lowest(); + // 遍历所有障碍物，找到 start_s 最大的那个 for (auto obs : obs_id_to_start_s) { if (obs.second > max_start_s) { nearest_obstcles_id = obs.first; max_start_s = obs.second; } } + // 返回 start_s 最大的障碍物 ID return nearest_obstcles_id; } - +/// @brief 左边界值从小到大排序 +/// @param lhs +/// @param rhs +/// @return bool CompareLeftBound(const std::pair& lhs, const std::pair& rhs) { if (lhs.first == rhs.first) { @@ -189,6 +237,10 @@ bool CompareLeftBound(const std::pair& lhs, } return lhs.second < rhs.second; } +/// @brief 右边界从大到小排序 +/// @param lhs +/// @param rhs +/// @return bool CompareRightBound(const std::pair& lhs, const std::pair& rhs) { if (lhs.first == rhs.first) { @@ -206,10 +258,13 @@ bool PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles( auto sorted_obstacles = SortObstaclesForSweepLine(indexed_obstacles, init_sl_state); AINFO << "There are " << sorted_obstacles.size() << " obstacles."; - double center_line = init_sl_state.second[0]; + double center_line = init_sl_state.second[0]; // // 当前 ADC 的初始横向位置 ADEBUG << "init l" << init_sl_state.second[0]; + // 指示当前正在处理的障碍物(上面排序过的)的索引 size_t obs_idx = 0; + // 存储被阻塞的位置的索引 int path_blocked_idx = -1; + // 存储路径右侧的障碍物边界，每个元素是一个 pair，包含障碍物的 ID 和相应的右侧边界的值（即障碍物在 l_max 方向的值） std::multiset, decltype(CompareRightBound)*> right_bounds(CompareRightBound); std::multiset, decltype(CompareLeftBound)*> @@ -219,11 +274,14 @@ bool PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles( left_bounds.insert(std::make_pair("", std::numeric_limits::max())); // Maps obstacle ID's to the decided ADC pass direction, if ADC should // pass from left, then true; otherwise, false. + // 存储障碍物的 ID 和自车（ADC）通过障碍物的方向（true 表示从左侧通过，false 表示从右侧通过） std::unordered_map obs_id_to_direction; // Maps obstacle ID's to the decision of whether side-pass on this obstacle // is allowed. If allowed, then true; otherwise, false. + // 用于存储障碍物的 ID 和是否允许侧方通过的决策（true 表示允许侧方通过，false 表示不允许） std::unordered_map obs_id_to_sidepass_decision; // Maps obstacle ID's to start s on this obstacle + // 存储障碍物的 ID 和障碍物的起始位置 s。s 表示障碍物在路径上的位置（沿着路径的纵向位置） std::unordered_map obs_id_to_start_s; // Step through every path point. for (size_t i = 1; i < path_boundaries->size(); ++i) { @@ -242,6 +300,7 @@ bool PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles( << "] curr_obstacle_l_min[" << curr_obstacle_l_min << "] curr_obstacle_l_max[" << curr_obstacle_l_max << "] center_line[" << center_line << "]"; + // 新障碍物进入（in） if (std::get<0>(curr_obstacle) == 1) { // A new obstacle enters into our scope: // - Decide which direction for the ADC to pass. @@ -249,7 +308,7 @@ bool PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles( // - If boundaries blocked, then decide whether can side-pass. // - If yes, then borrow neighbor lane to side-pass. - if (curr_obstacle_l_min + curr_obstacle_l_max < center_line * 2) { + if (curr_obstacle_l_min + curr_obstacle_l_max < center_line * 2) { // 靠右，建议从左绕 // Obstacle is to the right of center-line, should pass from // left. ADEBUG << curr_obstacle_id << "left nudge"; @@ -266,6 +325,7 @@ bool PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles( ADEBUG << curr_obstacle_id << "right nudge"; obs_id_to_start_s[curr_obstacle_id] = curr_obstacle_s; } else { + // 障碍物离开（out） // An existing obstacle exits our scope. if (obs_id_to_direction[curr_obstacle_id]) { right_bounds.erase(right_bounds.find( @@ -340,18 +400,26 @@ bool PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles( } // The tuple contains (is_start_s, s, l_min, l_max, obstacle_id) +// bool is_start_s: 障碍物是起始边界(1)还是结束边界(0) +// s:障碍物的纵向位置 +// l_min:障碍物在横向上的最小横向位置 +// l_max:障碍物在横向上的最大横向位置 +// obstacle_id: 障碍物的id std::vector PathBoundsDeciderUtil::SortObstaclesForSweepLine( const IndexedList& indexed_obstacles, const SLState& init_sl_state) { + // 初始化 sorted_obstacles 向量 std::vector sorted_obstacles; // Go through every obstacle and preprocess it. for (const auto* obstacle : indexed_obstacles.Items()) { // Only focus on those within-scope obstacles. + // 过滤出在路径规划范围内的障碍物 if (!IsWithinPathDeciderScopeObstacle(*obstacle)) { continue; } // Only focus on obstacles that are ahead of ADC. + // // 过滤后方的障碍物 if (obstacle->PerceptionSLBoundary().end_s() < init_sl_state.first[0]) { continue; } @@ -404,7 +472,7 @@ bool PathBoundsDeciderUtil::IsWithinPathDeciderScopeObstacle( } // Obstacle should not be moving obstacle. if (!obstacle.IsStatic() || - obstacle.speed() > FLAGS_static_obstacle_speed_threshold) { + obstacle.speed() > FLAGS_static_obstacle_speed_threshold) { // 0.5 return false; } // TODO(jiacheng): @@ -462,21 +530,35 @@ PathBoundsDeciderUtil::InferFrontAxeCenterFromRearAxeCenter( front_to_rear_axe_distance * std::sin(traj_point.path_point().theta())); return ret; } - +/// @brief 根据车辆（自动驾驶车辆，ADC）所在的车道信息和车辆位置，更新路径边界（PathBoundary）。其具体步骤包括获取当前车道的宽度、计算相应的路径边界，并更新路径边界数据 +/// @param reference_line_info +/// @param init_sl_state +/// @param path_bound +/// @return bool PathBoundsDeciderUtil::GetBoundaryFromSelfLane( const ReferenceLineInfo& reference_line_info, const SLState& init_sl_state, PathBoundary* const path_bound) { // Sanity checks. + // 输入合法性检查 + // 确保 path_bound 指针不为空 CHECK_NOTNULL(path_bound); + // path_bound 内的边界数据不为空 ACHECK(!path_bound->empty()); const ReferenceLine& reference_line = reference_line_info.reference_line(); + + // 通过初始点的 s 坐标获取 ADC 所在车道的总宽度，用于初始化历史值 + // 根据车辆起始位置（init_sl_state.first[0]）计算出车辆所在车道的宽度 adc_lane_width double adc_lane_width = GetADCLaneWidth(reference_line, init_sl_state.first[0]); // Go through every point, update the boundary based on lane info and // ADC's position. double past_lane_left_width = adc_lane_width / 2.0; double past_lane_right_width = adc_lane_width / 2.0; + // 记录路径上被阻塞的点的索引，初始值设为 -1 int path_blocked_idx = -1; + + // 遍历每一个 PathBoundary 点（每个 s） + // 开始循环遍历路径边界中的每个点，curr_s 是当前路径点在参考线上的位置 for (size_t i = 0; i < path_bound->size(); ++i) { double curr_s = (*path_bound)[i].s; // 1. Get the current lane width at current point. @@ -489,6 +571,7 @@ bool PathBoundsDeciderUtil::GetBoundaryFromSelfLane( curr_lane_left_width = past_lane_left_width; curr_lane_right_width = past_lane_right_width; } else { + // 当前点的参考线 reference_line 与 它所在车道的中心线之间的横向距离（即参考线是否偏离了该车道的中心线） 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; @@ -498,6 +581,8 @@ bool PathBoundsDeciderUtil::GetBoundaryFromSelfLane( // 3. Calculate the proper boundary based on lane-width, ADC's position, // and ADC's velocity. + // 计算当前路径点的实际左右边界。首先计算从参考线坐标系到地图坐标系的偏移量 offset_to_map + // 当前参考线 reference_line 相对于全局地图横向参考线的偏移，也可以理解为当前 s 位置下，参考线在横向 L 方向上的实际坐标 double offset_to_map = 0.0; reference_line.GetOffsetToMap(curr_s, &offset_to_map); @@ -520,21 +605,31 @@ bool PathBoundsDeciderUtil::GetBoundaryFromSelfLane( return true; } - +/// @brief 根据自车的状态扩展路径的左右边界 +/// @param reference_line_info 包含参考路径的相关信息 +/// @param init_sl_state 初始路径的 S 方向状态，包含车辆的横向位置（l）和速度（v） +/// @param extend_buffer 额外的扩展缓冲区，用来扩展路径的边界 +/// @param path_bound 路径的边界数据结构，存储路径的每一段的左右边界信息 +/// @return bool PathBoundsDeciderUtil::ExtendBoundaryByADC( const ReferenceLineInfo& reference_line_info, const SLState& init_sl_state, const double extend_buffer, PathBoundary* const path_bound) { + // 自车的横向位置，即自车相对于车道中心的横向偏移 double adc_l_to_lane_center = init_sl_state.second[0]; + // 计算自车在横向加速过程中的速度缓冲 static constexpr double kMaxLateralAccelerations = 1.5; - + // 如果速度大于零，缓冲区值为正（表示加速），如果速度小于零，缓冲区值为负（表示减速） + // 通过公式 v² / (2 * a) 来计算需要的缓冲区 double ADC_speed_buffer = (init_sl_state.second[1] > 0 ? 1.0 : -1.0) * init_sl_state.second[1] * init_sl_state.second[1] / kMaxLateralAccelerations / 2.0; double adc_half_width = VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0; + // left_bound_adc 是左侧边界，取自车的当前位置与速度缓冲区的最大值，后面加上自车的半宽度和扩展缓冲区 double left_bound_adc = std::fmax(adc_l_to_lane_center, adc_l_to_lane_center + ADC_speed_buffer) + adc_half_width + extend_buffer; + // right_bound_adc 是右侧边界，取自车的当前位置与速度缓冲区的最小值，减去自车的半宽度和扩展缓冲区 double right_bound_adc = std::fmin(adc_l_to_lane_center, adc_l_to_lane_center + ADC_speed_buffer) - adc_half_width - extend_buffer; @@ -547,7 +642,8 @@ bool PathBoundsDeciderUtil::ExtendBoundaryByADC( (*path_bound)[i].s, &road_left_width, &road_right_width); double left_bound_road = road_left_width - adc_half_width; double right_bound_road = -road_right_width + adc_half_width; - + // 如果计算出的左边界（left_bound_adc）大于当前路径边界的上边界（l_upper.l）， + // 则更新上边界为新的左边界，并将边界类型设置为 ADC（表示自车的边界），并将 ID 设置为 "adc" if (left_bound_adc > (*path_bound)[i].l_upper.l) { (*path_bound)[i].l_upper.l = std::max(std::min(left_bound_adc, left_bound_road), @@ -555,6 +651,8 @@ bool PathBoundsDeciderUtil::ExtendBoundaryByADC( (*path_bound)[i].l_upper.type = BoundType::ADC; (*path_bound)[i].l_upper.id = "adc"; } + //如果计算出的右边界（right_bound_adc）小于当前路径边界的下边界（l_lower.l）， + //则更新下边界为新的右边界，并将边界类型设置为 ADC，ID 设置为 "adc" if (right_bound_adc < (*path_bound)[i].l_lower.l) { (*path_bound)[i].l_lower.l = std::min(std::max(right_bound_adc, right_bound_road), diff --git a/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.h b/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.h index 4bd5ad42d76..b5d3ab95b2f 100644 --- a/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.h +++ b/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.h @@ -115,6 +115,16 @@ class PathBoundsDeciderUtil { std::string* const blocking_obstacle_id, double* const narrowest_width, LaneBorrowInfo lane_borrow_info = LaneBorrowInfo::NO_BORROW); +// The tuple contains (is_start_s, s, l_min, l_max, obstacle_id) +// bool is_start_s: 障碍物是起始边界(1)还是结束边界(0) +// s:障碍物的纵向位置 +// l_min:障碍物在横向上的最小横向位置 +// l_max:障碍物在横向上的最大横向位置 +// obstacle_id: 障碍物的id + /// @brief + /// @param indexed_obstacles + /// @param init_sl_state + /// @return static std::vector SortObstaclesForSweepLine( const IndexedList& indexed_obstacles, const SLState& init_sl_state); diff --git a/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util_test10.cc b/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util_test10.cc new file mode 100644 index 00000000000..ed971d6e827 --- /dev/null +++ b/modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util_test10.cc @@ -0,0 +1,1228 @@ +/****************************************************************************** + * 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/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "modules/common/configs/vehicle_config_helper.h" +#include "modules/common/math/linear_interpolation.h" +#include "modules/common/util/util.h" +#include "modules/planning/planning_base/common/sl_polygon.h" +#include "modules/planning/planning_base/common/util/util.h" +#include "modules/planning/planning_base/gflags/planning_gflags.h" + +namespace apollo { +namespace planning { + +using apollo::common::VehicleConfigHelper; + +bool PathBoundsDeciderUtil::InitPathBoundary( + const ReferenceLineInfo& reference_line_info, + PathBoundary* const path_bound, SLState init_sl_state) { + // Sanity checks. + CHECK_NOTNULL(path_bound); + path_bound->clear(); + const auto& reference_line = reference_line_info.reference_line(); + path_bound->set_delta_s(FLAGS_path_bounds_decider_resolution); + + const auto& vehicle_config = + common::VehicleConfigHelper::Instance()->GetConfig(); + const double ego_front_to_center = + vehicle_config.vehicle_param().front_edge_to_center(); + double index = 0; + const auto& reference_line_towing_l = + reference_line_info.reference_line_towing_l(); + for (double curr_s = init_sl_state.first[0]; + curr_s < std::fmin(init_sl_state.first[0] + + std::fmax(FLAGS_path_bounds_horizon, + reference_line_info.GetCruiseSpeed() * + FLAGS_trajectory_time_length), + reference_line.Length() - ego_front_to_center); + curr_s += FLAGS_path_bounds_decider_resolution) { + path_bound->emplace_back(curr_s, std::numeric_limits::lowest(), + std::numeric_limits::max()); + if (index < reference_line_towing_l.size()) { + path_bound->back().towing_l = reference_line_towing_l.at(index); + } + index++; + } + + // Return. + if (path_bound->empty()) { + ADEBUG << "Empty path boundary in InitPathBoundary"; + return false; + } + return true; +} + +void PathBoundsDeciderUtil::GetStartPoint( + common::TrajectoryPoint planning_start_point, + const ReferenceLine& reference_line, SLState* init_sl_state) { + if (FLAGS_use_front_axe_center_in_path_planning) { + planning_start_point = + InferFrontAxeCenterFromRearAxeCenter(planning_start_point); + } + AINFO << std::fixed << "Plan at the starting point: x = " + << planning_start_point.path_point().x() + << ", y = " << planning_start_point.path_point().y() + << ", and angle = " << planning_start_point.path_point().theta(); + + // Initialize some private variables. + // ADC s/l info. + *init_sl_state = reference_line.ToFrenetFrame(planning_start_point); +} + +double PathBoundsDeciderUtil::GetADCLaneWidth( + const ReferenceLine& reference_line, const double adc_s) { + double lane_left_width = 0.0; + double lane_right_width = 0.0; + if (!reference_line.GetLaneWidth(adc_s, &lane_left_width, + &lane_right_width)) { + constexpr double kDefaultLaneWidth = 5.0; + AWARN << "Failed to get lane width at planning start point."; + return kDefaultLaneWidth; + } else { + return lane_left_width + lane_right_width; + } +} + +bool PathBoundsDeciderUtil::UpdatePathBoundaryWithBuffer( + double left_bound, double right_bound, BoundType left_type, + BoundType right_type, std::string left_id, std::string right_id, + PathBoundPoint* const bound_point) { + if (!UpdateLeftPathBoundaryWithBuffer(left_bound, left_type, left_id, + bound_point)) { + return false; + } + if (!UpdateRightPathBoundaryWithBuffer(right_bound, right_type, right_id, + bound_point)) { + return false; + } + return true; +} + +bool PathBoundsDeciderUtil::UpdateLeftPathBoundaryWithBuffer( + double left_bound, BoundType left_type, std::string left_id, + PathBoundPoint* const bound_point) { + double adc_half_width = + VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0; + left_bound = left_bound - adc_half_width; + PathBoundPoint new_point = *bound_point; + if (new_point.l_upper.l > left_bound) { + new_point.l_upper.l = left_bound; + new_point.l_upper.type = left_type; + new_point.l_upper.id = left_id; + } + // Check if ADC is blocked. + // If blocked, don't update anything, return false. + if (new_point.l_lower.l > new_point.l_upper.l) { + ADEBUG << "Path is blocked at" << new_point.l_lower.l << " " + << new_point.l_upper.l; + return false; + } + // Otherwise, update path_boundaries and center_line; then return true. + *bound_point = new_point; + return true; +} + +bool PathBoundsDeciderUtil::UpdateRightPathBoundaryWithBuffer( + double right_bound, BoundType right_type, std::string right_id, + PathBoundPoint* const bound_point) { + double adc_half_width = + VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0; + right_bound = right_bound + adc_half_width; + PathBoundPoint new_point = *bound_point; + if (new_point.l_lower.l < right_bound) { + new_point.l_lower.l = right_bound; + new_point.l_lower.type = right_type; + new_point.l_lower.id = right_id; + } + // Check if ADC is blocked. + // If blocked, don't update anything, return false. + if (new_point.l_lower.l > new_point.l_upper.l) { + ADEBUG << "Path is blocked at"; + return false; + } + // Otherwise, update path_boundaries and center_line; then return true. + *bound_point = new_point; + return true; +} + +void PathBoundsDeciderUtil::TrimPathBounds( + const int path_blocked_idx, PathBoundary* const path_boundaries) { + if (path_blocked_idx != -1) { + if (path_blocked_idx == 0) { + AINFO << "Completely blocked. Cannot move at all."; + } + double front_edge_to_center = + VehicleConfigHelper::GetConfig().vehicle_param().front_edge_to_center(); + double trimmed_s = + path_boundaries->at(path_blocked_idx).s - front_edge_to_center; + AINFO << "Trimmed from " << path_boundaries->back().s << " to " + << path_boundaries->at(path_blocked_idx).s; + while (path_boundaries->size() > 1 && + path_boundaries->back().s > trimmed_s) { + path_boundaries->pop_back(); + } + } +} + +// 从 path_decision 中提取静态障碍物，转换为 SLPolygon，并按 s 排序 +void PathBoundsDeciderUtil::GetSLPolygons( + const ReferenceLineInfo& reference_line_info, + std::vector* polygons, const SLState& init_sl_state) { + polygons->clear(); + auto obstacles = reference_line_info.path_decision().obstacles(); + const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s(); + for (const auto* obstacle : obstacles.Items()) { + // 过滤条件： 非虚拟障碍物 不是 "Ignore" 决策 是静态的（速度 < 阈值） 障碍物的 s_end >= ego 车后端 s（即在前方） + if (!IsWithinPathDeciderScopeObstacle(*obstacle)) { + continue; + } + auto xy_poly = obstacle->PerceptionPolygon(); + + // if (obstacle->PerceptionSLBoundary().end_s() < init_sl_state.first[0]) { + // continue; + // } + if (obstacle->PerceptionSLBoundary().end_s() < adc_back_edge_s) { + continue; + } + const auto obstacle_sl = obstacle->PerceptionSLBoundary(); + polygons->emplace_back(obstacle_sl, obstacle->Id(), + obstacle->Perception().type()); + } + sort(polygons->begin(), polygons->end(), + [](const SLPolygon& a, const SLPolygon& b) { + return a.MinS() < b.MinS(); + }); +} + + +// 处理障碍物对边界的限制 +// 遍历路径上的每个 s 点，检查所有与之相交的障碍物，根据“绕行方向”（左/右绕）更新边界。 +bool PathBoundsDeciderUtil::UpdatePathBoundaryBySLPolygon( + const ReferenceLineInfo& reference_line_info, + std::vector* const sl_polygon, const SLState& init_sl_state, + PathBoundary* const path_boundary, std::string* const blocked_id, + double* const narrowest_width) { + // (1) 初始化中心线策略 + // 变道 or 左/右 regular 路径 → 使用当前边界中心作为参考 + // (2) 设置最大绕行检查距离（nudge_check_distance） + // 变道时用 FLAGS_max_nudge_check_distance_in_lc + // 跟车时用 FLAGS_max_nudge_check_distance_in_lk + std::vector center_l; + double max_nudge_check_distance; + if (reference_line_info.IsChangeLanePath() || + path_boundary->label().find("regular/left") != std::string::npos || + path_boundary->label().find("regular/right") != std::string::npos) { + center_l.push_back( + (path_boundary->front().l_upper.l + path_boundary->front().l_lower.l) * + 0.5); + max_nudge_check_distance = FLAGS_max_nudge_check_distance_in_lk; + } else { + center_l.push_back(0.0); + max_nudge_check_distance = FLAGS_max_nudge_check_distance_in_lc; + } + + *narrowest_width = + path_boundary->front().l_upper.l - path_boundary->front().l_lower.l; + double mid_l = + (path_boundary->front().l_upper.l + path_boundary->front().l_lower.l) / 2; + size_t nudge_check_count = + size_t(max_nudge_check_distance / FLAGS_path_bounds_decider_resolution); + double last_max_nudge_l = center_l.front(); + // bool obs_overlap_with_refer_center = false; + // (3) 对每个 s 点（i 从 1 开始） + // 获取当前边界 [l_lower, l_upper] + // 查找所有 sl_polygon 中与当前 s 相交的障碍物（s ∈ [min_s, max_s]） + for (size_t i = 1; i < path_boundary->size(); ++i) { + double path_boundary_s = path_boundary->at(i).s; + auto& left_bound = path_boundary->at(i).l_upper; + auto& right_bound = path_boundary->at(i).l_lower; + double default_width = right_bound.l - left_bound.l; + auto begin_it = + center_l.end() - std::min(nudge_check_count, center_l.size()); + last_max_nudge_l = *std::max_element( + begin_it, center_l.end(), + [](double a, double b) { return std::fabs(a) < std::fabs(b); }); + AINFO << "last max nudge l: " << last_max_nudge_l; + for (size_t j = 0; j < sl_polygon->size(); j++) { + if (sl_polygon->at(j).NudgeInfo() == SLPolygon::IGNORE) { + AINFO << "UpdatePathBoundaryBySLPolygon, ignore obs: " + << sl_polygon->at(j).id(); + continue; + } + double min_s = sl_polygon->at(j).MinS(); + double max_s = + sl_polygon->at(j).MaxS() + FLAGS_obstacle_lon_end_buffer_park; + if (max_s - min_s < FLAGS_path_bounds_decider_resolution) { + max_s += FLAGS_path_bounds_decider_resolution; + min_s -= FLAGS_path_bounds_decider_resolution; + } + if (max_s < path_boundary_s) { + continue; + } + if (min_s > path_boundary_s) { + break; + } + double adc_obs_edge_buffer = GetBufferBetweenADCCenterAndEdge(); + // (4) 对每个障碍物： + // 调用 UpdatePassableInfo() 判断左右是否可通过 + sl_polygon->at(j).UpdatePassableInfo(left_bound.l, right_bound.l, + adc_obs_edge_buffer); + // 计算障碍物在当前 s 处的左右边界 l_lower_obs, l_upper_obs + double l_lower = sl_polygon->at(j).GetRightBoundaryByS(path_boundary_s); + double l_upper = sl_polygon->at(j).GetLeftBoundaryByS(path_boundary_s); + // 添加安全缓冲 adc_obs_edge_buffer = 半车宽 + FLAGS_obstacle_lat_buffer + PathBoundPoint obs_left_nudge_bound( + path_boundary_s, l_upper + adc_obs_edge_buffer, left_bound.l); + obs_left_nudge_bound.towing_l = path_boundary->at(i).towing_l; + PathBoundPoint obs_right_nudge_bound(path_boundary_s, right_bound.l, + l_lower - adc_obs_edge_buffer); + obs_right_nudge_bound.towing_l = path_boundary->at(i).towing_l; + // obs_overlap_with_refer_center = + // left_bound.l < path_boundary->at(i).towing_l || + // right_bound.l > path_boundary->at(i).towing_l; + + // (5) 自动决定绕行方向（Nudge Direction） + if (sl_polygon->at(j).NudgeInfo() == SLPolygon::UNDEFINED) { + AINFO << "last_max_nudge_l: " << last_max_nudge_l + << ", obs id: " << sl_polygon->at(j).id() + << ", obs l: " << l_lower << ", " << l_upper; + double obs_l = (l_lower + l_upper) / 2; + if (sl_polygon->at(j).is_passable()[RIGHT_INDEX]) { + if (sl_polygon->at(j).is_passable()[LEFT_INDEX]) { + // 若障碍物靠近中心（<0.4m）且在起步 5m 内 → 根据 ego 初始 l 速度方向决定绕行 + if (std::fabs(obs_l - mid_l) < 0.4 && + std::fabs(path_boundary_s - init_sl_state.first[0]) < 5.0) { + if (init_sl_state.second[0] < obs_l) { + sl_polygon->at(j).SetNudgeInfo(SLPolygon::RIGHT_NUDGE); + AINFO << sl_polygon->at(j).id() + << " right nudge with init_sl_state"; + } else { + sl_polygon->at(j).SetNudgeInfo(SLPolygon::LEFT_NUDGE); + AINFO << sl_polygon->at(j).id() + << " left nudge width init_sl_state"; + } + } else { + // 否则 → 根据最近的最大绕行偏移 last_max_nudge_l 决定（避免来回晃） + if (last_max_nudge_l < obs_l) { + sl_polygon->at(j).SetNudgeInfo(SLPolygon::RIGHT_NUDGE); + AINFO << sl_polygon->at(j).id() + << " right nudge, according max_nudge_l"; + } else { + sl_polygon->at(j).SetNudgeInfo(SLPolygon::LEFT_NUDGE); + AINFO << sl_polygon->at(j).id() + << " left nudge, according max_nudge_l"; + } + } + } else { + // 如果只有右可通过 → 必须右绕 + sl_polygon->at(j).SetNudgeInfo(SLPolygon::RIGHT_NUDGE); + AINFO << sl_polygon->at(j).id() + << " right nudge, left is not passable"; + } + } else { + // 如果只有左可通过 → 必须左绕 + sl_polygon->at(j).SetNudgeInfo(SLPolygon::LEFT_NUDGE); + AINFO << sl_polygon->at(j).id() + << " left nudge, right is not passable"; + } + } else { + AINFO << "last_max_nudge_l: " << last_max_nudge_l + << ", obs id: " << sl_polygon->at(j).id() + << ", obs l: " << l_lower << ", " << l_upper + << ", nudge info: " << sl_polygon->at(j).NudgeInfo(); + } + + // (6) 根据绕行方向更新边界： + if (sl_polygon->at(j).NudgeInfo() == SLPolygon::RIGHT_NUDGE) { + // right nudge + // 新左边界 = obs_right_nudge_bound.l_upper.l（即障碍物右边界 + 缓冲） + if (obs_right_nudge_bound.l_upper.l < path_boundary->at(i).towing_l) { + sl_polygon->at(j).SetOverlapeWithReferCenter(true); + sl_polygon->at(j).SetOverlapeSizeWithReference( + path_boundary->at(i).towing_l - obs_right_nudge_bound.l_upper.l); + } + if (!sl_polygon->at(j).is_passable()[RIGHT_INDEX]) { + // boundary is blocked + *blocked_id = sl_polygon->at(j).id(); + AINFO << "blocked at " << *blocked_id << ", s: " << path_boundary_s + << ", left bound: " << left_bound.l + << ", right bound: " << right_bound.l; + sl_polygon->at(j).SetNudgeInfo(SLPolygon::BLOCKED); + break; + } + // 如果该值 < 当前左边界 → 更新左边界 + if (obs_right_nudge_bound.l_upper.l < left_bound.l) { + AINFO << "update left_bound: s " << path_boundary_s << ", l " + << left_bound.l << " -> " << obs_right_nudge_bound.l_upper.l; + left_bound.l = obs_right_nudge_bound.l_upper.l; + left_bound.type = BoundType::OBSTACLE; + left_bound.id = sl_polygon->at(j).id(); + *narrowest_width = + std::min(*narrowest_width, left_bound.l - right_bound.l); + } + } else if (sl_polygon->at(j).NudgeInfo() == SLPolygon::LEFT_NUDGE) { + // left nudge + // 新右边界 = obs_left_nudge_bound.l_lower.l（障碍物左边界 - 缓冲） + if (obs_left_nudge_bound.l_lower.l > path_boundary->at(i).towing_l) { + sl_polygon->at(j).SetOverlapeWithReferCenter(true); + sl_polygon->at(j).SetOverlapeSizeWithReference( + obs_left_nudge_bound.l_lower.l - path_boundary->at(i).towing_l); + } + if (!sl_polygon->at(j).is_passable()[LEFT_INDEX]) { + // boundary is blocked + *blocked_id = sl_polygon->at(j).id(); + AINFO << "blocked at " << *blocked_id << ", s: " << path_boundary_s + << ", left bound: " << left_bound.l + << ", right bound: " << right_bound.l; + sl_polygon->at(j).SetNudgeInfo(SLPolygon::BLOCKED); + break; + } + // 如果 > 当前右边界 → 更新右边界 + if (obs_left_nudge_bound.l_lower.l > right_bound.l) { + AINFO << "update right_bound: s " << path_boundary_s << ", l " + << right_bound.l << " -> " << obs_left_nudge_bound.l_lower.l; + right_bound.l = obs_left_nudge_bound.l_lower.l; + right_bound.type = BoundType::OBSTACLE; + right_bound.id = sl_polygon->at(j).id(); + *narrowest_width = + std::min(*narrowest_width, left_bound.l - right_bound.l); + } + } + // obs_overlap_with_refer_center = + // left_bound.l < path_boundary->at(i).towing_l || + // right_bound.l > path_boundary->at(i).towing_l; + + // double current_center_l = obs_overlap_with_refer_center + // ? (left_bound.l + right_bound.l) / 2.0 + // : path_boundary->at(i).towing_l; + // last_max_nudge_l = std::fabs(current_center_l - mid_l) > + // std::fabs(last_max_nudge_l - mid_l) + // ? current_center_l + // : last_max_nudge_l; + last_max_nudge_l = std::fabs((left_bound.l + right_bound.l) / 2.0 - + mid_l) > std::fabs(last_max_nudge_l - mid_l) + ? (left_bound.l + right_bound.l) / 2.0 + : last_max_nudge_l; + } + // if blocked, trim path + if (!blocked_id->empty()) { + TrimPathBounds(i, path_boundary); + *narrowest_width = default_width; + return false; + } + // double current_center_l = obs_overlap_with_refer_center + // ? (left_bound.l + right_bound.l) / 2.0 + // : path_boundary->at(i).towing_l; + // center_l.push_back(current_center_l); + center_l.push_back((left_bound.l + right_bound.l) / 2.0); + AINFO << "update s: " << path_boundary_s + << ", center_l: " << center_l.back(); + } + return true; +} + +bool PathBoundsDeciderUtil::AddCornerPoint( + double s, double l_lower, double l_upper, const PathBoundary& path_boundary, + ObsCornerConstraints* extra_constraints) { + size_t left_index = 0; + size_t right_index = 0; + double left_weight = 0.0; + double right_weight = 0.0; + if (!path_boundary.get_interpolated_s_weight(s, &left_weight, &right_weight, + &left_index, &right_index)) { + AERROR << "Fail to find extra path bound point in path boundary: " << s + << ", path boundary start s: " << path_boundary.front().s + << ", path boundary end s: " << path_boundary.back().s; + return false; + } + if (left_weight < 0.05 || right_weight < 0.05) { + // filter contraint that near evaulated point + return false; + } + ADEBUG << "corner" << s << "left_weight" << left_weight << "right_weight" + << right_weight << "left_index" << left_index << "right_index" + << right_index << "l_lower" << l_lower << "l_upper" << l_upper; + extra_constraints->emplace_back(left_weight, right_weight, l_lower, l_upper, + left_index, right_index, s); + return true; +} + +bool PathBoundsDeciderUtil::AddCornerPoint( + SLPoint sl_pt, const PathBoundary& path_boundary, + ObsCornerConstraints* extra_constraints, bool is_left, std::string obs_id, + bool is_front_pt) { + size_t left_index = 0; + size_t right_index = 0; + double left_weight = 0.0; + double right_weight = 0.0; + if (!path_boundary.get_interpolated_s_weight( + sl_pt.s(), &left_weight, &right_weight, &left_index, &right_index)) { + AERROR << "Fail to find extra path bound point in path boundary: " + << sl_pt.s() + << ", path boundary start s: " << path_boundary.front().s + << ", path boundary end s: " << path_boundary.back().s; + return true; + } + // if (left_weight < 0.05 || right_weight < 0.05) { + // // filter contraint that near evaulated point + // return true; + // } + + double bound_l_upper = path_boundary.get_upper_bound_by_interpolated_index( + left_weight, right_weight, left_index, right_index); + double bound_l_lower = path_boundary.get_lower_bound_by_interpolated_index( + left_weight, right_weight, left_index, right_index); + + double corner_l = is_left ? sl_pt.l() - GetBufferBetweenADCCenterAndEdge() + : sl_pt.l() + GetBufferBetweenADCCenterAndEdge(); + if ((is_left && corner_l < bound_l_upper) || + (!is_left && corner_l > bound_l_lower)) { + if (is_left) { + bound_l_upper = corner_l; + } else { + bound_l_lower = corner_l; + } + extra_constraints->emplace_back(left_weight, right_weight, bound_l_lower, + bound_l_upper, left_index, right_index, + sl_pt.s(), obs_id); + if (bound_l_upper < bound_l_lower) { + extra_constraints->blocked_id = obs_id; + extra_constraints->block_left_index = left_index; + extra_constraints->block_right_index = right_index; + AINFO << "AddCornerPoint blocked id: " << obs_id << ", index [" + << left_index << ", " << right_index << "]"; + return false; + } + } + + if (FLAGS_enable_expand_obs_corner) { + double add_s = is_front_pt ? sl_pt.s() - FLAGS_expand_obs_corner_lon_buffer + : sl_pt.s() + FLAGS_expand_obs_corner_lon_buffer; + if (!path_boundary.get_interpolated_s_weight( + add_s, &left_weight, &right_weight, &left_index, &right_index)) { + return true; + } + + bound_l_upper = path_boundary.get_upper_bound_by_interpolated_index( + left_weight, right_weight, left_index, right_index); + bound_l_lower = path_boundary.get_lower_bound_by_interpolated_index( + left_weight, right_weight, left_index, right_index); + + if ((is_left && corner_l < bound_l_upper) || + (!is_left && corner_l > bound_l_lower)) { + if (is_left) { + bound_l_upper = corner_l; + } else { + bound_l_lower = corner_l; + } + extra_constraints->emplace_back(left_weight, right_weight, bound_l_lower, + bound_l_upper, left_index, right_index, + add_s, obs_id); + if (bound_l_upper < bound_l_lower) { + extra_constraints->blocked_id = obs_id; + extra_constraints->block_left_index = left_index; + extra_constraints->block_right_index = right_index; + AINFO << "AddCornerPoint blocked id: " << obs_id << ", index [" + << left_index << ", " << right_index << "]"; + return false; + } + } + } + return true; +} + +// 遍历所有 SLPolygon，对其绕行侧的边界点调用 AddCornerPoint。 +// RIGHT_NUDGE → 添加右边界的点（作为左约束） +// LEFT_NUDGE → 添加左边界的点（作为右约束） +void PathBoundsDeciderUtil::AddCornerBounds( + const std::vector& sl_polygons, + PathBoundary* const path_boundary) { + auto* extra_path_bound = path_boundary->mutable_extra_path_bound(); + for (const auto& obs_polygon : sl_polygons) { + if (obs_polygon.MinS() > path_boundary->back().s) { + ADEBUG << "obs_polygon.MinS()" << obs_polygon.MinS() + << "path_boundary->back().s" << path_boundary->back().s; + break; + } + if (obs_polygon.MaxS() < path_boundary->front().s) { + continue; + } + if (obs_polygon.NudgeInfo() == SLPolygon::LEFT_NUDGE) { + for (size_t i = 0; i < obs_polygon.LeftBoundary().size(); i++) { + auto pt = obs_polygon.LeftBoundary().at(i); + bool is_front_pt = i < (obs_polygon.LeftBoundary().size() * 0.5); + if (!AddCornerPoint(pt, *path_boundary, extra_path_bound, false, + obs_polygon.id(), is_front_pt)) { + break; + } + } + // for (auto pt : obs_polygon.LeftBoundary()) { + // if (!AddCornerPoint(pt, *path_boundary, extra_path_bound, false, + // obs_polygon.id())) { + // break; + // } + // } + } else if (obs_polygon.NudgeInfo() == SLPolygon::RIGHT_NUDGE) { + for (size_t i = 0; i < obs_polygon.RightBoundary().size(); i++) { + auto pt = obs_polygon.RightBoundary().at(i); + bool is_front_pt = i > (obs_polygon.RightBoundary().size() * 0.5); + if (!AddCornerPoint(pt, *path_boundary, extra_path_bound, true, + obs_polygon.id(), is_front_pt)) { + break; + } + } + // for (auto pt : obs_polygon.RightBoundary()) { + // if (!AddCornerPoint(pt, *path_boundary, extra_path_bound, true, + // obs_polygon.id())) { + // break; + // } + // } + } else { + AINFO << "no nugde info, ignore obs: " << obs_polygon.id(); + } + if (!extra_path_bound->blocked_id.empty()) { + break; + } + } + // sort(extra_path_bound->begin(), extra_path_bound->end(), + // [](const InterPolatedPoint& a, const InterPolatedPoint& b) { + // return a.rear_axle_s < b.rear_axle_s; + // }); +} + +// 添加车辆顶点（如前保险杠角点）的约束，防止车辆旋转时出界。 +void PathBoundsDeciderUtil::AddAdcVertexBounds( + PathBoundary* const path_boundary) { + auto* adc_vertex_bound = path_boundary->mutable_adc_vertex_bound(); + // front_edge_to_center in Apollo is the front edge to rear center + double front_edge_to_center = apollo::common::VehicleConfigHelper::GetConfig() + .vehicle_param() + .front_edge_to_center(); + // 对每个路径点 s，计算后轴位置 = s - front_edge_to_center + for (size_t i = 0; i < path_boundary->size(); i++) { + double rear_axle_s = path_boundary->at(i).s - front_edge_to_center; + if (rear_axle_s <= path_boundary->start_s()) { + continue; + } + size_t left_index = 0; + size_t right_index = 0; + double left_weight = 0.0; + double right_weight = 0.0; + // 在该后轴 s 处插值得到上下边界 + if (!path_boundary->get_interpolated_s_weight(rear_axle_s, &left_weight, + &right_weight, &left_index, + &right_index)) { + AERROR << "Fail to find vertex path bound point in path boundary: " + << path_boundary->at(i).s + << "path boundary start s: " << path_boundary->front().s + << ", path boundary end s: " << path_boundary->back().s; + continue; + } + // 将原始边界（未缩进）作为顶点约束加入 adc_vertex_bound + adc_vertex_bound->emplace_back( + left_weight, right_weight, path_boundary->at(i).l_lower.l, + path_boundary->at(i).l_upper.l, left_index, right_index, rear_axle_s); + } + adc_vertex_bound->front_edge_to_center = front_edge_to_center; +} + +bool PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles( + const ReferenceLineInfo& reference_line_info, + std::vector* const sl_polygons, const SLState& init_sl_state, + PathBoundary* const path_boundary, std::string* const blocking_obstacle_id, + double* const narrowest_width) { + UpdatePathBoundaryBySLPolygon(reference_line_info, sl_polygons, init_sl_state, + path_boundary, blocking_obstacle_id, + narrowest_width); + AddExtraPathBound(*sl_polygons, path_boundary, init_sl_state, + blocking_obstacle_id); + return true; +} + +double PathBoundsDeciderUtil::GetBufferBetweenADCCenterAndEdge() { + double adc_half_width = + VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0; + + return (adc_half_width + FLAGS_obstacle_lat_buffer); +} + +bool PathBoundsDeciderUtil::IsWithinPathDeciderScopeObstacle( + const Obstacle& obstacle) { + // Obstacle should be non-virtual. + if (obstacle.IsVirtual()) { + return false; + } + // Obstacle should not have ignore decision. + if (obstacle.HasLongitudinalDecision() && obstacle.HasLateralDecision() && + obstacle.IsIgnore()) { + return false; + } + // Obstacle should not be moving obstacle. + if (!obstacle.IsStatic() || + obstacle.speed() > FLAGS_static_obstacle_speed_threshold) { + return false; + } + // TODO(jiacheng): + // Some obstacles are not moving, but only because they are waiting for + // red light (traffic rule) or because they are blocked by others (social). + // These obstacles will almost certainly move in the near future and we + // should not side-pass such obstacles. + + return true; +} + +// 根据后轴中心 + 车轮距（wheel_base），推算前轴中心坐标。 +common::TrajectoryPoint +PathBoundsDeciderUtil::InferFrontAxeCenterFromRearAxeCenter( + const common::TrajectoryPoint& traj_point) { + double front_to_rear_axe_distance = + VehicleConfigHelper::GetConfig().vehicle_param().wheel_base(); + common::TrajectoryPoint ret = traj_point; + ret.mutable_path_point()->set_x( + traj_point.path_point().x() + + front_to_rear_axe_distance * std::cos(traj_point.path_point().theta())); + ret.mutable_path_point()->set_y( + traj_point.path_point().y() + + front_to_rear_axe_distance * std::sin(traj_point.path_point().theta())); + return ret; +} + +bool PathBoundsDeciderUtil::GetBoundaryFromSelfLane( + const ReferenceLineInfo& reference_line_info, const SLState& init_sl_state, + PathBoundary* const path_bound) { + // Sanity checks. + CHECK_NOTNULL(path_bound); + ACHECK(!path_bound->empty()); + const ReferenceLine& reference_line = reference_line_info.reference_line(); + double adc_lane_width = + GetADCLaneWidth(reference_line, init_sl_state.first[0]); + // Go through every point, update the boundary based on lane info and + // ADC's position. + double past_lane_left_width = adc_lane_width / 2.0; + double past_lane_right_width = adc_lane_width / 2.0; + int path_blocked_idx = -1; + for (size_t i = 0; i < path_bound->size(); ++i) { + double curr_s = (*path_bound)[i].s; + // 1. Get the current lane width at current point. + double curr_lane_left_width = 0.0; + double curr_lane_right_width = 0.0; + double offset_to_lane_center = 0.0; + if (!reference_line.GetLaneWidth(curr_s, &curr_lane_left_width, + &curr_lane_right_width)) { + AWARN << "Failed to get lane width at s = " << curr_s; + curr_lane_left_width = past_lane_left_width; + curr_lane_right_width = past_lane_right_width; + } else { + 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; + past_lane_left_width = curr_lane_left_width; + past_lane_right_width = curr_lane_right_width; + } + + // 3. Calculate the proper boundary based on lane-width, ADC's position, + // and ADC's velocity. + double offset_to_map = 0.0; + reference_line.GetOffsetToMap(curr_s, &offset_to_map); + + double curr_left_bound = 0.0; + double curr_right_bound = 0.0; + curr_left_bound = curr_lane_left_width - offset_to_map; + curr_right_bound = -curr_lane_right_width - offset_to_map; + // 4. Update the boundary. + if (!UpdatePathBoundaryWithBuffer(curr_left_bound, curr_right_bound, + BoundType::LANE, BoundType::LANE, "", "", + &path_bound->at(i))) { + path_blocked_idx = static_cast(i); + } + if (path_blocked_idx != -1) { + break; + } + } + + PathBoundsDeciderUtil::TrimPathBounds(path_blocked_idx, path_bound); + + return true; +} + +bool PathBoundsDeciderUtil::GetBoundaryFromRoad( + const ReferenceLineInfo& reference_line_info, const SLState& init_sl_state, + PathBoundary* const path_bound) { + // Sanity checks. + CHECK_NOTNULL(path_bound); + ACHECK(!path_bound->empty()); + const ReferenceLine& reference_line = reference_line_info.reference_line(); + double adc_lane_width = + GetADCLaneWidth(reference_line, init_sl_state.first[0]); + // Go through every point, update the boudnary based on the road boundary. + double past_road_left_width = adc_lane_width / 2.0; + double past_road_right_width = adc_lane_width / 2.0; + int path_blocked_idx = -1; + for (size_t i = 0; i < path_bound->size(); ++i) { + // 1. Get road boundary. + double curr_s = (*path_bound)[i].s; + double curr_road_left_width = 0.0; + double curr_road_right_width = 0.0; + if (!reference_line.GetRoadWidth(curr_s, &curr_road_left_width, + &curr_road_right_width)) { + AWARN << "Failed to get lane width at s = " << curr_s; + curr_road_left_width = past_road_left_width; + curr_road_right_width = past_road_right_width; + } + + past_road_left_width = curr_road_left_width; + past_road_right_width = curr_road_right_width; + + double curr_left_bound = curr_road_left_width; + double curr_right_bound = -curr_road_right_width; + ADEBUG << "At s = " << curr_s + << ", left road bound = " << curr_road_left_width + << ", right road bound = " << curr_road_right_width; + + // 2. Update into path_bound. + if (!UpdatePathBoundaryWithBuffer(curr_left_bound, curr_right_bound, + BoundType::ROAD, BoundType::ROAD, "", "", + &path_bound->at(i))) { + path_blocked_idx = static_cast(i); + } + if (path_blocked_idx != -1) { + break; + } + } + AINFO << "path_blocked_idx: " << path_blocked_idx; + TrimPathBounds(path_blocked_idx, path_bound); + return true; +} + +bool PathBoundsDeciderUtil::ExtendBoundaryByADC( + const ReferenceLineInfo& reference_line_info, const SLState& init_sl_state, + const double extend_buffer, PathBoundary* const path_bound) { + double adc_l_to_lane_center = init_sl_state.second[0]; + static constexpr double kMaxLateralAccelerations = 1.5; + + double ADC_speed_buffer = (init_sl_state.second[1] > 0 ? 1.0 : -1.0) * + init_sl_state.second[1] * init_sl_state.second[1] / + kMaxLateralAccelerations / 2.0; + double adc_half_width = + VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0; + double left_bound_adc = + std::fmax(adc_l_to_lane_center, adc_l_to_lane_center + ADC_speed_buffer) + + adc_half_width + extend_buffer; + double right_bound_adc = + std::fmin(adc_l_to_lane_center, adc_l_to_lane_center + ADC_speed_buffer) - + adc_half_width - extend_buffer; + + static constexpr double kEpsilon = 0.05; + for (size_t i = 0; i < path_bound->size(); ++i) { + double road_left_width = std::fabs(left_bound_adc) + kEpsilon; + double road_right_width = std::fabs(right_bound_adc) + kEpsilon; + reference_line_info.reference_line().GetRoadWidth( + (*path_bound)[i].s, &road_left_width, &road_right_width); + double left_bound_road = road_left_width - adc_half_width; + double right_bound_road = -road_right_width + adc_half_width; + + if (left_bound_adc > (*path_bound)[i].l_upper.l) { + (*path_bound)[i].l_upper.l = + std::max(std::min(left_bound_adc, left_bound_road), + (*path_bound)[i].l_upper.l); + (*path_bound)[i].l_upper.type = BoundType::ADC; + (*path_bound)[i].l_upper.id = "adc"; + } + if (right_bound_adc < (*path_bound)[i].l_lower.l) { + (*path_bound)[i].l_lower.l = + std::min(std::max(right_bound_adc, right_bound_road), + (*path_bound)[i].l_lower.l); + (*path_bound)[i].l_lower.type = BoundType::ADC; + (*path_bound)[i].l_lower.id = "adc"; + } + } + return true; +} + +// 将边界从“车道中心”坐标系转换为“参考线”坐标系（通过 offset_to_lane_center）。 +void PathBoundsDeciderUtil::ConvertBoundarySAxisFromLaneCenterToRefLine( + const ReferenceLineInfo& reference_line_info, + PathBoundary* const path_bound) { + const ReferenceLine& reference_line = reference_line_info.reference_line(); + for (size_t i = 0; i < path_bound->size(); ++i) { + // 1. Get road boundary. + double curr_s = (*path_bound)[i].s; + double refline_offset_to_lane_center = 0.0; + reference_line.GetOffsetToMap(curr_s, &refline_offset_to_lane_center); + (*path_bound)[i].l_lower.l -= refline_offset_to_lane_center; + (*path_bound)[i].l_upper.l -= refline_offset_to_lane_center; + } +} + +int PathBoundsDeciderUtil::IsPointWithinPathBound( + const ReferenceLineInfo& reference_line_info, const double x, + const double y, const PathBound& path_bound) { + common::SLPoint point_sl; + reference_line_info.reference_line().XYToSL({x, y}, &point_sl); + if (point_sl.s() > path_bound.back().s || + point_sl.s() < + path_bound.front().s - FLAGS_path_bounds_decider_resolution * 2) { + ADEBUG << "Longitudinally outside the boundary."; + return -1; + } + int idx_after = 0; + while (idx_after < static_cast(path_bound.size()) && + path_bound[idx_after].s < point_sl.s()) { + ++idx_after; + } + ADEBUG << "The idx_after = " << idx_after; + ADEBUG << "The boundary is: " + << "[" << path_bound[idx_after].l_lower.l << ", " + << path_bound[idx_after].l_upper.l << "]."; + ADEBUG << "The point is at: " << point_sl.l(); + int idx_before = idx_after - 1; + if (path_bound[idx_before].l_lower.l <= point_sl.l() && + path_bound[idx_before].l_upper.l >= point_sl.l() && + path_bound[idx_after].l_lower.l <= point_sl.l() && + path_bound[idx_after].l_upper.l >= point_sl.l()) { + return idx_after; + } + ADEBUG << "Laterally outside the boundary."; + return -1; +} + +// QP 路径优化器要求路径曲率连续。若障碍物边界太“尖锐”，可能导致不可行解。因此需根据车辆最小转弯半径放宽边界 +/// @brief +/// @param path_bound_point 当前点 +/// @param is_left +/// @param init_l +/// @param heading +/// @param delta_s +/// @param init_frenet_kappa +/// @param min_radius +/// @return +bool PathBoundsDeciderUtil::RelaxBoundaryPoint( + PathBoundPoint* const path_bound_point, bool is_left, double init_l, + double heading, double delta_s, double init_frenet_kappa, + double min_radius) { + bool is_success = false; + double protective_restrict = 0.0; + double relax_constraint = 0.0; + // 1. 曲率与转弯半径 + double radius = 1.0 / std::fabs(init_frenet_kappa); + double old_buffer = FLAGS_obstacle_lat_buffer; + double new_buffer = std::max(FLAGS_ego_front_slack_buffer, + FLAGS_nonstatic_obstacle_nudge_l_buffer); + // 2. 左右边界处理分支 + // is_left == true：处理左边界（l_upper） + if (is_left) { + // 3. 特殊情形：反向曲率 + 反向航向 + // 4. 计算可达横向偏移（protective_restrict）相对于 init_l 的最大可达 l 偏移量（带符号） + if (init_frenet_kappa > 0 && heading < 0) { + is_success = util::left_arc_bound_with_heading_with_reverse_kappa( + delta_s, min_radius, heading, init_frenet_kappa, + &protective_restrict); + } else { + // 计算圆弧可达区域 + is_success = util::left_arc_bound_with_heading(delta_s, radius, heading, + &protective_restrict); + } + // 5. 边界放松逻辑 + // 原左边界是 l_upper.l，现在可达位置是 init_l + protective_restrict。 + // 取两者中更大的值（因为左边界是上限，越大表示越靠左，约束越松）。 + relax_constraint = + std::max(path_bound_point->l_upper.l, init_l + protective_restrict); + AINFO << "init_pt_l: " << init_l + << ", left_bound: " << path_bound_point->l_upper.l + << ", diff s: " << delta_s << ", radius: " << radius + << ", protective_restrict: " << protective_restrict + << ", left_obs_constraint: " << relax_constraint; + + if (path_bound_point->is_nudge_bound[LEFT_INDEX]) { + old_buffer = std::max(FLAGS_obstacle_lat_buffer, + FLAGS_static_obstacle_nudge_l_buffer); + } + // old_buffer：原障碍物缓冲（如 0.3m） + // new_buffer：ego 自身的松弛缓冲（如 0.1m） + // + old_buffer - new_buffer 表示：把障碍物边界向外挪（放松）了 (old - new) 的距离 + // 但不能比可达区域更松，所以取 min(可达值, 放松后边界) + relax_constraint = + std::min(path_bound_point->l_upper.l + old_buffer - new_buffer, + relax_constraint); + AINFO << "left_obs_constraint: " << relax_constraint; + path_bound_point->l_upper.l = relax_constraint; + } else { + if (init_frenet_kappa < 0 && heading > 0) { + is_success = util::right_arc_bound_with_heading_with_reverse_kappa( + delta_s, min_radius, heading, init_frenet_kappa, + &protective_restrict); + } else { + is_success = util::right_arc_bound_with_heading(delta_s, radius, heading, + &protective_restrict); + } + relax_constraint = + std::min(path_bound_point->l_lower.l, init_l + protective_restrict); + AINFO << "init_pt_l: " << init_l + << ", right_bound: " << path_bound_point->l_lower.l + << ", diff s: " << delta_s << ", radius: " << radius + << ", protective_restrict: " << protective_restrict + << ", right_obs_constraint: " << relax_constraint; + + if (path_bound_point->is_nudge_bound[RIGHT_INDEX]) { + old_buffer = std::max(FLAGS_obstacle_lat_buffer, + FLAGS_static_obstacle_nudge_l_buffer); + } + // 将障碍物边界向外放松到该可达区域 + // 同时调整缓冲（从 obstacle buffer → ego slack buffer） + relax_constraint = + std::max(path_bound_point->l_lower.l - old_buffer + new_buffer, + relax_constraint); + AINFO << "right_obs_constraint: " << relax_constraint; + path_bound_point->l_lower.l = relax_constraint; + } + return is_success; +} + +/* +对主路径边界（前若干米）应用 RelaxBoundaryPoint +仅处理 OBSTACLE 类型的边界点 +*/ +bool PathBoundsDeciderUtil::RelaxEgoPathBoundary( + PathBoundary* const path_boundary, const SLState& init_sl_state) { + if (path_boundary->size() < 2) { + AINFO << "path_boundary size = 0, return."; + return false; + } + const auto& veh_param = + common::VehicleConfigHelper::GetConfig().vehicle_param(); + double min_radius = + std::max(veh_param.min_turn_radius(), + std::tan(veh_param.max_steer_angle() / veh_param.steer_ratio()) / + veh_param.wheel_base()); + + double init_frenet_kappa = + init_sl_state.second[2] / + std::pow(1 + std::pow(init_sl_state.second[1], 2), 1.5); + if (init_frenet_kappa < 0) { + init_frenet_kappa = std::min( + -1.0 / (min_radius + FLAGS_relax_ego_radius_buffer), init_frenet_kappa); + } else { + init_frenet_kappa = std::max( + 1.0 / (min_radius + FLAGS_relax_ego_radius_buffer), init_frenet_kappa); + } + + const auto& init_pt = path_boundary->at(0); + // l' = dl/ds 是 Frenet 坐标系下横向偏移对纵向距离的导数 + double init_frenet_heading = + common::math::Vec2d(1.0, init_sl_state.second[1]).Angle(); + double init_pt_l = init_sl_state.second[0]; + bool left_calculate_success = true; + bool right_calculate_success = true; + for (size_t i = 1; i < path_boundary->size(); ++i) { + auto& left_bound = path_boundary->at(i).l_upper; + auto& right_bound = path_boundary->at(i).l_lower; + double delta_s = path_boundary->at(i).s - init_pt.s; + if (delta_s > FLAGS_relax_path_s_threshold) { + left_calculate_success = false; + right_calculate_success = false; + break; + } + if (left_calculate_success && + (left_bound.type == BoundType::OBSTACLE || + path_boundary->at(i).is_nudge_bound[LEFT_INDEX])) { + left_calculate_success = RelaxBoundaryPoint( + &path_boundary->at(i), true, init_pt_l, init_frenet_heading, delta_s, + init_frenet_kappa, min_radius); + } + if (right_calculate_success && + (right_bound.type == BoundType::OBSTACLE || + path_boundary->at(i).is_nudge_bound[RIGHT_INDEX])) { + right_calculate_success = RelaxBoundaryPoint( + &path_boundary->at(i), false, init_pt_l, init_frenet_heading, delta_s, + init_frenet_kappa, min_radius); + } + if (!left_calculate_success && !right_calculate_success) { + break; + } + } + return true; +} + +// 对 extra_path_bound（角点约束）同样进行松弛 +bool PathBoundsDeciderUtil::RelaxObsCornerBoundary( + PathBoundary* const path_boundary, const SLState& init_sl_state) { + if (path_boundary->size() < 2) { + AINFO << "path_boundary size = 0, return."; + return false; + } + const auto& veh_param = + common::VehicleConfigHelper::GetConfig().vehicle_param(); + double min_radius = + std::max(veh_param.min_turn_radius(), + std::tan(veh_param.max_steer_angle() / veh_param.steer_ratio()) / + veh_param.wheel_base()); + + double init_frenet_kappa = + std::fabs(init_sl_state.second[2] / + std::pow(1 + std::pow(init_sl_state.second[1], 2), 1.5)); + if (init_frenet_kappa < 0) { + init_frenet_kappa = std::min( + -1.0 / (min_radius + FLAGS_relax_ego_radius_buffer), init_frenet_kappa); + } else { + init_frenet_kappa = std::max( + 1.0 / (min_radius + FLAGS_relax_ego_radius_buffer), init_frenet_kappa); + } + double kappa_radius = 1.0 / std::fabs(init_frenet_kappa); + + const auto& init_pt = path_boundary->at(0); + double init_frenet_heading = + common::math::Vec2d(1.0, init_sl_state.second[1]).Angle(); + double init_pt_l = init_sl_state.second[0]; + bool left_calculate_success = true; + bool right_calculate_success = true; + double new_buffer = std::max(FLAGS_ego_front_slack_buffer, + FLAGS_nonstatic_obstacle_nudge_l_buffer); + for (auto& extra_path_bound : *(path_boundary->mutable_extra_path_bound())) { + double delta_s = extra_path_bound.rear_axle_s - init_pt.s; + if (delta_s > FLAGS_relax_path_s_threshold) { + break; + } + + // calculate the left side + if (left_calculate_success) { + double left_protective_restrict = 0.0; + if (init_frenet_kappa > 0 && init_frenet_heading < 0) { + left_calculate_success = + util::left_arc_bound_with_heading_with_reverse_kappa( + delta_s, min_radius, init_frenet_heading, init_frenet_kappa, + &left_protective_restrict); + } else { + left_calculate_success = util::left_arc_bound_with_heading( + delta_s, kappa_radius, init_frenet_heading, + &left_protective_restrict); + } + + double left_obs_constraint = std::max( + extra_path_bound.upper_bound, init_pt_l + left_protective_restrict); + AINFO << "extra_path_bound, init_pt_l: " << init_pt_l + << ", left_bound: " << extra_path_bound.upper_bound + << ", diff s: " << delta_s << ", min_radius: " << min_radius + << ", init_frenet_heading: " << init_frenet_heading + << ", protective_restrict: " << left_protective_restrict + << ", left_obs_constraint: " << left_obs_constraint; + left_obs_constraint = std::min( + extra_path_bound.upper_bound + FLAGS_obstacle_lat_buffer - new_buffer, + left_obs_constraint); + AINFO << "extra_path_bound left_obs_constraint: " << left_obs_constraint; + extra_path_bound.upper_bound = left_obs_constraint; + } + + if (right_calculate_success) { + // calculate the right side + double right_protective_restrict = 0.0; + if (init_frenet_kappa < 0 && init_frenet_heading > 0) { + right_calculate_success = + util::right_arc_bound_with_heading_with_reverse_kappa( + delta_s, min_radius, init_frenet_heading, init_frenet_kappa, + &right_protective_restrict); + } else { + right_calculate_success = util::right_arc_bound_with_heading( + delta_s, kappa_radius, init_frenet_heading, + &right_protective_restrict); + } + + double right_obs_constraint = std::min( + extra_path_bound.lower_bound, init_pt_l + right_protective_restrict); + AINFO << "extra_path_bound, init_pt_l: " << init_pt_l + << ", right_bound: " << extra_path_bound.lower_bound + << ", diff s: " << delta_s << ", min_radius: " << min_radius + << ", init_frenet_heading: " << init_frenet_heading + << ", protective_restrict: " << right_protective_restrict + << ", right_obs_constraint: " << right_obs_constraint; + right_obs_constraint = std::max( + extra_path_bound.lower_bound - FLAGS_obstacle_lat_buffer + new_buffer, + right_obs_constraint); + AINFO << "extra_path_bound, right_obs_constraint: " + << right_obs_constraint; + extra_path_bound.lower_bound = right_obs_constraint; + } + + if (!left_calculate_success && !right_calculate_success) { + break; + } + } + return true; +} + +// 如果角点约束导致堵塞，更新 blocked_id 并裁剪路径 +bool PathBoundsDeciderUtil::UpdateBlockInfoWithObsCornerBoundary( + PathBoundary* const path_boundary, std::string* const blocked_id) { + if (path_boundary->extra_path_bound().blocked_id.empty()) { + AINFO << "UpdateBlockInfoWithObsCornerBoundary, block id empty"; + return true; + } + auto* extra_path_bound = path_boundary->mutable_extra_path_bound(); + size_t path_boundary_block_index = extra_path_bound->block_right_index; + + // trim path boundary width corner constraints block obstacle id + *blocked_id = extra_path_bound->blocked_id; + TrimPathBounds(path_boundary_block_index, path_boundary); + AINFO << "update block id: " << *blocked_id + << ", path_boundary size: " << path_boundary->size(); + + if (path_boundary->size() < 1) { + extra_path_bound->clear(); + AERROR << "UpdateBlockInfoWithObsCornerBoundary, new_path_index < 1"; + return false; + } + + size_t new_path_index = path_boundary->size() - 1; + while (extra_path_bound->size() > 0 && + (extra_path_bound->back().id == *blocked_id || + extra_path_bound->back().right_index > new_path_index)) { + AINFO << "remove extra_path_bound: s " + << extra_path_bound->back().rear_axle_s << ", index [" + << extra_path_bound->back().left_index << ", " + << extra_path_bound->back().right_index << "]"; + extra_path_bound->pop_back(); + } + return false; +} + +bool PathBoundsDeciderUtil::AddExtraPathBound( + const std::vector& sl_polygons, + PathBoundary* const path_boundary, const SLState& init_sl_state, + std::string* const blocked_id) { + RelaxEgoPathBoundary(path_boundary, init_sl_state); + if (FLAGS_enable_corner_constraint) { + AddCornerBounds(sl_polygons, path_boundary); + RelaxObsCornerBoundary(path_boundary, init_sl_state); + UpdateBlockInfoWithObsCornerBoundary(path_boundary, blocked_id); + } + if (FLAGS_enable_adc_vertex_constraint) { + AddAdcVertexBounds(path_boundary); + } + return true; +} + +} // namespace planning +} // namespace apollo \ No newline at end of file diff --git a/modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.cc b/modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.cc index e69cfe88dde..248458a4a09 100644 --- a/modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.cc +++ b/modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.cc @@ -68,9 +68,13 @@ FrenetFramePath PathOptimizerUtil::ToPiecewiseJerkPath( double PathOptimizerUtil::EstimateJerkBoundary(const double vehicle_speed) { const auto& veh_param = common::VehicleConfigHelper::GetConfig().vehicle_param(); - const double axis_distance = veh_param.wheel_base(); + const double axis_distance = veh_param.wheel_base(); // 轴距 + // max_steer_angle_rate() 是转向盘角速度的最大值（单位：弧度/秒或度/秒） + // 除以转向比 (steer_ratio) 后得到前轮实际转向角速度最大值 const double max_yaw_rate = veh_param.max_steer_angle_rate() / veh_param.steer_ratio(); + // 最大偏航角速度除以轴距，得到最大曲率变化率（单位 1/m/s） + // 再除以车辆当前速度，转成曲率的“变化率的变化率”，即路径的横向 jerk 边界 return max_yaw_rate / axis_distance / vehicle_speed; } @@ -187,6 +191,7 @@ bool PathOptimizerUtil::OptimizePath( void PathOptimizerUtil::UpdatePathRefWithBound( const PathBoundary& path_boundary, double weight, std::vector* ref_l, std::vector* weight_ref_l) { + // 大小调整为和路径边界点数量相同，准备存储每个路径点的参考横向偏移值和权重 ref_l->resize(path_boundary.size()); weight_ref_l->resize(path_boundary.size()); for (size_t i = 0; i < ref_l->size(); i++) { @@ -194,21 +199,33 @@ void PathOptimizerUtil::UpdatePathRefWithBound( path_boundary[i].l_upper.type == BoundType::OBSTACLE) { ref_l->at(i) = (path_boundary[i].l_lower.l + path_boundary[i].l_upper.l) / 2.0; + // 将权重设置为传入的参数 weight，表示路径优化时尽量靠近这个参考横向位置，权重越大越重要 weight_ref_l->at(i) = weight; } else { + // 如果当前路径点左右边界都不是障碍物（没有特别限制），则权重设为0，不强制路径靠近某个横向参考 weight_ref_l->at(i) = 0; } } } - +/// @brief 考虑了横向加速度和路径的曲率，基于车辆的物理参数和路径的几何特征来估算加速度边界 +/// @param path_boundary +/// @param reference_line +/// @param ddl_bounds void PathOptimizerUtil::CalculateAccBound( const PathBoundary& path_boundary, const ReferenceLine& reference_line, std::vector>* ddl_bounds) { +// 获取车辆的物理参数（例如最大转向角、转向比、轴距等） const auto& veh_param = common::VehicleConfigHelper::GetConfig().vehicle_param(); +// 计算车辆在最大转向角下的横向加速度边界 +// veh_param.max_steer_angle() 是最大前轮转向角（单位一般是弧度） +// 除以 veh_param.steer_ratio() 把转向盘角度换算成前轮角度 +// tan(前轮角度) 代表车辆侧向力的切线值 +// 除以轴距（wheel_base）得到车辆绕垂直轴转动的曲率上限 const double lat_acc_bound = std::tan(veh_param.max_steer_angle() / veh_param.steer_ratio()) / veh_param.wheel_base(); +// 获取路径边界点数量 size_t path_boundary_size = path_boundary.boundary().size(); for (size_t i = 0; i < path_boundary_size; ++i) { double s = static_cast(i) * path_boundary.delta_s() + diff --git a/modules/planning/planning_interface_base/task_base/task.cc b/modules/planning/planning_interface_base/task_base/task.cc index a014e232942..e8811cb0537 100644 --- a/modules/planning/planning_interface_base/task_base/task.cc +++ b/modules/planning/planning_interface_base/task_base/task.cc @@ -36,7 +36,11 @@ Task::Task() config_path_(""), default_config_path_(""), name_("") {} - +/// @brief +/// @param config_dir 一个字符串，表示配置文件所在的目录路径 +/// @param name 一个字符串，表示任务的名称 +/// @param injector 一个智能指针，指向 DependencyInjector 对象，用于依赖注入 +/// @return bool Task::Init(const std::string& config_dir, const std::string& name, const std::shared_ptr& injector) { injector_ = injector; diff --git a/modules/planning/planning_open_space/coarse_trajectory_generator/hybrid_a_star.cc b/modules/planning/planning_open_space/coarse_trajectory_generator/hybrid_a_star.cc index 53ec2e271a0..87088e78e8d 100644 --- a/modules/planning/planning_open_space/coarse_trajectory_generator/hybrid_a_star.cc +++ b/modules/planning/planning_open_space/coarse_trajectory_generator/hybrid_a_star.cc @@ -37,28 +37,40 @@ using apollo::cyber::Clock; HybridAStar::HybridAStar(const PlannerOpenSpaceConfig& open_space_conf) { planner_open_space_config_.CopyFrom(open_space_conf); + // ReedShepp 是一种计算两点之间满足车辆运动学约束的最短路径的算法（考虑前进/后退、最小转弯半径） reed_shepp_generator_ = std::make_unique(vehicle_param_, planner_open_space_config_); + // GridSearch 是一个基于栅格地图的 A* 算法，用于预计算从终点反向搜索的启发式代价 + // 这里不是最终路径，而是为 Hybrid A* 提供“到终点大概多远”的估计值 grid_a_star_heuristic_generator_ = std::make_unique(planner_open_space_config_); + // 每个节点扩展的子节点数量：前进和后退之和 next_node_num_ = planner_open_space_config_.warm_start_config().next_node_num(); + // 最大有效前轮转角（带缩放）R = L / tan(δ) max_steer_angle_ = vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio() * planner_open_space_config_.warm_start_config() .traj_kappa_contraint_ratio(); + // 每步(从一个状态到另一个状态)前进/后退的距离 step_size_ = planner_open_space_config_.warm_start_config().step_size(); + // 栅格分辨率 xy_grid_resolution_ = planner_open_space_config_.warm_start_config().xy_grid_resolution(); + // 确保每一步至少跨越一个栅格对角线，防止在同一栅格内“打转”。 + // phi_grid_resolution 是角度分辨率，结合车辆几何（wheel_base）和转向角，换算成实际行驶弧长 arc_length_ = planner_open_space_config_.warm_start_config().phi_grid_resolution() * vehicle_param_.wheel_base() / std::tan(max_steer_angle_ * 2 / (next_node_num_ / 2 - 1)); + // 确保每一步至少跨越一个栅格对角线，避免在同一个栅格内反复震荡 if (arc_length_ < std::sqrt(2) * xy_grid_resolution_) { arc_length_ = std::sqrt(2) * xy_grid_resolution_; } AINFO << "arc_length" << arc_length_; + // 用于后续速度剖面生成的时间间隔 delta_t_ = planner_open_space_config_.delta_t(); + // Hybrid A* 的总代价 = g(n)（实际代价） + h(n)（启发式） traj_forward_penalty_ = planner_open_space_config_.warm_start_config().traj_forward_penalty(); traj_back_penalty_ = @@ -69,6 +81,8 @@ HybridAStar::HybridAStar(const PlannerOpenSpaceConfig& open_space_conf) { planner_open_space_config_.warm_start_config().traj_steer_penalty(); traj_steer_change_penalty_ = planner_open_space_config_.warm_start_config() .traj_steer_change_penalty(); + + acc_weight_ = planner_open_space_config_.iterative_anchoring_smoother_config() .s_curve_config() .acc_weight(); @@ -88,6 +102,8 @@ HybridAStar::HybridAStar(const PlannerOpenSpaceConfig& open_space_conf) { planner_open_space_config_.iterative_anchoring_smoother_config() .s_curve_config() .ref_v_weight(); + + max_forward_v_ = planner_open_space_config_.iterative_anchoring_smoother_config() .max_forward_v(); @@ -105,6 +121,7 @@ HybridAStar::HybridAStar(const PlannerOpenSpaceConfig& open_space_conf) { .max_acc_jerk(); } +// 尝试从 current_node 到 end_node_ 用 Reed-Shepp 路径 直接连接 bool HybridAStar::AnalyticExpansion( std::shared_ptr current_node, std::shared_ptr* candidate_final_node) { @@ -114,6 +131,8 @@ bool HybridAStar::AnalyticExpansion( reeds_shepp_to_check)) { return false; } + // 将 Reed-Shepp 路径离散化为一系列 (x, y, phi) + // 创建临时 Node3d，调用 ValidityCheck 检查是否与障碍物碰撞或越界 if (!RSPCheck(reeds_shepp_to_check)) { return false; } @@ -146,12 +165,16 @@ bool HybridAStar::ValidityCheck(std::shared_ptr node) { // The first {x, y, phi} is collision free unless they are start and end // configuration of search problem size_t check_start_index = 0; + // 当 step_size == 1：表示这是一个“单点”节点（通常是起始节点或极小步长），需要检查第 0 个点 if (node_step_size == 1) { check_start_index = 0; } else { + // 当 step_size > 1：跳过第 0 个点（i=0），从 i=1 开始检查 check_start_index = 1; } - + // 在 Hybrid A* 搜索中，每个新节点的轨迹是 从前一个节点的最后一个状态开始模拟的 + // 当前节点的 traversed_x[0] 实际上等于 父节点的最后一个点 + // 为了避免重复检测（父节点已经检测过该点），跳过索引 0，只检测新生成的部分 for (size_t i = check_start_index; i < node_step_size; ++i) { if (traversed_x[i] > XYbounds_[1] || traversed_x[i] < XYbounds_[0] || traversed_y[i] > XYbounds_[3] || traversed_y[i] < XYbounds_[2]) { @@ -175,47 +198,61 @@ bool HybridAStar::ValidityCheck(std::shared_ptr node) { return true; } +// 加载 Reed-Shepp 路径到闭集 std::shared_ptr HybridAStar::LoadRSPinCS( const std::shared_ptr reeds_shepp_to_end, std::shared_ptr current_node) { + // 创建终点节点，设置其前驱为 current_node std::shared_ptr end_node = std::shared_ptr(new Node3d( reeds_shepp_to_end->x, reeds_shepp_to_end->y, reeds_shepp_to_end->phi, XYbounds_, planner_open_space_config_)); end_node->SetPre(current_node); + // 总轨迹代价 = 当前代价 + Reed-Shepp 路径长度（reeds_shepp_to_end->cost） end_node->SetTrajCost(current_node->GetTrajCost() + reeds_shepp_to_end->cost); return end_node; } +/// @brief 根据 next_node_index 选择一个方向盘转角（steering） 和 行驶方向（前进/后退） +// 使用自行车运动学模型（bicycle model）向前（或向后）积分一段轨迹 +// 生成包含多个中间点的轨迹 (x, y, phi) +// 构造一个新的 Node3d 节点并返回 +/// @param current_node +/// @param next_node_index +/// @return std::shared_ptr HybridAStar::Next_node_generator( std::shared_ptr current_node, size_t next_node_index) { double steering = 0.0; double traveled_distance = 0.0; if (next_node_index < static_cast(next_node_num_) / 2) { + // 前进：steering 从 -max 到 +max steering = -max_steer_angle_ + (2 * max_steer_angle_ / (static_cast(next_node_num_) / 2 - 1)) * static_cast(next_node_index); - traveled_distance = step_size_; + traveled_distance = step_size_; // 正值 → 前进 } else { + // 后退：同样 steering 范围，但距离为负 size_t index = next_node_index - next_node_num_ / 2; steering = -max_steer_angle_ + (2 * max_steer_angle_ / (static_cast(next_node_num_) / 2 - 1)) * static_cast(index); - traveled_distance = -step_size_; + traveled_distance = -step_size_; // 负值 → 后退 } // take above motion primitive to generate a curve driving the car to a // different grid + // 轨迹积分 std::vector intermediate_x; std::vector intermediate_y; std::vector intermediate_phi; double last_x = current_node->GetX(); double last_y = current_node->GetY(); double last_phi = current_node->GetPhi(); - intermediate_x.push_back(last_x); + intermediate_x.push_back(last_x); // 起点 = 当前节点终点 intermediate_y.push_back(last_y); intermediate_phi.push_back(last_phi); for (size_t i = 0; i < arc_length_ / step_size_; ++i) { + // 使用中点法（trapezoidal integration）更新状态 const double next_phi = last_phi + traveled_distance / vehicle_param_.wheel_base() * @@ -251,17 +288,24 @@ std::shared_ptr HybridAStar::Next_node_generator( void HybridAStar::CalculateNodeCost( std::shared_ptr current_node, std::shared_ptr next_node) { + // 更新轨迹代价 g_cost + // current_node->GetTrajCost()：从起点到当前节点的累计代价（即 g(current)） + // TrajCost(current_node, next_node)：从 current → next 这一步的增量代价 next_node->SetTrajCost( current_node->GetTrajCost() + TrajCost(current_node, next_node)); // evaluate heuristic cost + // 计算启发式代价（h-cost） double optimal_path_cost = 0.0; + // 假设车辆能横移” 但 “避开障碍物” 的最短距离估计 optimal_path_cost += HoloObstacleHeuristic(next_node); next_node->SetHeuCost(optimal_path_cost); } +// distance_weight > gear_switch >> steer_change > steer_abs double HybridAStar::TrajCost(std::shared_ptr current_node, std::shared_ptr next_node) { // evaluate cost on the trajectory and add current cost + // 路径长度代价（主项） double piecewise_cost = 0.0; if (next_node->GetDirec()) { piecewise_cost += @@ -275,10 +319,13 @@ double HybridAStar::TrajCost(std::shared_ptr current_node, traj_back_penalty_; } AINFO << "traj cost: " << piecewise_cost; + // 换挡惩罚（Gear Switch Penalty） if (current_node->GetDirec() != next_node->GetDirec()) { piecewise_cost += traj_gear_switch_penalty_; } + // 方向盘转角绝对值惩罚 piecewise_cost += traj_steer_penalty_ * std::abs(next_node->GetSteer()); + // 转角变化率 piecewise_cost += traj_steer_change_penalty_ * std::abs(next_node->GetSteer() - current_node->GetSteer()); return piecewise_cost; @@ -719,6 +766,8 @@ bool HybridAStar::Plan( PrintCurves print_curves; std::vector> obstacles_linesegments_vec; + // 将每个障碍物多边形转为一组 LineSegment2d + // 后续碰撞检测用的是 线段 vs 车辆包围盒（Box2d） 的相交判断，效率高 for (const auto& obstacle_vertices : obstacles_vertices_vec) { size_t vertices_num = obstacle_vertices.size(); std::vector obstacle_linesegments; @@ -779,12 +828,16 @@ bool HybridAStar::Plan( return false; } double map_time = Clock::NowInSeconds(); + // 基于动态规划算法，求解每个二维节点的代价值，作为混合A*的启发函数h值 + // 使用 二维动态规划（DP） 在 xy 平面上计算从任意点到终点的最短距离（忽略朝向和车辆模型） grid_a_star_heuristic_generator_->GenerateDpMap( ex, ey, XYbounds_, obstacles_linesegments_vec_); ADEBUG << "map time " << Clock::NowInSeconds() - map_time; + + // 初始化搜索结构 // load open set, pq - open_set_.insert(start_node_->GetIndex()); - open_pq_.emplace(start_node_, start_node_->GetCost()); + open_set_.insert(start_node_->GetIndex()); // 哈希集合，快速查重 + open_pq_.emplace(start_node_, start_node_->GetCost()); // 优先队列（最小堆），按 f = g + h 排序 // Hybrid A* begins size_t explored_node_num = 0; size_t available_result_num = 0; @@ -839,20 +892,26 @@ bool HybridAStar::Plan( std::shared_ptr next_node = Next_node_generator(current_node, i); node_generator_time += Clock::NowInSeconds() - gen_node_time; + + // 对每个 next_node： // boundary check failure handle + // 越界？ → Next_node_generator 返回 nullptr（已处理） if (next_node == nullptr) { continue; } // check if the node is already in the close set + // 已在 closed set？ → 跳过（避免重复） if (close_set_.count(next_node->GetIndex()) > 0) { continue; } // collision check const double validity_check_start_time = Clock::NowInSeconds(); + // 碰撞？ → ValidityCheck(next_node) 返回 false → 跳过 if (!ValidityCheck(next_node)) { continue; } validity_check_time += Clock::NowInSeconds() - validity_check_start_time; + // 未在 open set？ → 计算代价，加入 open set if (open_set_.count(next_node->GetIndex()) == 0) { const double start_time = Clock::NowInSeconds(); CalculateNodeCost(current_node, next_node); diff --git a/modules/planning/planning_open_space/utils/open_space_roi_util.cc b/modules/planning/planning_open_space/utils/open_space_roi_util.cc index 3f830215ab2..b85aa72055b 100644 --- a/modules/planning/planning_open_space/utils/open_space_roi_util.cc +++ b/modules/planning/planning_open_space/utils/open_space_roi_util.cc @@ -295,6 +295,7 @@ void OpenSpaceRoiUtil::TransformByOriginPoint( auto xy_boundary = open_space_info->mutable_ROI_xy_boundary(); GetRoiXYBoundary(*obstacles_vertices_vec, xy_boundary); } +// 判断多边形顶点是否为顺时针 bool OpenSpaceRoiUtil::IsPolygonClockwise(const std::vector &polygon) { double s = 0; for (size_t i = 0; i < polygon.size(); i++) { @@ -306,9 +307,10 @@ bool OpenSpaceRoiUtil::IsPolygonClockwise(const std::vector &polygon) { polygon.at(i).y() * polygon.at(0).x(); } } - if (s < 0) { + // 基于有向面积判断顺逆时针 + if (s < 0) { // 顺时针 return true; - } else { + } else { // 逆时针 return false; } } @@ -318,6 +320,7 @@ bool OpenSpaceRoiUtil::AdjustPointsOrderToClockwise( if (!IsPolygonClockwise(*polygon)) { // counter clockwise reverse it ADEBUG << "point is anticlockwise,reverse"; + // 强制转换为顺时针 std::reverse(polygon->begin(), polygon->end()); return true; } else { @@ -328,6 +331,7 @@ bool OpenSpaceRoiUtil::AdjustPointsOrderToClockwise( bool OpenSpaceRoiUtil::UpdateParkingPointsOrder( const apollo::hdmap::Path &nearby_path, std::vector *points) { AdjustPointsOrderToClockwise(points); + // 找到“最靠近参考路径起点”的顶点作为新起点 double min_dist = std::numeric_limits::max(); size_t min_index = 0; for (size_t i = 0; i < points->size(); i++) { diff --git a/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc b/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc index be9cd89ffea..4d589162874 100644 --- a/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc +++ b/modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc @@ -118,6 +118,9 @@ std::vector LaneFollowMap::FutureRouteWaypoints() const { waypoints.begin() + next_routing_waypoint_index_, waypoints.end()); } +/// @brief 从最近的命令（last_command_）中获取结束车道航路点并通过引用返回 +// 检查是否有有效的车道跟随命令和有效的路由请求，然后提取路由请求中的最后一个航路点作为车道的结束点 +/// @param end_point void LaneFollowMap::GetEndLaneWayPoint( std::shared_ptr &end_point) const { if (!last_command_.has_lane_follow_command() || @@ -125,13 +128,17 @@ void LaneFollowMap::GetEndLaneWayPoint( end_point = nullptr; return; } + // 获取路由请求 const auto &routing_request = last_command_.lane_follow_command().routing_request(); if (routing_request.waypoint().size() < 1) { end_point = nullptr; return; } + // 获取并设置结束航路点 + // 如果 waypoint 中有至少一个点，使用 std::make_shared() 创建一个新的 LaneWaypoint 对象，并将其指针赋给 end_point end_point = std::make_shared(); + // 将该终点的内容复制到新的 LaneWaypoint 对象 end_point->CopyFrom(*(routing_request.waypoint().rbegin())); } @@ -143,25 +150,31 @@ hdmap::LaneInfoConstPtr LaneFollowMap::GetLaneById(const hdmap::Id &id) const { } bool LaneFollowMap::IsValid(const planning::PlanningCommand &command) const { + // 检查是否可以处理该命令 if (!CanProcess(command)) { return false; } + // 获取车道跟随命令的路由信息 const auto &routing = command.lane_follow_command(); const int num_road = routing.road_size(); + // 如果没有路信息，返回无效 if (num_road == 0) { return false; } + // 检查路由请求是否存在且包含至少两个航路点 if (!routing.has_routing_request() || routing.routing_request().waypoint_size() < 2) { AERROR << "Routing does not have request."; return false; } + // 遍历航路点，检查每个航路点是否包含必要的字段：id 和 s for (const auto &waypoint : routing.routing_request().waypoint()) { if (!waypoint.has_id() || !waypoint.has_s()) { AERROR << "Routing waypoint has no lane_id or s."; return false; } } + // 如果所有验证通过，返回有效 return true; } @@ -409,16 +422,26 @@ std::vector LaneFollowMap::GetNeighborPassages( } return result; } +/// @brief 获取路线段 +/// @param vehicle_state 一个常量引用，表示车辆的状态（例如，车辆的速度、位置等） +/// @param route_segments 一个指向 std::list 类型的指针，函数通过该参数返回路线段数据 +/// @return bool LaneFollowMap::GetRouteSegments( const VehicleState &vehicle_state, std::list *const route_segments) { double look_forward_distance = LookForwardDistance(vehicle_state.linear_velocity()); - double look_backward_distance = FLAGS_look_backward_distance; + double look_backward_distance = FLAGS_look_backward_distance; // 50 return GetRouteSegments(vehicle_state, look_backward_distance, look_forward_distance, route_segments); } +/// @brief 根据当前车辆的状态和指定的前后长度（backward_length 和 forward_length）获取路线的多个路段（route_segments） +/// @param vehicle_state +/// @param backward_length +/// @param forward_length +/// @param route_segments +/// @return bool LaneFollowMap::GetRouteSegments( const VehicleState &vehicle_state, const double backward_length, const double forward_length, @@ -429,28 +452,40 @@ bool LaneFollowMap::GetRouteSegments( } // Vehicle has to be this close to lane center before considering change // lane + // 判断当前的车辆所在车道是否有效、路线索引是否有效 if (!adc_waypoint_.lane || adc_route_index_ < 0 || adc_route_index_ >= static_cast(route_indices_.size())) { AERROR << "Invalid vehicle state in pnc_map, update vehicle state first."; return false; } + // 获取当前路线的索引，拆解出道路索引（road_index）和通道索引（passage_index）， + //并通过 last_command_ 获取当前道路的信息 const auto &route_index = route_indices_[adc_route_index_].index; const int road_index = route_index[0]; const int passage_index = route_index[1]; const auto &road = last_command_.lane_follow_command().road(road_index); + // Raw filter to find all neighboring passages + // 获取邻近的通道 auto drive_passages = GetNeighborPassages(road, passage_index); + // 遍历所有邻近的通道，获取每个通道的详细信息，并准备一个空的路段对象 segments for (const int index : drive_passages) { const auto &passage = road.passage(index); hdmap::RouteSegments segments; + +// 如果无法将通道转换为路段（RouteSegments），则输出调试信息并跳过当前通道 if (!PassageToSegments(passage, &segments)) { ADEBUG << "Failed to convert passage to lane segments."; continue; } + +// 计算当前车辆的位置，如果当前通道是车辆所在通道，则获取平滑路径点；否则，将车辆状态转换为 PointENU 坐标点 const PointENU nearest_point = index == passage_index ? adc_waypoint_.lane->GetSmoothPoint(adc_waypoint_.s) : PointFactory::ToPointENU(adc_state_); + +// 获取当前车辆在路段上的投影点（sl）和路段的点位（segment_waypoint）。如果失败，则输出调试信息并跳过当前通道 common::SLPoint sl; hdmap::LaneWaypoint segment_waypoint; if (!segments.GetProjection(nearest_point, &sl, &segment_waypoint)) { @@ -458,6 +493,8 @@ bool LaneFollowMap::GetRouteSegments( << nearest_point.ShortDebugString(); continue; } + +// 如果当前通道不是车辆所在通道，则检查是否可以从当前位置进入该通道。如果不行，则输出调试信息并跳过当前通道 if (index != passage_index) { if (!segments.CanDriveFrom(adc_waypoint_)) { ADEBUG << "You cannot drive from current waypoint to passage: " @@ -465,8 +502,12 @@ bool LaneFollowMap::GetRouteSegments( continue; } } + +// 向返回的路线段列表中添加一个新路段对象，并获取该路段的最后一个路点 route_segments->emplace_back(); const auto last_waypoint = segments.LastWaypoint(); + +// 通过 ExtendSegments 方法扩展路段，前后分别延伸 backward_length 和 forward_length 的长度。如果扩展失败，则打印错误信息并返回 false if (!ExtendSegments(segments, sl.s() - backward_length, sl.s() + forward_length, &route_segments->back())) { AERROR << "Failed to extend segments with s=" << sl.s() @@ -474,14 +515,25 @@ bool LaneFollowMap::GetRouteSegments( << ", forward: " << forward_length; return false; } + +// 如果最后一个路点在当前路段上，则设置当前路段的结束点为该路点 if (route_segments->back().IsWaypointOnSegment(last_waypoint)) { route_segments->back().SetRouteEndWaypoint(last_waypoint); } + +// 设置当前路段是否可以退出（can_exit），以及下一个动作（如是否变道） route_segments->back().SetCanExit(passage.can_exit()); route_segments->back().SetNextAction(passage.change_lane_type()); + +// 为当前路段生成一个唯一的标识符 route_segment_id，并设置给当前路段 const std::string route_segment_id = absl::StrCat(road_index, "_", index); route_segments->back().SetId(route_segment_id); + +// 设置当前路段是否需要为目的地停车（stop_for_destination_） route_segments->back().SetStopForDestination(stop_for_destination_); + +// 如果当前通道是车辆所在通道，设置当前路段的相关信息为“在该路段上”，并且设置之前的动作为“向前”； +// 否则根据投影点的横向偏移（sl.l()）设置前进方向（右或左） if (index == passage_index) { route_segments->back().SetIsOnSegment(true); route_segments->back().SetPreviousAction(routing::FORWARD); @@ -491,6 +543,7 @@ bool LaneFollowMap::GetRouteSegments( route_segments->back().SetPreviousAction(routing::LEFT); } } + // 完成所有邻近通道的处理后，检查 route_segments 是否非空 return !route_segments->empty(); } diff --git a/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc b/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc index 59bab1cf954..f1a3594ecb8 100644 --- a/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc +++ b/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc @@ -25,7 +25,10 @@ namespace apollo { namespace planning { - +/// @brief +/// @param other_scenario 当前正在运行的场景（可能为 nullptr，表示初始状态） +/// @param frame 当前规划周期的全局上下文（含感知、地图、定位、交通规则等） +/// @return bool LaneFollowScenario::IsTransferable(const Scenario* other_scenario, const Frame& frame) { if (!frame.local_view().planning_command->has_lane_follow_command()) { diff --git a/modules/planning/scenarios/lane_follow/lane_follow_stage.cc b/modules/planning/scenarios/lane_follow/lane_follow_stage.cc index 6d7223154d0..c193749fdf3 100644 --- a/modules/planning/scenarios/lane_follow/lane_follow_stage.cc +++ b/modules/planning/scenarios/lane_follow/lane_follow_stage.cc @@ -19,22 +19,34 @@ **/ #include "modules/planning/scenarios/lane_follow/lane_follow_stage.h" - +// 引入 C++ 标准库中的工具函数（如 std::move, std::swap #include - +// 引入 Apollo 的日志宏，如 ADEBUG, AINFO, AERROR，用于输出调试或错误信息 #include "cyber/common/log.h" +// 用于获取系统时间，计算每个规划任务（Task）的执行耗时（性能分析） #include "cyber/time/clock.h" +// 提供数学工具函数（如角度归一化、插值等），可能被 Task 内部调用 #include "modules/common/math/math_utils.h" +// 用于构造 TrajectoryPoint、SLPoint 等几何点对象 #include "modules/common/util/point_factory.h" +// 字符串处理工具（如格式化），可能用于日志输出 #include "modules/common/util/string_util.h" +// 获取车辆当前状态（位置、速度、加速度、航向等） #include "modules/common/vehicle_state/vehicle_state_provider.h" +// 高精地图接口，用于查询车道、路口、交通标志等信息 #include "modules/map/hdmap/hdmap.h" #include "modules/map/hdmap/hdmap_common.h" +// 自车（Ego Vehicle）相关信息封装 #include "modules/planning/planning_base/common/ego_info.h" +// Frame 是规划模块的核心数据结构，包含感知、定位、地图、参考线等所有上下文信息 #include "modules/planning/planning_base/common/frame.h" +// 用于生成速度剖面（fallback 轨迹等） #include "modules/planning/planning_base/common/speed_profile_generator.h" +// 引入规划模块的运行时配置参数（如 FLAGS_enable_record_debug, FLAGS_enable_trajectory_check） #include "modules/planning/planning_base/gflags/planning_gflags.h" +// 轨迹合法性检查器（验证曲率、加速度、速度是否超限） #include "modules/planning/planning_base/math/constraint_checker/constraint_checker.h" +// Task 基类定义，LaneFollowStage 会依次执行一系列 Task（如 PathDecider、SpeedDecider） #include "modules/planning/planning_interface_base/task_base/task.h" namespace apollo { @@ -51,26 +63,35 @@ namespace { constexpr double kStraightForwardLineCost = 10.0; } // namespace +/// @brief 将障碍物的决策信息记录到调试日志中，便于可视化分析 +/// @param reference_line_info void LaneFollowStage::RecordObstacleDebugInfo( ReferenceLineInfo* reference_line_info) { + // 如果未启用调试记录（通过 GFlag 控制），则直接返回 if (!FLAGS_enable_record_debug) { ADEBUG << "Skip record debug info"; return; } + // 获取可修改的调试信息对象指针 auto ptr_debug = reference_line_info->mutable_debug(); - + // 获取路径决策器对所有障碍物做出的决策（如避让、跟随、忽略） const auto path_decision = reference_line_info->path_decision(); + // 遍历所有障碍物 for (const auto obstacle : path_decision->obstacles().Items()) { + // 将障碍物 ID 和其在 SL 坐标系下的边界（s: 纵向, l: 横向）拷贝到调试结构中 auto obstacle_debug = ptr_debug->mutable_planning_data()->add_obstacle(); obstacle_debug->set_id(obstacle->Id()); obstacle_debug->mutable_sl_boundary()->CopyFrom( obstacle->PerceptionSLBoundary()); + + // 每个障碍物可能被多个 Decider 处理（如 PathDecider、SpeedDecider），这里检查标签和决策数量是否一致，不一致说明逻辑错误 const auto& decider_tags = obstacle->decider_tags(); const auto& decisions = obstacle->decisions(); if (decider_tags.size() != decisions.size()) { AERROR << "decider_tags size: " << decider_tags.size() << " different from decisions size:" << decisions.size(); } + // 将每个 Decider 的名称和对应决策（如“横向忽略，纵向停车”）记录到调试信息中 for (size_t i = 0; i < decider_tags.size(); ++i) { auto decision_tag = obstacle_debug->add_decision_tag(); decision_tag->set_decider_tag(decider_tags[i]); @@ -79,47 +100,61 @@ void LaneFollowStage::RecordObstacleDebugInfo( } } +// 处理所有候选参考线，选择一条可行驶的路径 StageResult LaneFollowStage::Process( const TrajectoryPoint& planning_start_point, Frame* frame) { + // 如果没有参考线（如地图缺失），直接返回 FINISHED（无事可做） if (frame->reference_line_info().empty()) { return StageResult(StageStatusType::FINISHED); } - + // 标志位，记录是否已找到可行驶的参考线 bool has_drivable_reference_line = false; ADEBUG << "Number of reference lines:\t" << frame->mutable_reference_line_info()->size(); - + + // 遍历每条参考线（通常是 [当前车道, 左变道, 右变道]） unsigned int count = 0; StageResult result; + // 遍历所有的参考线，直到找到可用来规划的参考线后退出 for (auto& reference_line_info : *frame->mutable_reference_line_info()) { // TODO(SHU): need refactor + // 这是一个冗余的安全检查（因为 range-for 不会越界），可能是历史遗留代码，可忽略 if (count++ == frame->mutable_reference_line_info()->size()) { break; } ADEBUG << "No: [" << count << "] Reference Line."; ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath(); - + // 找到可用来行驶的参考线，退出循环 + // 一旦找到可行驶路径，立即退出循环，后续参考线标记为不可用 if (has_drivable_reference_line) { reference_line_info.SetDrivable(false); break; } - + // 执行LaneFollow的规划 + // 在当前参考线上执行完整的规划流程（路径+速度） result = PlanOnReferenceLine(planning_start_point, frame, &reference_line_info); - + // 判断规划结果是否OK + // 如果规划成功（无 Task 错误） if (!result.HasError()) { + // 不是变道的参考线，则为可行驶参考线 + // 如果是直行路径，直接接受，继续下一轮（实际会因 has_drivable...=true 而退出 if (!reference_line_info.IsChangeLanePath()) { ADEBUG << "reference line is NOT lane change ref."; has_drivable_reference_line = true; continue; } + + // 如果是变道路径，只有当总成本 低于 10.0 才接受 + // 如果是变道参考线，则对参考线的cost进行判断 if (reference_line_info.Cost() < kStraightForwardLineCost) { // If the path and speed optimization succeed on target lane while // under smart lane-change or IsClearToChangeLane under older version has_drivable_reference_line = true; reference_line_info.SetDrivable(true); } else { + // 如果规划失败（Task 报错），标记为不可用 reference_line_info.SetDrivable(false); ADEBUG << "\tlane change failed"; } @@ -127,32 +162,42 @@ StageResult LaneFollowStage::Process( reference_line_info.SetDrivable(false); } } - + // 如果找到可行驶路径 → 返回 RUNNING（正常） return has_drivable_reference_line ? result.SetStageStatus(StageStatusType::RUNNING) : result.SetStageStatus(StageStatusType::ERROR); } - +/// @brief +/// @param planning_start_point +/// @param frame +/// @param reference_line_info +/// @return StageResult LaneFollowStage::PlanOnReferenceLine( const TrajectoryPoint& planning_start_point, Frame* frame, ReferenceLineInfo* reference_line_info) { + // 判断是否有lanechange意图，如果否增加当前参考线的cost？有点疑问，增加了变道的可能 + // 为直行路径增加 10.0 成本（但不影响其优先级，仅用于变道比较） if (!reference_line_info->IsChangeLanePath()) { reference_line_info->AddCost(kStraightForwardLineCost); } ADEBUG << "planning start point:" << planning_start_point.DebugString(); ADEBUG << "Current reference_line_info is IsChangeLanePath: " << reference_line_info->IsChangeLanePath(); - + // 顺序执行stage中的任务 + // 依次执行 Task 列表（如 PATH_BOUNDS_DECIDER → PATH_DECIDER → SPEED_DECIDER） StageResult ret; for (auto task : task_list_) { + // 执行每个 Task，若失败则中断 const double start_timestamp = Clock::NowInSeconds(); + // 性能计时 const auto start_planning_perf_timestamp = std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()) .count(); - + // 执行每个task的具体逻辑 + //依次调用task_list中的task ret.SetTaskStatus(task->Execute(frame, reference_line_info)); - + // 记录耗时 const double end_timestamp = Clock::NowInSeconds(); const double time_diff_ms = (end_timestamp - start_timestamp) * 1000; ADEBUG << "after task[" << task->Name() @@ -168,7 +213,7 @@ StageResult LaneFollowStage::PlanOnReferenceLine( (end_planning_perf_timestamp - start_planning_perf_timestamp) * 1000; AINFO << "Planning Perf: task name [" << task->Name() << "], " << plnning_perf_ms << " ms."; - + // 如果task执行失败，退出task执行序列，并且记录失败信息 if (ret.IsTaskError()) { AERROR << "Failed to run tasks[" << task->Name() << "], Error message: " << ret.GetTaskStatus().error_message(); @@ -182,15 +227,18 @@ StageResult LaneFollowStage::PlanOnReferenceLine( // ADEBUG << "Current reference_line_info is IsChangeLanePath: " // << reference_line_info->IsChangeLanePath(); } - + // 记录障碍物调试信息 RecordObstacleDebugInfo(reference_line_info); // check path and speed results for path or speed fallback reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL); + // 如果task执行失败，则使用备用的规划轨迹 + // Task 失败时，执行回退策略（如生成安全停车轨迹） if (ret.IsTaskError()) { fallback_task_->Execute(frame, reference_line_info); } - + // 对规划的轨迹进行合成，如果合成失败，返回失败状态 + // 将规划出的路径（s-l）和速度（s-t）合并为时空轨迹（x-y-t）。失败则报错 DiscretizedTrajectory trajectory; if (!reference_line_info->CombinePathAndSpeedProfile( planning_start_point.relative_time(), @@ -201,6 +249,7 @@ StageResult LaneFollowStage::PlanOnReferenceLine( } // determine if there is a destination on reference line. + // 查找是否在本参考线上有终点停车点（用于区分普通障碍物停车） double dest_stop_s = -1.0; for (const auto* obstacle : reference_line_info->path_decision()->obstacles().Items()) { @@ -212,7 +261,8 @@ StageResult LaneFollowStage::PlanOnReferenceLine( dest_stop_s = dest_sl.s(); } } - +// 增加障碍物的代价 +// 如果路径上有一个非终点的静态障碍物需要停车，且距离终点较近（<20m），就增加 1000 成本，降低该参考线优先级（避免选一条“走几步就停”的路） for (const auto* obstacle : reference_line_info->path_decision()->obstacles().Items()) { if (obstacle->IsVirtual()) { @@ -240,7 +290,7 @@ StageResult LaneFollowStage::PlanOnReferenceLine( } } } - + // 启用轨迹检查时，验证轨迹是否满足车辆动力学约束 if (FLAGS_enable_trajectory_check) { if (ConstraintChecker::ValidTrajectory(trajectory) != ConstraintChecker::Result::VALID) { @@ -249,13 +299,14 @@ StageResult LaneFollowStage::PlanOnReferenceLine( return ret.SetStageStatus(StageStatusType::ERROR, msg); } } - + // 设置最终轨迹，标记为可行驶，返回成功 reference_line_info->SetTrajectory(trajectory); reference_line_info->SetDrivable(true); ret.SetStageStatus(StageStatusType::RUNNING); return ret; } +// 将障碍物的停车点（x, y）转换到当前参考线的 SL 坐标系（s: 纵向距离, l: 横向偏移） SLPoint LaneFollowStage::GetStopSL(const ObjectStop& stop_decision, const ReferenceLine& reference_line) const { SLPoint sl_point; diff --git a/modules/planning/scenarios/park_and_go/park_and_go_scenario.cc b/modules/planning/scenarios/park_and_go/park_and_go_scenario.cc index 2395dcaca4c..014b08152d5 100644 --- a/modules/planning/scenarios/park_and_go/park_and_go_scenario.cc +++ b/modules/planning/scenarios/park_and_go/park_and_go_scenario.cc @@ -80,14 +80,16 @@ bool ParkAndGoScenario::IsTransferable(const Scenario* const other_scenario, if (nullptr == routing_end) { return false; } + // 把导航终点转换到参考线的 SL 坐标（主要用 s） common::SLPoint dest_sl; const auto& reference_line_info = frame.reference_line_info().front(); const auto& reference_line = reference_line_info.reference_line(); reference_line.XYToSL(routing_end->pose(), &dest_sl); const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s(); - + // 标记车辆是不是在车道上、是否是城市驾驶车道（CITY_DRIVING） bool is_ego_on_lane = false; bool is_lane_type_city_driving = false; + // 按照位置 + 方向 找附近车道 HDMapUtil::BaseMap().GetNearestLaneWithHeading( adc_point, 5.0, vehicle_state.heading(), M_PI / 3.0, &lane, &s, &l); if (lane != nullptr && lane->IsOnLane({adc_point.x(), adc_point.y()})) { @@ -96,7 +98,7 @@ bool ParkAndGoScenario::IsTransferable(const Scenario* const other_scenario, is_lane_type_city_driving = true; } } - + // 计算车辆距离目的地的 s 距离 const double adc_distance_to_dest = dest_sl.s() - adc_front_edge_s; ADEBUG << "adc_distance_to_dest:" << adc_distance_to_dest; bool is_vehicle_static = (std::fabs(adc_speed) < max_abs_speed_when_stopped); diff --git a/modules/planning/tasks/lane_borrow_path/lane_borrow_path.cc b/modules/planning/tasks/lane_borrow_path/lane_borrow_path.cc index 40121d686ad..92c91312c75 100644 --- a/modules/planning/tasks/lane_borrow_path/lane_borrow_path.cc +++ b/modules/planning/tasks/lane_borrow_path/lane_borrow_path.cc @@ -345,42 +345,56 @@ void LaneBorrowPath::UpdateSelfPathInfo() { } blocking_obstacle_id_ = cur_path.blocking_obstacle_id(); } +/// @brief 判断当前是否有必要借用邻车道（lane borrowing） +/// @return bool LaneBorrowPath::IsNecessaryToBorrowLane() { + // mutable_path_decider_status 是一个指针，指向当前路径决策器的可变状态 auto* mutable_path_decider_status = injector_->planning_context() ->mutable_planning_status() ->mutable_path_decider(); + // 判断当前是否已经处于借用邻车道的场景中。如果是，进入接下来的逻辑 if (mutable_path_decider_status->is_in_path_lane_borrow_scenario()) { + // 更新车辆自身的路径信息 UpdateSelfPathInfo(); // If originally borrowing neighbor lane: if (use_self_lane_ >= 6) { // If have been able to use self-lane for some time, then switch to // non-lane-borrowing. + // 这里判断 use_self_lane_ 的值是否大于等于6。如果是，说明车辆已经使用了自车道足够长的时间，可以考虑切换到不借用邻车道的状态 + // 如果条件满足，就更新状态为不借道，并清空已决定的侧向通过方向 (decided_side_pass_direction_)，输出日志 mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false); decided_side_pass_direction_.clear(); AINFO << "Switch from LANE-BORROW path to SELF-LANE path."; } } else { // If originally not borrowing neighbor lane: + // 如果当前不在借道状态中，输出阻塞障碍物的ID AINFO << "Blocking obstacle ID[" << mutable_path_decider_status->front_static_obstacle_id() << "]"; // ADC requirements check for lane-borrowing: + // 只有一条参考线，才能借道 if (!HasSingleReferenceLine(*frame_)) { - return false; + return false; // 不可以借道 } + // 起点速度小于最大借道允许速度 if (!IsWithinSidePassingSpeedADC(*frame_)) { return false; } // Obstacle condition check for lane-borrowing: + // 阻塞障碍物是否远离路口 if (!IsBlockingObstacleFarFromIntersection(*reference_line_info_)) { return false; } + // 阻塞障碍物长期存在 if (!IsLongTermBlockingObstacle()) { return false; } + // 阻塞障碍物是否在终点位置与自车间距之内 if (!IsBlockingObstacleWithinDestination(*reference_line_info_)) { return false; } + // 为可侧面通过的障碍物 if (!IsSidePassableObstacle(*reference_line_info_)) { return false; } @@ -388,6 +402,7 @@ bool LaneBorrowPath::IsNecessaryToBorrowLane() { // switch to lane-borrowing if (decided_side_pass_direction_.empty()) { // first time init decided_side_pass_direction + // 如果 decided_side_pass_direction_ 为空，说明这是第一次判断是否需要借道，需要进行初始化 bool left_borrowable; bool right_borrowable; CheckLaneBorrow(*reference_line_info_, &left_borrowable, @@ -408,9 +423,11 @@ bool LaneBorrowPath::IsNecessaryToBorrowLane() { } } } + // 将 use_self_lane_ 重置为0，并输出日志，表示从自车道路径切换到借道路径 use_self_lane_ = 0; AINFO << "Switch from SELF-LANE path to LANE-BORROW path."; } + // 最后返回当前是否处于借道场景的状态 return mutable_path_decider_status->is_in_path_lane_borrow_scenario(); } @@ -421,13 +438,14 @@ bool LaneBorrowPath::HasSingleReferenceLine(const Frame& frame) { bool LaneBorrowPath::IsWithinSidePassingSpeedADC(const Frame& frame) { return frame.PlanningStartPoint().v() < config_.lane_borrow_max_speed(); } - +/// @brief 判断是否存在长期阻塞障碍物 +/// @return bool LaneBorrowPath::IsLongTermBlockingObstacle() { if (injector_->planning_context() ->planning_status() .path_decider() .front_static_obstacle_cycle_counter() >= - config_.long_term_blocking_obstacle_cycle_threshold()) { + config_.long_term_blocking_obstacle_cycle_threshold()) { // 3 ADEBUG << "The blocking obstacle is long-term existing."; return true; } else { @@ -435,91 +453,115 @@ bool LaneBorrowPath::IsLongTermBlockingObstacle() { return false; } } - +/// @brief 判断一个阻塞障碍物是否位于目标区域内 +/// @param reference_line_info +/// @return bool LaneBorrowPath::IsBlockingObstacleWithinDestination( const ReferenceLineInfo& reference_line_info) { + // 获取路径决策器（path_decider_status）的状态信息 const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider(); const std::string blocking_obstacle_id = path_decider_status.front_static_obstacle_id(); + // 如果 blocking_obstacle_id 为空，意味着没有检测到任何阻塞障碍物 if (blocking_obstacle_id.empty()) { ADEBUG << "There is no blocking obstacle."; return true; } + // 在路径决策中的障碍物集合中查找指定 ID 的障碍物，结果存储在 blocking_obstacle 指针中 const Obstacle* blocking_obstacle = reference_line_info.path_decision().obstacles().Find( blocking_obstacle_id); + // 如果 blocking_obstacle 为 nullptr，意味着找不到该障碍物，可能它已经被移除或者不再存在 if (blocking_obstacle == nullptr) { ADEBUG << "Blocking obstacle is no longer there."; return true; } - +// 获取该阻塞障碍物的起始位置 s 坐标（通常用于表示障碍物在参考线上的位置） double blocking_obstacle_s = blocking_obstacle->PerceptionSLBoundary().start_s(); +// 获取自动驾驶车辆（ADC）的末端位置 s 坐标 double adc_end_s = reference_line_info.AdcSlBoundary().end_s(); ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s; ADEBUG << "ADC is at s = " << adc_end_s; ADEBUG << "Destination is at s = " << reference_line_info.SDistanceToDestination() + adc_end_s; + // blocking_obstacle_s - adc_end_s 计算的是障碍物起始位置到 ADC 末端的距离 + // reference_line_info.SDistanceToDestination() 是目标点距离 ADC 末端的距离 if (blocking_obstacle_s - adc_end_s > reference_line_info.SDistanceToDestination()) { + // 障碍物的起始位置超出了目标区域，则表示该障碍物不在目标区域 return false; } return true; } - +/// @brief 判断一个静态障碍物是否阻挡了前方交叉口，如果阻挡物距离交叉口过近，就不允许车辆侧向通过 +/// @param reference_line_info +/// @return bool LaneBorrowPath::IsBlockingObstacleFarFromIntersection( const ReferenceLineInfo& reference_line_info) { const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider(); + // 获取前方静态障碍物的ID const std::string blocking_obstacle_id = path_decider_status.front_static_obstacle_id(); + // 如果 blocking_obstacle_id 为空，说明没有阻挡物 if (blocking_obstacle_id.empty()) { ADEBUG << "There is no blocking obstacle."; return true; } + // 查找阻挡物 const Obstacle* blocking_obstacle = reference_line_info.path_decision().obstacles().Find( blocking_obstacle_id); + // 如果 blocking_obstacle 为 nullptr，表示阻挡物已经不存在了，输出调试信息并返回 true if (blocking_obstacle == nullptr) { ADEBUG << "Blocking obstacle is no longer there."; return true; } // Get blocking obstacle's s. + // 获取阻挡物的感知边界，并获取其结束位置的s坐标（end_s()） double blocking_obstacle_s = blocking_obstacle->PerceptionSLBoundary().end_s(); ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s; // Get intersection's s and compare with threshold. + // 获取 reference_line_info 中首次遇到的交叉口信息。overlaps 应该包含与路径重叠的其他道路设施（如交通信号、停车标志等） const auto& first_encountered_overlaps = reference_line_info.FirstEncounteredOverlaps(); + // 遍历 first_encountered_overlaps，对每个重叠物体（交叉口或其他设施）进行处理。overlap.first 是设施类型，overlap.second 是设施的具体信息（可能包含位置、大小等数据） for (const auto& overlap : first_encountered_overlaps) { ADEBUG << overlap.first << ", " << overlap.second.DebugString(); + // 如果设施不是信号灯或停车标志，跳过当前迭代，继续处理下一个设施 if (overlap.first != ReferenceLineInfo::SIGNAL && overlap.first != ReferenceLineInfo::STOP_SIGN) { continue; } - + // 计算从阻挡物的s坐标到设施开始位置的距离 distance auto distance = overlap.second.start_s - blocking_obstacle_s; if (overlap.first == ReferenceLineInfo::SIGNAL || overlap.first == ReferenceLineInfo::STOP_SIGN) { - if (distance < kIntersectionClearanceDist) { + // 根据计算出的距离判断，若距离小于预设的阈值 + if (distance < kIntersectionClearanceDist) { // 20 + // 表示阻挡物离交叉口太近，无法执行“侧向通过”操作 ADEBUG << "Too close to signal intersection (" << distance << "m); don't SIDE_PASS."; return false; } } else { - if (distance < kJunctionClearanceDist) { + if (distance < kJunctionClearanceDist) { // 15 ADEBUG << "Too close to overlap_type[" << overlap.first << "] (" << distance << "m); don't SIDE_PASS"; return false; } } } - + // 如果所有检查通过，说明阻挡物距离交叉口足够远，可以继续侧向通过操作 return true; } - +/// @brief +/// @param reference_line_info +/// @return bool LaneBorrowPath::IsSidePassableObstacle( const ReferenceLineInfo& reference_line_info) { const auto& path_decider_status = @@ -540,7 +582,10 @@ bool LaneBorrowPath::IsSidePassableObstacle( return IsNonmovableObstacle(reference_line_info, *blocking_obstacle); } - +/// @brief +/// @param reference_line_info +/// @param left_neighbor_lane_borrowable +/// @param right_neighbor_lane_borrowable void LaneBorrowPath::CheckLaneBorrow( const ReferenceLineInfo& reference_line_info, bool* left_neighbor_lane_borrowable, bool* right_neighbor_lane_borrowable) { @@ -548,18 +593,22 @@ void LaneBorrowPath::CheckLaneBorrow( *left_neighbor_lane_borrowable = true; *right_neighbor_lane_borrowable = true; - + // 定义一个常量kLookforwardDistance，表示检查时需要向前查看的距离 static constexpr double kLookforwardDistance = 100.0; + // 返回车辆所在位置的s坐标 double check_s = reference_line_info.AdcSlBoundary().end_s(); + // 计算实际需要查看的最大前进距离，取当前位置check_s加上kLookforwardDistance和参考线总长度reference_line.Length()中的较小值 const double lookforward_distance = std::min(check_s + kLookforwardDistance, reference_line.Length()); while (check_s < lookforward_distance) { + // 获取参考线中与当前s坐标最近的参考点ref_point auto ref_point = reference_line.GetNearestReferencePoint(check_s); if (ref_point.lane_waypoints().empty()) { *left_neighbor_lane_borrowable = false; *right_neighbor_lane_borrowable = false; return; } + // 通过check_s查找当前参考点对应的车道信息ptr_lane_info auto ptr_lane_info = reference_line_info.LocateLaneInfo(check_s); if (ptr_lane_info->lane().left_neighbor_forward_lane_id().empty() && ptr_lane_info->lane().left_neighbor_reverse_lane_id().empty()) { @@ -569,15 +618,17 @@ void LaneBorrowPath::CheckLaneBorrow( ptr_lane_info->lane().right_neighbor_reverse_lane_id().empty()) { *right_neighbor_lane_borrowable = false; } + // 获取当前参考点车道的第一个路标点waypoint const auto waypoint = ref_point.lane_waypoints().front(); + // 存储当前路标点的车道边界类型，初始化为UNKNOWN hdmap::LaneBoundaryType::Type lane_boundary_type = hdmap::LaneBoundaryType::UNKNOWN; if (*left_neighbor_lane_borrowable) { lane_boundary_type = hdmap::LeftBoundaryType(waypoint); - if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW || - lane_boundary_type == hdmap::LaneBoundaryType::DOUBLE_YELLOW || - lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) { + if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW || // 实黄线 + lane_boundary_type == hdmap::LaneBoundaryType::DOUBLE_YELLOW || // 双黄线 + lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) { // 实白线 *left_neighbor_lane_borrowable = false; } ADEBUG << "s[" << check_s << "] left_lane_boundary_type[" @@ -592,6 +643,7 @@ void LaneBorrowPath::CheckLaneBorrow( ADEBUG << "s[" << check_s << "] right_neighbor_lane_borrowable[" << LaneBoundaryType_Type_Name(lane_boundary_type) << "]"; } + // 每次检查完一个位置后，check_s增加2.0米，继续向前检查 check_s += 2.0; } } diff --git a/modules/planning/tasks/lane_follow_path/lane_follow_path.cc b/modules/planning/tasks/lane_follow_path/lane_follow_path.cc index 5ad0d7cbc16..f343993873d 100644 --- a/modules/planning/tasks/lane_follow_path/lane_follow_path.cc +++ b/modules/planning/tasks/lane_follow_path/lane_follow_path.cc @@ -15,17 +15,27 @@ *****************************************************************************/ #include "modules/planning/tasks/lane_follow_path/lane_follow_path.h" - +// 用于 std::max 等算法 #include +// 智能指针（如 shared_ptr） #include +// 字符串操作 #include +// std::pair, std::move #include +// 动态数组 #include +// 获取车辆几何参数（如轴距） #include "modules/common/configs/vehicle_config_helper.h" +// 调试绘图工具（PrintCurves） #include "modules/planning/planning_base/common/util/print_debug_info.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 { @@ -36,15 +46,23 @@ using apollo::common::VehicleConfigHelper; bool LaneFollowPath::Init(const std::string& config_dir, const std::string& name, const std::shared_ptr& injector) { + // 调用父类 Task::Init() 完成基础初始化 if (!Task::Init(config_dir, name, injector)) { return false; } // Load the config this task. + // 从配置文件加载 LaneFollowPathConfig 到成员变量 config_ + // 使用模板方法 LoadConfig() 自动解析 protobuf 配置。 return Task::LoadConfig(&config_); } +/// @brief +/// @param frame 当前规划周期的全局状态（含自车状态、地图、障碍物等） +/// @param reference_line_info 当前参考线相关信息（含路径、决策、障碍物等） +/// @return apollo::common::Status LaneFollowPath::Process( Frame* frame, ReferenceLineInfo* reference_line_info) { + // 已有路径数据 或者 路径可以复用 if (!reference_line_info->path_data().Empty() || reference_line_info->path_reusable()) { ADEBUG << "Skip this time path empty:" @@ -52,10 +70,16 @@ apollo::common::Status LaneFollowPath::Process( << "path reusable: " << reference_line_info->path_reusable(); return Status::OK(); } + // 候选路径边界（横向可行区域） std::vector candidate_path_boundaries; + // 候选路径（Frenet 坐标下的 L(s) 曲线） std::vector candidate_path_data; - + + // 获取车辆起点的 SL 坐标（S：沿参考线的距离，L：垂直偏移） + // std::pair, std::array> GetStartPointSLState(); + + // 计算候选路径边界（避障的左右限位） if (!DecidePathBounds(&candidate_path_boundaries)) { AERROR << "Decide path bound failed"; return Status::OK(); @@ -64,6 +88,7 @@ apollo::common::Status LaneFollowPath::Process( AERROR << "Optmize path failed"; return Status::OK(); } + // 评估路径并写入 reference_line_info->path_data() if (!AssessPath(&candidate_path_data, reference_line_info->mutable_path_data())) { AERROR << "Path assessment failed"; @@ -71,14 +96,25 @@ apollo::common::Status LaneFollowPath::Process( return Status::OK(); } - +/// @brief +/// @param boundary 一个指向 std::vector 的指针，boundary 用来存储计算出的路径边界 +/// @return bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { + // 创建一个新的 PathBoundary 对象并获取引用 boundary->emplace_back(); auto& path_bound = boundary->back(); + + // 初始化局部变量，用于记录阻塞障碍物等信息 + // 存储障碍物ID std::string blocking_obstacle_id = ""; + // 车道类型 std::string lane_type = ""; + // 路径最窄宽度 double path_narrowest_width = 0; + + // 先初始化路径边界为一个非常宽的区域，保证后续有空间收窄 // 1. Initialize the path boundaries to be an indefinitely large area. + // 步骤1：初始化一个“无限宽”的边界（如 L ∈ [-1e5, 1e5]），作为后续裁剪的基础 if (!PathBoundsDeciderUtil::InitPathBoundary(*reference_line_info_, &path_bound, init_sl_state_)) { const std::string msg = "Failed to initialize path boundaries."; @@ -86,23 +122,35 @@ bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { return false; } std::string borrow_lane_type; + + // 配置决定是否将自动驾驶车本体边界包含在路径边界内，避免规划路径脱离车辆实际占据空间 + // 将自动驾驶车辆（ADC）包括在车道边界内 bool is_include_adc = config_.is_extend_lane_bounds_to_include_adc() && !injector_->planning_context() ->planning_status() .path_decider() .is_in_path_lane_borrow_scenario(); // 2. Decide a rough boundary based on lane info and ADC's position + // 根据车道信息和车辆的位置来决定一个粗略的路径边界 + // 步骤2：根据自车道（self lane）的左右边界，生成粗略的 L 上下界 if (!PathBoundsDeciderUtil::GetBoundaryFromSelfLane( *reference_line_info_, init_sl_state_, &path_bound)) { AERROR << "Failed to decide a rough boundary based on self lane."; return false; } + + // 将车辆占据的空间（含一定缓冲）加入路径边界，避免规划路径侵入车辆自身 + // 若需要，扩展边界确保自车完全包含在内（加 buffer）。 if (is_include_adc) { PathBoundsDeciderUtil::ExtendBoundaryByADC( *reference_line_info_, init_sl_state_, config_.extend_buffer(), &path_bound); } + + // 调试：将所有障碍物在 SL 坐标系下的边界点记录到日志（用于可视化分析）。 PrintCurves print_curve; + + // 遍历障碍物，将其在路径坐标系上的边界点保存，用于后续可视化或日志调试 auto indexed_obstacles = reference_line_info_->path_decision()->obstacles(); for (const auto* obs : indexed_obstacles.Items()) { const auto& sl_bound = obs->PerceptionSLBoundary(); @@ -115,7 +163,13 @@ bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { } print_curve.PrintToLog(); // 3. Fine-tune the boundary based on static obstacles + // 保存原始边界副本，用于后续补充尾部点 PathBound temp_path_bound = path_bound; + // 根据静态障碍物进一步微调路径边界 + // 会选出最远的那个阻塞障碍物id + // 步骤3：根据静态障碍物收缩边界，避免碰撞 + // blocking_obstacle_id：最近/最窄处的障碍物 ID + // path_narrowest_width：最窄宽度 if (!PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles( *reference_line_info_, init_sl_state_, &path_bound, &blocking_obstacle_id, &path_narrowest_width)) { @@ -126,31 +180,41 @@ bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { return false; } // 4. Append some extra path bound points to avoid zero-length path data. + // 如果有阻挡障碍物，给路径边界尾部加额外点，防止路径变成零长度 + // 向路径边界中添加一些额外的边界点，以避免生成零长度的路径数据 + // 步骤4：若因障碍物截断导致路径过短，从原始边界中补点（最多 FLAGS_num_extra_tail_bound_point 个） int counter = 0; while (!blocking_obstacle_id.empty() && path_bound.size() < temp_path_bound.size() && - counter < FLAGS_num_extra_tail_bound_point) { + counter < FLAGS_num_extra_tail_bound_point) { // 20 path_bound.push_back(temp_path_bound[path_bound.size()]); counter++; } + // 更新规划状态中的障碍物阻塞信息 // lane_follow_status update + // 获取规划上下文中的 lane_follow 状态对象，用于更新阻塞信息 auto* lane_follow_status = injector_->planning_context() ->mutable_planning_status() ->mutable_lane_follow(); + // 如果有阻塞障碍物 if (!blocking_obstacle_id.empty()) { + // 如果有阻塞的障碍物，记录阻塞的障碍物ID，并更新阻塞持续时间 double current_time = ::apollo::cyber::Clock::NowInSeconds(); lane_follow_status->set_block_obstacle_id(blocking_obstacle_id); + // 若之前已阻塞 → 累加阻塞时长 if (lane_follow_status->lane_follow_block()) { lane_follow_status->set_block_duration( lane_follow_status->block_duration() + current_time - lane_follow_status->last_block_timestamp()); } else { + // 否则 → 标记为开始阻塞，重置时长 lane_follow_status->set_block_duration(0); lane_follow_status->set_lane_follow_block(true); } lane_follow_status->set_last_block_timestamp(current_time); - } else { + } else { + // 无阻塞，重置相关状态 if (lane_follow_status->lane_follow_block()) { lane_follow_status->set_block_duration(0); lane_follow_status->set_lane_follow_block(false); @@ -159,10 +223,13 @@ bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { } ADEBUG << "Completed generating path boundaries."; + // 检查初始车辆横向位置是否在路径边界内 + // 如果需要包括 ADC，检查初始化时车辆的横向位置是否在路径边界内。如果不在边界内，表示可能发生了车道借道，返回 false if (is_include_adc) { CHECK_LE(init_sl_state_.second[0], path_bound[0].l_upper.l); CHECK_GE(init_sl_state_.second[0], path_bound[0].l_lower.l); } + // 合法性检查：自车初始 L 位置必须在边界内。 if (init_sl_state_.second[0] > path_bound[0].l_upper.l || init_sl_state_.second[0] < path_bound[0].l_lower.l) { AINFO << "not in self lane maybe lane borrow , init l : " @@ -176,28 +243,51 @@ bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { // regular_path_bound_pair.emplace_back(std::get<1>(path_bound[i]), // std::get<2>(path_bound[i])); // } + // 设置路径边界的标签，并记录调试信息（包括边界标签和阻塞障碍物ID） + // 设置边界标签为 "regular/self"，便于追踪来源。 path_bound.set_label(absl::StrCat("regular/", "self")); + // 设置阻塞障碍物 ID path_bound.set_blocking_obstacle_id(blocking_obstacle_id); RecordDebugInfo(path_bound, path_bound.label(), reference_line_info_); return true; } +/// @brief +/// @param path_boundaries +/// @param candidate_path_data +/// @return bool LaneFollowPath::OptimizePath( const std::vector& path_boundaries, std::vector* candidate_path_data) { + // 从成员变量中提取路径优化器的参数配置 const auto& config = config_.path_optimizer_config(); + // 获取参考线: 用于计算曲率（kappa）、起止点坐标等几何信息 const ReferenceLine& reference_line = reference_line_info_->reference_line(); + // 设置终点状态: 路径优化器使用起点、终点状态作为优化约束，这里设定终点为 0 横向偏移、0 速度、0 加速度 + // 硬编码终止状态：(l=0, dl=0, ddl=0) —— 假设终点回到车道中心且平直 std::array end_state = {0.0, 0.0, 0.0}; + // 遍历所有路径边界: 对每一条候选路径边界执行一次路径优化 + // 遍历所有候选边界（通常只有1个） for (const auto& path_boundary : path_boundaries) { + // 路径边界校验: 确保路径边界的点数量足够（至少两个点），否则报错返回失败 + // 边界点数必须 ≥2，否则无法优化 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; } + + // 计算每个 s 点的加速度（ddl）上下界，基于车辆动力学和道路曲率。 + // 定义优化结果容器: 存储优化得到的横向偏移 l、一阶导数 dl（速度）和二阶导数 ddl（曲率变化） std::vector opt_l, opt_dl, opt_ddl; + // 计算曲率二阶导（加速度）的边界：根据路径边界和参考线信息，计算每个点允许的最大曲率变化范围（上下限） std::vector> ddl_bounds; PathOptimizerUtil::CalculateAccBound(path_boundary, reference_line, &ddl_bounds); + + + // 调试：记录参考线曲率 κ(s) + // 调试输出参考线曲率：收集每个点对应的参考线曲率，并输出至日志用于调试（可视化工具查看路径曲率变化） PrintCurves print_debug; for (size_t i = 0; i < path_boundary_size; ++i) { double s = static_cast(i) * path_boundary.delta_s() + @@ -207,31 +297,61 @@ bool LaneFollowPath::OptimizePath( "ref_kappa", static_cast(i) * path_boundary.delta_s(), kappa); } print_debug.PrintToLog(); + + // 估算 jerk（加加速度）上限，基于初始纵向速度（防止高速时路径抖动） + // 估计路径平滑性限制（jerk 限制）：通过当前初始状态的横向速度估算路径允许的最大三阶导数（jerk）边界 const double jerk_bound = PathOptimizerUtil::EstimateJerkBoundary( std::fmax(init_sl_state_.first[1], 1e-12)); + + // 构建参考 L 值（通常为0，即车道中心）和对应权重（越靠近中心权重越高） + // 生成路径参考偏移（ref_l）及其权重：通常以中心线或安全区域为参考，引导优化结果向安全区域居中，同时赋予不同位置不同的优化权重 std::vector ref_l(path_boundary_size, 0); std::vector weight_ref_l(path_boundary_size, 0); PathOptimizerUtil::UpdatePathRefWithBound( path_boundary, config.path_reference_l_weight(), &ref_l, &weight_ref_l); + + // 核心优化：求解分段 jerk 最小化问题，在边界和动力学约束下生成平滑 L(s) + // 执行路径优化： 对当前 path_boundary 区域执行最小化 cost 的轨迹规划，结果保存在 opt_l, opt_dl, opt_ddl 中 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); + + // 将离散优化结果转为 PiecewiseJerkPath（分段三次多项式） if (res_opt) { + // 生成 Frenet 坐标系下的路径结构： 将优化结果转为可用于后续坐标转换与跟踪控制的结构体 auto frenet_frame_path = PathOptimizerUtil::ToPiecewiseJerkPath( opt_l, opt_dl, opt_ddl, path_boundary.delta_s(), path_boundary.start_s()); + + // 构造 PathData 对象 + // 构造路径数据对象 PathData：绑定参考线并设置刚才优化得到的 Frenet 路径 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); } + + // 设置标签和阻塞障碍物，并加入候选列表 + // 设置路径标签和阻塞障碍物 ID + // path_label：用于标识该路径用途（如主道、换道、临停等） + // blocking_obstacle_id：路径被谁阻挡了，便于后续决策使用 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)); + // 调试：记录生成路径的曲率 + PrintCurves print_path_kappa; + for (const auto& p : candidate_path_data->back().discretized_path()) { + print_path_kappa.AddPoint(path_boundary.label() + "_path_kappa", + p.s() + init_sl_state_.first[0], p.kappa()); + } + print_path_kappa.PrintToLog(); } } if (candidate_path_data->empty()) { @@ -242,20 +362,24 @@ bool LaneFollowPath::OptimizePath( bool LaneFollowPath::AssessPath(std::vector* candidate_path_data, PathData* final_path) { + // 取最后一个（当前唯一）路径，记录调试信息。 PathData& curr_path_data = candidate_path_data->back(); RecordDebugInfo(curr_path_data, curr_path_data.path_label(), reference_line_info_); + // 检查路径是否有效（无碰撞、曲率合理、在边界内等） if (!PathAssessmentDeciderUtil::IsValidRegularPath(*reference_line_info_, curr_path_data)) { AINFO << "Lane follow path is invalid"; return false; } - + + // 为每个路径点打标签（如 IN_LANE），供后续速度规划使用。 std::vector path_decision; PathAssessmentDeciderUtil::InitPathPointDecision( curr_path_data, PathData::PathPointType::IN_LANE, &path_decision); curr_path_data.SetPathPointDecisionGuide(std::move(path_decision)); - + + // 若裁剪后路径为空，无效。 if (curr_path_data.Empty()) { AINFO << "Lane follow path is empty after trimed"; return false; diff --git a/modules/planning/tasks/open_space_pre_stop_decider/open_space_pre_stop_decider.cc b/modules/planning/tasks/open_space_pre_stop_decider/open_space_pre_stop_decider.cc index 7d94586ff2e..df1233208ca 100644 --- a/modules/planning/tasks/open_space_pre_stop_decider/open_space_pre_stop_decider.cc +++ b/modules/planning/tasks/open_space_pre_stop_decider/open_space_pre_stop_decider.cc @@ -49,20 +49,27 @@ bool OpenSpacePreStopDecider::Init( AINFO << "Load config:" << config_.DebugString(); return res; } - +/// @brief +/// @param frame +/// @param reference_line_info +/// @return Status OpenSpacePreStopDecider::Process( Frame* frame, ReferenceLineInfo* reference_line_info) { + // 检查输入指针是否为空 CHECK_NOTNULL(frame); CHECK_NOTNULL(reference_line_info); + // 存储目标停车位置的坐标 double target_s = 0.0; const auto& stop_type = config_.stop_type(); switch (stop_type) { case OpenSpacePreStopDeciderConfig::PARKING: + // 检查停车位的预停车条件 if (!CheckParkingSpotPreStop(frame, reference_line_info, &target_s)) { const std::string msg = "Checking parking spot pre stop fails"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } + // 设置虚拟墙 SetParkingSpotStopFence(target_s, frame, reference_line_info); break; case OpenSpacePreStopDeciderConfig::PULL_OVER: @@ -80,10 +87,15 @@ Status OpenSpacePreStopDecider::Process( } return Status::OK(); } - +/// @brief +/// @param frame +/// @param reference_line_info +/// @param target_s 指向目标 s 坐标的指针，函数会将计算出的目标停车位置的 s 坐标存储在此变量中 +/// @return bool OpenSpacePreStopDecider::CheckPullOverPreStop( Frame* const frame, ReferenceLineInfo* const reference_line_info, double* target_s) { + // 目标 s 坐标的初始值 *target_s = 0.0; const auto& pull_over_status = injector_->planning_context()->planning_status().pull_over(); @@ -96,23 +108,34 @@ bool OpenSpacePreStopDecider::CheckPullOverPreStop( } return true; } - +/// @brief 检查目标停车位是否存在，并计算该停车位在参考线上的位置 +/// @param frame +/// @param reference_line_info +/// @param target_s +/// @return bool OpenSpacePreStopDecider::CheckParkingSpotPreStop( Frame* const frame, ReferenceLineInfo* const reference_line_info, double* target_s) { + // 获取目标停车位的 ID const auto& target_parking_spot_id = frame->open_space_info().target_parking_spot_id(); + // 获取参考线信息，并进一步获取与该参考线相关的地图路径（map_path），这将用于定位停车位的位置 const auto& nearby_path = reference_line_info->reference_line().map_path(); if (target_parking_spot_id.empty()) { AERROR << "no target parking spot id found when setting pre stop fence"; return false; } - + // 目标停车区域的 s 坐标，初始化为 0.0 double target_area_center_s = 0.0; + // 是否找到目标停车区域，初始化为 false bool target_area_found = false; + // 获取与参考线相关的停车位重叠区域 const auto& parking_space_overlaps = nearby_path.parking_space_overlaps(); + // 存储目标停车位的信息 ParkingSpaceInfoConstPtr target_parking_spot_ptr; + // 访问地图数据 const hdmap::HDMap* hdmap = hdmap::HDMapUtil::BaseMapPtr(); + // 遍历所有停车位重叠区域（parking_space_overlaps） for (const auto& parking_overlap : parking_space_overlaps) { if (parking_overlap.object_id == target_parking_spot_id) { // TODO(Jinyun) parking overlap s are wrong on map, not usable @@ -121,15 +144,18 @@ bool OpenSpacePreStopDecider::CheckParkingSpotPreStop( hdmap::Id id; id.set_id(parking_overlap.object_id); target_parking_spot_ptr = hdmap->GetParkingSpaceById(id); + // 获取目标停车位的四个顶点坐标（左下、右下、右上、左上） Vec2d left_bottom_point = target_parking_spot_ptr->polygon().points().at(0); Vec2d right_bottom_point = target_parking_spot_ptr->polygon().points().at(1); Vec2d right_up_point = target_parking_spot_ptr->polygon().points().at(2); Vec2d left_up_point = target_parking_spot_ptr->polygon().points().at(3); + // 计算停车位的中心点坐标，即四个顶点坐标的平均值 Vec2d center_point = (left_bottom_point + right_bottom_point + right_up_point + left_up_point) / 4.0; + // 获取停车位中心点 center_point 在参考线中的最近点，并将对应的 s 坐标存储在 target_area_center_s double center_l; nearby_path.GetNearestPoint(center_point, &target_area_center_s, ¢er_l); @@ -144,36 +170,54 @@ bool OpenSpacePreStopDecider::CheckParkingSpotPreStop( *target_s = target_area_center_s; return true; } - +/// @brief 根据目标停车位置和车辆状态计算停车线的位置，并根据情况设置停车围栏。如果目标停车位置距离车辆的前端较远，停车线会设置在目标位置之前； +// 如果目标停车位置距离车辆前端较近，停车线则会设置在目标位置之后。停车线计算完成后，函数会更新停车围栏的位置并构建停车决策 +/// @param target_s +/// @param frame +/// @param reference_line_info void OpenSpacePreStopDecider::SetParkingSpotStopFence( const double target_s, Frame* const frame, ReferenceLineInfo* const reference_line_info) { + // 获取与当前参考线相关的地图路径（map_path），表示车辆所处的道路路径 const auto& nearby_path = reference_line_info->reference_line().map_path(); + // 获取自动驾驶车辆（ADC）的前端边界的 s 坐标 const double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s(); + // 获取车辆的当前状态（vehicle_state），包括速度、加速度等信息 const VehicleState& vehicle_state = frame->vehicle_state(); + // 计算出的停车线的 s 坐标 double stop_line_s = 0.0; + // 配置中获取停车线与目标停车位置之间的距离 double stop_distance_to_target = config_.stop_distance_to_target(); + // 判断车辆是否几乎静止 double static_linear_velocity_epsilon = 1.0e-2; + // 停车缓冲区，用于设置停车线的容忍范围 static constexpr double kStopBuffer = 0.2; + // 检查 stop_distance_to_target 是否大于或等于 1.0e-8 CHECK_GE(stop_distance_to_target, 1.0e-8); + // 目标停车位置与车辆前端边界之间的偏移量 double target_vehicle_offset = target_s - adc_front_edge_s; if (target_vehicle_offset > stop_distance_to_target) { stop_line_s = target_s - stop_distance_to_target; } else if (target_vehicle_offset < stop_distance_to_target - kStopBuffer) { stop_line_s = target_s + stop_distance_to_target; } else if (target_vehicle_offset < -stop_distance_to_target) { + // 如果没有设置 "立即停车" 的标志 if (!frame->open_space_info().pre_stop_rightaway_flag()) { // TODO(Jinyun) Use constant comfortable deacceleration rather than // distance by config to set stop fence stop_line_s = adc_front_edge_s + config_.rightaway_stop_distance(); + // 车辆几乎静止，停车线设置为车辆前端的位置 if (std::abs(vehicle_state.linear_velocity()) < static_linear_velocity_epsilon) { stop_line_s = adc_front_edge_s; } + // 将计算出的停车线位置转换为参考线上的平滑点，并更新到 frame 的开放空间信息中 *(frame->mutable_open_space_info()->mutable_pre_stop_rightaway_point()) = nearby_path.GetSmoothPoint(stop_line_s); + // 设置 "立即停车" 标志为 true，表示已经设置了停车围栏 frame->mutable_open_space_info()->set_pre_stop_rightaway_flag(true); } else { + // 如果 "立即停车" 标志已经设置，那么获取前一次计算的停车点，找到该点在参考线上的最近点并更新停车线位置 double stop_point_s = 0.0; double stop_point_l = 0.0; nearby_path.GetNearestPoint( @@ -182,11 +226,14 @@ void OpenSpacePreStopDecider::SetParkingSpotStopFence( stop_line_s = stop_point_s; } } - + // 定义停车围栏的标识符 stop_wall_id，使用常量 OPEN_SPACE_STOP_ID 作为唯一标识符 const std::string stop_wall_id = OPEN_SPACE_STOP_ID; + // 存储需要等待的障碍物标识符 std::vector wait_for_obstacles; + // 将计算得到的停车线位置（stop_line_s）更新到 frame 的开放空间信息中，表示停车围栏的 s 坐标 frame->mutable_open_space_info()->set_open_space_pre_stop_fence_s( stop_line_s); + // 创建停车决策 util::BuildStopDecision(stop_wall_id, stop_line_s, 0.0, StopReasonCode::STOP_REASON_PRE_OPEN_SPACE_STOP, wait_for_obstacles, "OpenSpacePreStopDecider", frame, @@ -197,7 +244,9 @@ void OpenSpacePreStopDecider::SetPullOverStopFence( const double target_s, Frame* const frame, ReferenceLineInfo* const reference_line_info) { const auto& nearby_path = reference_line_info->reference_line().map_path(); + // 车辆前端在参考线上的位置 const double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s(); + // 获取当前车辆的状态信息，包括速度、加速度等 const VehicleState& vehicle_state = frame->vehicle_state(); double stop_line_s = 0.0; double stop_distance_to_target = config_.stop_distance_to_target(); @@ -206,7 +255,7 @@ void OpenSpacePreStopDecider::SetPullOverStopFence( double target_vehicle_offset = target_s - adc_front_edge_s; if (target_vehicle_offset > stop_distance_to_target) { stop_line_s = target_s - stop_distance_to_target; - } else { + } else {// 如果尚未设置 "右侧停车优先" 标志 if (!frame->open_space_info().pre_stop_rightaway_flag()) { // TODO(Jinyun) Use constant comfortable deacceleration rather than // distance by config to set stop fence diff --git a/modules/planning/tasks/open_space_roi_decider/open_space_roi_decider.cc b/modules/planning/tasks/open_space_roi_decider/open_space_roi_decider.cc index 0543ead0019..025dd7b0219 100644 --- a/modules/planning/tasks/open_space_roi_decider/open_space_roi_decider.cc +++ b/modules/planning/tasks/open_space_roi_decider/open_space_roi_decider.cc @@ -58,7 +58,9 @@ bool OpenSpaceRoiDecider::Init( AINFO << config_.DebugString(); return res; } - +/// @brief 处理不同类型的开放空间区域（ROI，Region of Interest） +/// @param frame +/// @return Status OpenSpaceRoiDecider::Process(Frame *frame) { if (frame == nullptr) { const std::string msg = @@ -69,15 +71,19 @@ Status OpenSpaceRoiDecider::Process(Frame *frame) { vehicle_state_ = frame->vehicle_state(); obstacles_by_frame_ = frame->GetObstacleList(); - + // 存储停车位的四个顶点坐标 std::array spot_vertices; + // 存储附近的路径 Path nearby_path; // @brief vector of different obstacle consisting of vertice points.The // obstacle and the vertices order are in counter-clockwise order + // 一个二维vector，用于存储开放空间区域的边界，边界由一系列顶点（Vec2d）表示 std::vector> roi_boundary; - + // 获取当前选择的开放空间区域类型（roi_type） const auto &roi_type = config_.roi_type(); + // 检查roi_type是否为PARKING（停车场） if (roi_type == OpenSpaceRoiDeciderConfig::PARKING) { + // 从frame中获取目标停车位的ID，并存储到target_parking_spot_id_中 target_parking_spot_id_ = frame->open_space_info().target_parking_spot_id(); ParkingInfo parking_info; if (!GetParkingSpot(frame, &parking_info)) { @@ -88,34 +94,36 @@ Status OpenSpaceRoiDecider::Process(Frame *frame) { frame->mutable_open_space_info()->set_parking_type( parking_info.parking_type); - + // 根据停车位信息设置原点 SetOrigin(parking_info, frame); - + // 设置停车位的结束姿态（即停车时车辆的位置和朝向） SetParkingSpotEndPose(parking_info, frame); - + // 从地图上获取停车位的边界，并将其存储在roi_boundary中 if (!GetParkingBoundary(parking_info, *nearby_path_, frame, &roi_boundary)) { const std::string msg = "Fail to get parking boundary from map"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } + // 检查roi_type是否为PULL_OVER（停车） } else if (roi_type == OpenSpaceRoiDeciderConfig::PULL_OVER) { if (!GetPullOverSpot(frame, &spot_vertices, &nearby_path)) { const std::string msg = "Fail to get parking boundary from map"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } - + // 根据停车点的四个顶点设置原点 SetOrigin(frame, spot_vertices); - + // 设置停车结束姿态 SetPullOverSpotEndPose(frame); - + // 从地图上获取停车区域的边界，并将其存储在roi_boundary中 if (!GetPullOverBoundary(frame, spot_vertices, nearby_path, &roi_boundary)) { const std::string msg = "Fail to get parking boundary from map"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } + // 检查roi_type是否为PARK_AND_GO（停放并行驶） } else if (roi_type == OpenSpaceRoiDeciderConfig::PARK_AND_GO) { ADEBUG << "in Park_and_Go"; nearby_path = @@ -142,6 +150,7 @@ Status OpenSpaceRoiDecider::Process(Frame *frame) { adc_point, 2.0, vehicle_state_.heading(), M_PI / 3.0, &lane, &s, &l) == -1; } + // 根据是否处于停车区域外，分别获取停车并行驶区域的边界 if (is_parking_out) { AINFO << "GetParkingOutBoundary!!"; if (!GetParkingOutBoundary(nearby_path, frame, &roi_boundary)) { @@ -1210,20 +1219,25 @@ bool OpenSpaceRoiDecider::GetParkingSpot(Frame *const frame, } const auto &parking_spot_id_string = frame->open_space_info().target_parking_spot_id(); + // 将字符串转换为地图内部使用的 Id 类型 hdmap::Id parking_spot_id = hdmap::MakeMapId(parking_spot_id_string); + // 从高精地图中获取对应的 ParkingSpace 对象（包含多边形、朝向等信息） auto parking_spot = hdmap_->GetParkingSpaceById(parking_spot_id); if (!nearby_path_) { + // 获取停车位附近的参考路径: 作为开放空间规划的参考线 GetNearbyPath(frame->local_view().planning_command->lane_follow_command(), parking_spot, &nearby_path_); } // points in polygon is always clockwise auto points = parking_spot->polygon().points(); OpenSpaceRoiUtil::UpdateParkingPointsOrder(*nearby_path_, &points); + // 计算停车位中心点 Vec2d center_point(0, 0); for (size_t i = 0; i < points.size(); i++) { center_point += points[i]; } center_point /= 4.0; + // 获取中心点在参考路径上的投影与航向 double lane_heading = 0; parking_info->center_point = center_point; nearby_path_->GetHeadingAlongPath(center_point, &lane_heading); @@ -1235,6 +1249,7 @@ bool OpenSpaceRoiDecider::GetParkingSpot(Frame *const frame, } else { parking_info->is_on_left = false; } + // 初步判断泊车类型（基于航向角差异） double diff_angle = common::math::AngleDiff( lane_heading, parking_spot->parking_space().heading()); if (std::fabs(diff_angle) < M_PI / 3.0) { @@ -1242,9 +1257,13 @@ bool OpenSpaceRoiDecider::GetParkingSpot(Frame *const frame, } else { parking_info->parking_type = ParkingType::VERTICAL_PARKING; } + // “top” 指的是靠近道路的一侧（即入口边），"bottom" 是远离道路的一侧 parking_info->corner_points = points; + // 最终判断泊车类型（基于几何尺寸） + // parallel_dist：[0] 到 [1] 的距离（假设为长边） double parallel_dist = parking_info->corner_points[0].DistanceTo(parking_info->corner_points[1]); + // verticle_dist：[0] 到 [3] 的距离（假设为短边） double verticle_dist = parking_info->corner_points[0].DistanceTo(parking_info->corner_points[3]); if (parallel_dist > verticle_dist) { @@ -1674,26 +1693,38 @@ void OpenSpaceRoiDecider::GetAllLaneSegments( } } +/// @brief +/// @param routing_response 全局路径规划结果（包含车辆应行驶的车道序列） +/// @param parking_spot 目标停车位信息（来自高精地图） +/// @param nearby_path 生成的局部参考路径（由 1~3 个连续车道段组成） +/// @return bool OpenSpaceRoiDecider::GetNearbyPath( const apollo::routing::RoutingResponse &routing_response, const ParkingSpaceInfoConstPtr &parking_spot, std::shared_ptr *nearby_path) { + // 1.校验停车位有效性 LaneInfoConstPtr nearest_lane; if (nullptr == parking_spot) { - AERROR << "The parking spot id is invalid!" << parking_spot->id().id(); + // AERROR << "The parking spot id is invalid!" << parking_spot->id().id(); + AERROR << "The parking spot is invalid!"; return false; } + // 2.通过 Overlap 关系找到关联车道 auto parking_space = parking_spot->parking_space(); + // 返回与该停车位有空间重叠（或逻辑连接）的 Overlap 对象 ID 列表 auto overlap_ids = parking_space.overlap_id(); if (overlap_ids.empty()) { AERROR << "There is no lane overlaps with the parking spot: " << parking_spot->id().id(); return false; } + // 获取 Routing 中的所有车道段 std::vector lane_segments; GetAllLaneSegments(routing_response, &lane_segments); + // 遍历 Overlap，查找在 Routing 中的最近车道 bool has_found_nearest_lane = false; size_t nearest_lane_index = 0; + // 有问题： for (auto id : overlap_ids) { auto overlaps = hdmap_->GetOverlapById(id)->overlap(); for (auto object : overlaps.object()) { @@ -1705,6 +1736,7 @@ bool OpenSpaceRoiDecider::GetNearbyPath( continue; } // Check if the lane is contained in the routing response. + // 检查该车道是否在 routing 路径中 for (auto &segment : lane_segments) { if (segment.id() == nearest_lane->id().id()) { has_found_nearest_lane = true; @@ -1725,6 +1757,7 @@ bool OpenSpaceRoiDecider::GetNearbyPath( // Get the lane nearest to the current position of the vehicle. If the // vehicle has not reached the nearest lane to the parking spot, set the // lane nearest to the vehicle as "nearest_lane". + // 考虑车辆当前位置（动态调整参考路径） LaneInfoConstPtr nearest_lane_to_vehicle; auto point = common::util::PointFactory::ToPointENU(vehicle_state_); double vehicle_lane_s = 0.0; @@ -1732,7 +1765,7 @@ bool OpenSpaceRoiDecider::GetNearbyPath( int status = hdmap_->GetNearestLaneWithHeading( point, 10.0, vehicle_state_.heading(), M_PI / 2.0, &nearest_lane_to_vehicle, &vehicle_lane_s, &vehicle_lane_l); - if (status == 0) { + if (status == 0) { // 成功找到 size_t nearest_lane_to_vehicle_index = 0; bool has_found_nearest_lane_to_vehicle = false; for (auto &segment : lane_segments) { @@ -1742,22 +1775,26 @@ bool OpenSpaceRoiDecider::GetNearbyPath( } ++nearest_lane_to_vehicle_index; } + // 确保 nearby_path 从车辆当前位置开始，而不是直接跳到停车位附近 // The vehicle has not reached the nearest lane to the parking spot。 + // 如果车辆所在车道在 routing 中，且排在停车位车道之前 if (has_found_nearest_lane_to_vehicle && nearest_lane_to_vehicle_index < nearest_lane_index) { - nearest_lane = nearest_lane_to_vehicle; + nearest_lane = nearest_lane_to_vehicle; // 替换为车辆当前车道 } } // Find parking spot by getting nearestlane + // 构建 nearby_path（1~3 个连续车道段） ParkingSpaceInfoConstPtr target_parking_spot = nullptr; LaneSegment nearest_lanesegment = LaneSegment(nearest_lane, nearest_lane->accumulate_s().front(), nearest_lane->accumulate_s().back()); std::vector segments_vector; + // 添加后继车道（最多再加两个） int next_lanes_num = nearest_lane->lane().successor_id_size(); if (next_lanes_num != 0) { - auto next_lane_id = nearest_lane->lane().successor_id(0); + auto next_lane_id = nearest_lane->lane().successor_id(0); // 取第一个后继 segments_vector.push_back(nearest_lanesegment); auto next_lane = hdmap_->GetLaneById(next_lane_id); LaneSegment next_lanesegment = @@ -1765,6 +1802,7 @@ bool OpenSpaceRoiDecider::GetNearbyPath( next_lane->accumulate_s().back()); segments_vector.push_back(next_lanesegment); size_t succeed_lanes_num = next_lane->lane().successor_id_size(); + // 再取后继的后继 if (succeed_lanes_num != 0) { auto succeed_lane_id = next_lane->lane().successor_id(0); auto succeed_lane = hdmap_->GetLaneById(succeed_lane_id); diff --git a/modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_optimizer.cc b/modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_optimizer.cc index 9f64f1aa86b..eb09e5ecccb 100644 --- a/modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_optimizer.cc +++ b/modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_optimizer.cc @@ -36,21 +36,37 @@ OpenSpaceTrajectoryOptimizer::OpenSpaceTrajectoryOptimizer( // Load config AINFO << config_.DebugString(); // Initialize hybrid astar class pointer + // 用于生成初始可行路径（warm start），考虑车辆运动学约束和障碍物 warm_start_.reset(new HybridAStar(config.planner_open_space_config())); // Initialize dual variable warm start class pointer + // 为后续优化提供对偶变量初值（用于距离约束的拉格朗日乘子） dual_variable_warm_start_.reset( new DualVariableWarmStartProblem(config.planner_open_space_config())); // Initialize distance approach trajectory smootherclass pointer + // 基于距离约束的轨迹平滑器（使用非线性优化/NLP） distance_approach_.reset( new DistanceApproachProblem(config.planner_open_space_config())); // Initialize iterative anchoring smoother config class pointer + // 另一种平滑方法（可能更轻量或用于 fallback） iterative_anchoring_smoother_.reset( new IterativeAnchoringSmoother(config.planner_open_space_config())); } +/// @brief +/// @param stitching_trajectory 上一周期规划的轨迹末尾点，用于衔接（stitching） +/// @param end_pose 目标位姿 [x, y, theta, v] +/// @param XYbounds ROI 区域边界 [xmin, xmax, ymin, ymax] +/// @param rotate_angle 坐标系变换参数（将世界坐标转为局部坐标） +/// @param translate_origin +/// @param obstacles_edges_num +/// @param obstacles_A +/// @param obstacles_b +/// @param obstacles_vertices_vec +/// @param time_latency 输出：本次规划耗时（ms） +/// @return Status OpenSpaceTrajectoryOptimizer::Plan( const std::vector& stitching_trajectory, const std::vector& end_pose, const std::vector& XYbounds, @@ -98,6 +114,7 @@ Status OpenSpaceTrajectoryOptimizer::Plan( << ")"; // Rotate and scale the state + // 将世界坐标下的初始点转换到 局部 ROI 坐标系（原点在 translate_origin，x 轴旋转 rotate_angle） PathPointNormalizing(rotate_angle, translate_origin, &init_x, &init_y, &init_phi); @@ -125,7 +142,7 @@ Status OpenSpaceTrajectoryOptimizer::Plan( Eigen::MatrixXd n_warm_up; Eigen::MatrixXd dual_l_result_ds; Eigen::MatrixXd dual_n_result_ds; - + // 分段平滑（partition + parallel smooth） if (FLAGS_enable_parallel_trajectory_smoothing) { std::vector partition_trajectories; if (!warm_start_->TrajectoryPartition(result, &partition_trajectories)) { @@ -240,6 +257,7 @@ Status OpenSpaceTrajectoryOptimizer::Plan( &n_warm_up, &dual_l_result_ds, &dual_n_result_ds); } else { + // 单段平滑 AINFO << "use distance approach smoother"; LoadHybridAstarResultInEigen(&result, &xWS, &uWS); diff --git a/modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_provider.cc b/modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_provider.cc index 116f5ac5fe8..7639b9048c7 100644 --- a/modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_provider.cc +++ b/modules/planning/tasks/open_space_trajectory_provider/open_space_trajectory_provider.cc @@ -43,14 +43,18 @@ using apollo::cyber::Clock; bool OpenSpaceTrajectoryProvider::Init( const std::string& config_dir, const std::string& name, const std::shared_ptr& injector) { + // 调用基类 Task::Init() 初始化通用任务框架 if (!Task::Init(config_dir, name, injector)) { return false; } // Load the config this task. + // 加载本任务的专属配置（OpenSpaceTrajectoryProviderConfig），存入 config_ if (!Task::LoadConfig(&config_)) { return false; } + // 从配置中读取“直线轨迹长度”（用于泊车后微调） straight_trajectory_length_ = config_.open_space_straight_trajectory_length(); + // 创建轨迹优化器 OpenSpaceTrajectoryOptimizer 实例（核心规划算法） open_space_trajectory_optimizer_.reset(new OpenSpaceTrajectoryOptimizer( config_.open_space_trajectory_optimizer_config())); return true; @@ -92,10 +96,12 @@ void OpenSpaceTrajectoryProvider::Restart() { Status OpenSpaceTrajectoryProvider::Process() { ADEBUG << "trajectory provider"; + // 1.获取当前帧中用于存储最终轨迹的可变引用 auto trajectory_data = frame_->mutable_open_space_info()->mutable_stitched_trajectory_result(); // generate stop trajectory at park_and_go check_stage + // 2.如果处于“泊车检查阶段”，直接生成原地停车轨迹（不规划） if (injector_->planning_context() ->mutable_planning_status() ->mutable_park_and_go() @@ -104,12 +110,16 @@ Status OpenSpaceTrajectoryProvider::Process() { GenerateStopTrajectory(trajectory_data); return Status::OK(); } + + // 3.若启用多线程且尚未启动，则用 cyber::Async 启动 GenerateTrajectoryThread // Start thread when getting in Process() for the first time if (config_.enable_open_space_planner_thread() && !thread_init_flag_) { task_future_ = cyber::Async( &OpenSpaceTrajectoryProvider::GenerateTrajectoryThread, this); thread_init_flag_ = true; } + + // 4.判断是否需要重新规划（replan） bool need_replan = false; // Get stitching trajectory from last frame const common::VehicleState vehicle_state = frame_->vehicle_state(); @@ -122,6 +132,8 @@ Status OpenSpaceTrajectoryProvider::Process() { previous_frame->open_space_info().fallback_flag(), vehicle_state)) { is_stop_due_to_fallback = true; } + + // 从未规划成功（!is_planned_）或上一帧因回退（fallback）导致车辆停下，则需要 重新初始化轨迹 if (!is_planned_ || is_stop_due_to_fallback) { AINFO << "need to fallback: is_planned" << is_planned_ << "is_stop_due_to_fallback" << is_stop_due_to_fallback; @@ -129,6 +141,7 @@ Status OpenSpaceTrajectoryProvider::Process() { 1.0 / static_cast(FLAGS_planning_loop_rate); stitching_trajectory = TrajectoryStitcher::ComputeReinitStitchingTrajectory( planning_cycle_time, vehicle_state); + // 设置 need_replan = true，触发新规划 need_replan = true; injector_->planning_context() ->mutable_planning_status() @@ -138,25 +151,29 @@ Status OpenSpaceTrajectoryProvider::Process() { } // Get open_space_info from current frame const auto& open_space_info = frame_->open_space_info(); - + // 5.多线程模式处理（enable_open_space_planner_thread == true） if (config_.enable_open_space_planner_thread()) { ADEBUG << "Open space plan in multi-threads mode"; - + // 若线程已停止 → 生成停车轨迹 if (is_generation_thread_stop_) { GenerateStopTrajectory(trajectory_data); return Status(ErrorCode::OK, "Parking finished"); } - + + // 若需要重新规划 → 准备数据给后台线程 if (need_replan) { std::vector temp_target = frame_->open_space_info().open_space_end_pose(); if (open_space_info.target_parking_spot_id() != "") { double angle = open_space_info.open_space_end_pose()[2]; + // 将目标点向前延伸一段（straight_trajectory_length_），用于更平滑泊入 + // 目标点前移是为了让车辆在到达真实停车位前就开始减速对齐 temp_target[0] = straight_trajectory_length_ * cos(angle) + open_space_info.open_space_end_pose()[0]; temp_target[1] = straight_trajectory_length_ * sin(angle) + open_space_info.open_space_end_pose()[1]; } + // 加锁，将规划所需数据拷贝到 thread_data_ std::lock_guard lock(open_space_mutex_); thread_data_.stitching_trajectory = stitching_trajectory; thread_data_.end_pose = temp_target; @@ -168,7 +185,8 @@ Status OpenSpaceTrajectoryProvider::Process() { thread_data_.obstacles_vertices_vec = open_space_info.obstacles_vertices_vec(); thread_data_.XYbounds = open_space_info.ROI_xy_boundary(); - data_ready_.store(true); + // ... 其他障碍物、边界等数据 ... + data_ready_.store(true); // 通知后台线程可以开始规划 is_planned_ = true; } else { std::lock_guard lock(open_space_mutex_); @@ -176,6 +194,7 @@ Status OpenSpaceTrajectoryProvider::Process() { } // Check vehicle state + // 检查是否接近终点 if (IsVehicleNearDestination( vehicle_state, open_space_info.open_space_end_pose(), open_space_info.origin_heading(), open_space_info.origin_point())) { @@ -186,12 +205,15 @@ Status OpenSpaceTrajectoryProvider::Process() { } // Check if trajectory updated + // 若后台线程已生成新轨迹 → 加载结果 if (trajectory_updated_) { std::lock_guard lock(open_space_mutex_); + // LoadResult() 会拼接“衔接轨迹”和“优化轨迹”，形成完整轨迹 LoadResult(trajectory_data); if (FLAGS_enable_record_debug) { // call merge debug ptr, open_space_trajectory_optimizer_ auto* ptr_debug = frame_->mutable_open_space_info()->mutable_debug(); + // 同步调试信息 open_space_trajectory_optimizer_->UpdateDebugInfo( ptr_debug->mutable_planning_data()->mutable_open_space()); @@ -203,7 +225,8 @@ Status OpenSpaceTrajectoryProvider::Process() { trajectory_updated_.store(false); return Status::OK(); } - + + // 若规划出错 → 计数并重试 if (trajectory_error_) { AINFO << "error"; ++optimizer_thread_counter; @@ -218,7 +241,8 @@ Status OpenSpaceTrajectoryProvider::Process() { "open_space_optimizer failed too many times"); } } - + + // 若上一帧成功且无需 replan → 复用上一帧轨迹 if (previous_frame && previous_frame->open_space_info().open_space_provider_success() && !need_replan) { @@ -234,10 +258,12 @@ Status OpenSpaceTrajectoryProvider::Process() { "open_space_trajectory_provider"); } else { AINFO << "Stop due to computation not finished"; + // 否则 → 生成停车轨迹（等待规划完成） GenerateStopTrajectory(trajectory_data); return Status(ErrorCode::OK, "Stop due to computation not finished"); } } else { + // 6.单线程模式（enable_open_space_planner_thread == false） const auto& end_pose = open_space_info.open_space_end_pose(); const auto& rotate_angle = open_space_info.origin_heading(); const auto& translate_origin = open_space_info.origin_point(); @@ -277,6 +303,7 @@ Status OpenSpaceTrajectoryProvider::Process() { void OpenSpaceTrajectoryProvider::GenerateTrajectoryThread() { while (!is_generation_thread_stop_) { if (!trajectory_updated_ && data_ready_) { + // 拷贝 thread_data_（加锁） OpenSpaceTrajectoryThreadData thread_data; { std::lock_guard lock(open_space_mutex_); @@ -310,6 +337,7 @@ bool OpenSpaceTrajectoryProvider::IsVehicleNearDestination( const common::VehicleState& vehicle_state, const std::vector& end_pose, double rotate_angle, const Vec2d& translate_origin) { + // 将目标点从局部坐标系转换到世界坐标系（旋转 + 平移） CHECK_EQ(end_pose.size(), 4U); Vec2d end_pose_to_world_frame = Vec2d(end_pose[0], end_pose[1]); @@ -372,6 +400,7 @@ bool OpenSpaceTrajectoryProvider::IsVehicleStopDueToFallBack( return false; } +//生成一段原地停车轨迹（10 个点，每 0.1 秒一个） void OpenSpaceTrajectoryProvider::GenerateStopTrajectory( DiscretizedTrajectory* const trajectory_data) { double relative_time = 0.0; @@ -425,7 +454,7 @@ void OpenSpaceTrajectoryProvider::LoadResult( optimizer_trajectory_ptr->at(i).path_point().s() + stitching_point_relative_s); } - + // 最后那段倒车轨迹是硬编码的匀减速运动（v=0.3m/s, a=-v/t），方向与末尾朝向相反 if (frame_->open_space_info().target_parking_spot_id() != "") { double distance = straight_trajectory_length_; double v = 0.3; diff --git a/modules/planning/tasks/path_decider/path_decider.cc b/modules/planning/tasks/path_decider/path_decider.cc index ad6823c0507..ac974def9be 100644 --- a/modules/planning/tasks/path_decider/path_decider.cc +++ b/modules/planning/tasks/path_decider/path_decider.cc @@ -52,46 +52,71 @@ Status PathDecider::Execute(Frame *frame, reference_line_info->path_decision()); } +/// @brief 根据生成好的路径 path_data 和当前障碍物信息，更新规划状态、识别阻塞障碍物、并为障碍物决策 path_decision 做准备 +/// @param reference_line_info +/// @param path_data +/// @param path_decision +/// @return Status PathDecider::Process(const ReferenceLineInfo *reference_line_info, const PathData &path_data, PathDecision *const path_decision) { + // 启用跳过路径任务、同时路径是可复用的 // skip path_decider if reused path if (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) { return Status::OK(); } + + // 创建一个用于调试绘图的对象 debug_info PrintCurves debug_info; + // 从 path_data 中取出离散化路径 const auto &path = path_data.discretized_path(); + + // 获取路径起点对应的车辆包围盒，并将其所有角点添加到调试输出中 if (!path.empty()) { const auto &vehicle_box = common::VehicleConfigHelper::Instance()->GetBoundingBox(path[0]); debug_info.AddPoint("start_point_box", vehicle_box.GetAllCorners()); } + // 把路径中所有点的坐标加入调试曲线 for (const auto &path_pt : path) { debug_info.AddPoint("output_path", path_pt.x(), path_pt.y()); } + // 输出调试信息到日志中 debug_info.PrintToLog(); + + // 初始化阻塞障碍物 ID（blocking_obstacle_id） std::string blocking_obstacle_id; + // 获取 planning_context 中的 path_decider_status 对象，用于记录阻塞障碍物状态 auto *mutable_path_decider_status = injector_->planning_context() ->mutable_planning_status() ->mutable_path_decider(); + // 如果当前参考线上有阻塞障碍物 if (reference_line_info->GetBlockingObstacle() != nullptr) { + // 获取该障碍物 ID 作为阻塞障碍物 blocking_obstacle_id = reference_line_info->GetBlockingObstacle()->Id(); + // 保证周期计数器大于等于 0（代表出现过） int front_static_obstacle_cycle_counter = mutable_path_decider_status->front_static_obstacle_cycle_counter(); mutable_path_decider_status->set_front_static_obstacle_cycle_counter( std::max(front_static_obstacle_cycle_counter, 0)); + // 每次检测到阻塞障碍物，将计数器上限增加到 10（最多累计 10 次） mutable_path_decider_status->set_front_static_obstacle_cycle_counter( std::min(front_static_obstacle_cycle_counter + 1, 10)); + // 在 planning_context 中记录阻塞障碍物的 ID mutable_path_decider_status->set_front_static_obstacle_id( reference_line_info->GetBlockingObstacle()->Id()); } else { + // 如果当前没有检测到阻塞障碍物 + // 确保计数器不为正数 int front_static_obstacle_cycle_counter = mutable_path_decider_status->front_static_obstacle_cycle_counter(); mutable_path_decider_status->set_front_static_obstacle_cycle_counter( std::min(front_static_obstacle_cycle_counter, 0)); + // 每次未检测到阻塞物，计数器减少一次，最低为 -10 mutable_path_decider_status->set_front_static_obstacle_cycle_counter( std::max(front_static_obstacle_cycle_counter - 1, -10)); + // 如果连续多帧（超过2次）未检测到障碍物，将 front_static_obstacle_id 清空（设为空格字符串） if (mutable_path_decider_status->front_static_obstacle_cycle_counter() < -2) { std::string id = " "; @@ -106,6 +131,11 @@ Status PathDecider::Process(const ReferenceLineInfo *reference_line_info, return Status::OK(); } +/// @brief 生成针对路径上的障碍物的决策（如绕行、停车、忽略等） +/// @param path_data 当前规划路径数据 +/// @param blocking_obstacle_id 路径上认为“阻塞”的主要障碍物 ID +/// @param path_decision 最终决策会写入这个对象中 +/// @return bool PathDecider::MakeObjectDecision(const PathData &path_data, const std::string &blocking_obstacle_id, PathDecision *const path_decision) { @@ -124,10 +154,12 @@ bool PathDecider::MakeObjectDecision(const PathData &path_data, // TODO(jiacheng): eventually this entire "path_decider" should be retired. // Before it gets retired, its logics are slightly modified so that everything // still works well for now. +// 根据路径数据和阻挡障碍物 ID，对路径决策对象中的每个静态障碍物做出决策 bool PathDecider::MakeStaticObstacleDecision( const PathData &path_data, const std::string &blocking_obstacle_id, PathDecision *const path_decision) { // Sanity checks and get important values. + // 合法性检查 ACHECK(path_decision); const auto &frenet_path = path_data.frenet_frame_path(); if (frenet_path.empty()) { @@ -136,32 +168,42 @@ bool PathDecider::MakeStaticObstacleDecision( } const double half_width = common::VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0; - const double lateral_radius = half_width + FLAGS_lateral_ignore_buffer; + const double lateral_radius = half_width + FLAGS_lateral_ignore_buffer; // FLAGS_lateral_ignore_buffer：3 // Go through every obstacle and make decisions. + // 遍历所有障碍物指针 for (const auto *obstacle : path_decision->obstacles().Items()) { + // 获取障碍物 ID 和类型（调试打印） const std::string &obstacle_id = obstacle->Id(); const std::string obstacle_type_name = PerceptionObstacle_Type_Name(obstacle->Perception().type()); ADEBUG << "obstacle_id[<< " << obstacle_id << "] type[" << obstacle_type_name << "]"; - + + // 跳过非静态障碍物或虚拟障碍物 if (!obstacle->IsStatic() || obstacle->IsVirtual()) { continue; } + // - skip decision making for obstacles with IGNORE/STOP decisions already. + // 如果该障碍物已经被设定为前向和横向都是 IGNORE 决策，则跳过 if (obstacle->HasLongitudinalDecision() && obstacle->LongitudinalDecision().has_ignore() && obstacle->HasLateralDecision() && obstacle->LateralDecision().has_ignore()) { continue; } + + // 如果已经有 STOP 决策，也跳过 if (obstacle->HasLongitudinalDecision() && obstacle->LongitudinalDecision().has_stop()) { // STOP decision continue; } + // - add STOP decision for blocking obstacles. + // 针对阻挡型障碍物添加 STOP 决策 + // 如果当前障碍物是 blocking_obstacle_id 并且不是车道借道场景 if (obstacle->Id() == blocking_obstacle_id && !injector_->planning_context() ->planning_status() @@ -175,15 +217,21 @@ bool PathDecider::MakeStaticObstacleDecision( obstacle->Id(), object_decision); continue; } + // - skip decision making for clear-zone obstacles. + // 忽略 CLEAR 区域的障碍物 if (obstacle->reference_line_st_boundary().boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) { continue; } // 0. IGNORE by default and if obstacle is not in path s at all. + // 初始化一个 IGNORE 决策 ObjectDecisionType object_decision; object_decision.mutable_ignore(); + + // 如果障碍物 s 范围和路径完全没有重叠 + // 则设置为 IGNORE 并跳过 const auto &sl_boundary = obstacle->PerceptionSLBoundary(); if (sl_boundary.end_s() < frenet_path.front().s() || sl_boundary.start_s() > frenet_path.back().s()) { @@ -193,21 +241,28 @@ bool PathDecider::MakeStaticObstacleDecision( object_decision); continue; } - + + // 判断障碍物横向位置（是否 STOP 或 NUDGE） + // 获取路径上与障碍物最近的点 l 值；min_nudge_l 是“会撞上”的横向判定距离 const auto frenet_point = frenet_path.GetNearestPoint(sl_boundary); const double curr_l = frenet_point.l(); double min_nudge_l = half_width + config_.static_obstacle_buffer() / 2.0; - + if (curr_l - lateral_radius > sl_boundary.end_l() || curr_l + lateral_radius < sl_boundary.start_l()) { + // 障碍物在横向上离得太远 // 1. IGNORE if laterally too far away. + // 条件 1：障碍物横向离得很远，IGNORE path_decision->AddLateralDecision("PathDecider/not-in-l", obstacle->Id(), object_decision); } else if (sl_boundary.end_l() >= curr_l - min_nudge_l && sl_boundary.start_l() <= curr_l + min_nudge_l) { + // 若障碍物和路径横向重叠范围超过最小避让范围 // 2. STOP if laterally too overlapping. + // 条件 2：障碍物横向重叠，STOP *object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle); - + + // 如果当前 STOP 可以合并为主停车决策，则添加，否则设为 IGNORE if (path_decision->MergeWithMainStop( object_decision.stop(), obstacle->Id(), reference_line_info_->reference_line(), @@ -226,6 +281,7 @@ bool PathDecider::MakeStaticObstacleDecision( << min_nudge_l; } else { // 3. NUDGE if laterally very close. + // 条件 3：距离很近但不重叠，NUDGE if (sl_boundary.end_l() < curr_l - min_nudge_l) { // && // sl_boundary.end_l() > curr_l - min_nudge_l - 0.3) { // LEFT_NUDGE @@ -252,29 +308,43 @@ bool PathDecider::MakeStaticObstacleDecision( ObjectStop PathDecider::GenerateObjectStopDecision( const Obstacle &obstacle) const { ObjectStop object_stop; - + // 把停车点设置为车辆前缘刚好到达该停止点 double stop_distance = obstacle.MinRadiusStopDistance( VehicleConfigHelper::GetConfig().vehicle_param()); + // 设置停车的原因码为：由于障碍物停车（STOP_REASON_OBSTACLE） object_stop.set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE); + // 设置停车点相对于障碍物的 s 向距离。注意是负值，意味着停车点位于障碍物前方（在车辆前进方向上提前停车） object_stop.set_distance_s(-stop_distance); + // 计算停车点在参考线上的 s 坐标：障碍物的前沿 s 减去安全停车距离 const double stop_ref_s = obstacle.PerceptionSLBoundary().start_s() - stop_distance; + // 在参考线上获取 stop_ref_s 位置对应的 x, y, heading 世界坐标 const auto stop_ref_point = reference_line_info_->reference_line().GetReferencePoint(stop_ref_s); + // 将停止点的 x 和 y 坐标设置为参考线上的对应位置 object_stop.mutable_stop_point()->set_x(stop_ref_point.x()); object_stop.mutable_stop_point()->set_y(stop_ref_point.y()); + // 设置停车点的朝向（heading），与参考线保持一致 object_stop.set_stop_heading(stop_ref_point.heading()); return object_stop; } +// 忽略在车辆后方的非静态障碍物，避免对它们进行无意义的决策处理 bool PathDecider::IgnoreBackwardObstacle(PathDecision *const path_decision) { + // 获取车辆起始位置 s 值 double adc_start_s = reference_line_info_->AdcSlBoundary().start_s(); + + // 遍历路径决策中的所有障碍物 for (const auto *obstacle : path_decision->obstacles().Items()) { + // 过滤掉静态或虚拟障碍物 if (obstacle->IsStatic() || obstacle->IsVirtual()) { continue; } + + // 判断障碍物是否在车辆后方 if (obstacle->Obstacle::PerceptionSLBoundary().end_s() < adc_start_s) { + // 为后方障碍物添加 IGNORE 决策 ObjectDecisionType object_decision; object_decision.mutable_ignore(); path_decision->AddLongitudinalDecision( diff --git a/modules/planning/tasks/path_time_heuristic/gridded_path_time_graph.cc b/modules/planning/tasks/path_time_heuristic/gridded_path_time_graph.cc index 145a3f4e43f..8f2dad75432 100644 --- a/modules/planning/tasks/path_time_heuristic/gridded_path_time_graph.cc +++ b/modules/planning/tasks/path_time_heuristic/gridded_path_time_graph.cc @@ -91,15 +91,19 @@ GriddedPathTimeGraph::GriddedPathTimeGraph( std::min(std::abs(vehicle_param_.max_deceleration()), std::abs(gridded_path_time_graph_config_.max_deceleration())); } - +/// @brief 通过动态规划方法搜索最优的速度轨迹。这个函数会计算路径上的每个点的最优速度，同时考虑障碍物和速度限制等约束条件。 +/// @param speed_data 输出参数，存储了最终计算出的速度数据，表示路径上每个时刻的速度 +/// @return Status GriddedPathTimeGraph::Search(SpeedData* const speed_data) { static constexpr double kBounadryEpsilon = 1e-2; for (const auto& boundary : st_graph_data_.st_boundaries()) { // KeepClear obstacles not considered in Dp St decision + // 判断当前边界是否为 KEEP_CLEAR 类型的障碍物，这种障碍物不会被纳入动态规划计算中 if (boundary->boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) { continue; } // If init point in collision with obstacle, return speed fallback + // 检查初始化点（通常是 (0, 0) 代表起始位置）是否与当前障碍物发生碰撞 if (boundary->IsPointInBoundary({0.0, 0.0}) || (std::fabs(boundary->min_t()) < kBounadryEpsilon && std::fabs(boundary->min_s()) < kBounadryEpsilon)) { diff --git a/modules/planning/tasks/path_time_heuristic/path_time_heuristic_optimizer.cc b/modules/planning/tasks/path_time_heuristic/path_time_heuristic_optimizer.cc index 9b518a34ba6..394673c5b17 100644 --- a/modules/planning/tasks/path_time_heuristic/path_time_heuristic_optimizer.cc +++ b/modules/planning/tasks/path_time_heuristic/path_time_heuristic_optimizer.cc @@ -28,7 +28,7 @@ namespace apollo { namespace planning { - +// https://blog.csdn.net/sinat_52032317/article/details/132612308 using apollo::common::ErrorCode; using apollo::common::Status; @@ -41,7 +41,9 @@ bool PathTimeHeuristicOptimizer::Init( // Load the config_ this task. return SpeedOptimizer::LoadConfig(&config_); } - +/// @brief +/// @param speed_data +/// @return bool PathTimeHeuristicOptimizer::SearchPathTimeGraph( SpeedData* speed_data) const { const auto& dp_st_speed_optimizer_config = @@ -59,7 +61,11 @@ bool PathTimeHeuristicOptimizer::SearchPathTimeGraph( } return true; } - +/// @brief 通过动态规划（Dynamic Programming，DP）方法来优化路径的时间分配，并生成对应的速度数据 +/// @param path_data 包含路径信息的结构体，通常包括离散化的路径（路径上的多个点）、参考线等 +/// @param init_point 初始轨迹点，通常包含车辆的初始位置和速度 +/// @param speed_data 存储计算后的速度数据，通常是车辆在路径上每个点的速度信息 +/// @return Status PathTimeHeuristicOptimizer::Process( const PathData& path_data, const common::TrajectoryPoint& init_point, SpeedData* const speed_data) { diff --git a/modules/planning/tasks/pull_over_path/proto/pull_over_path.proto b/modules/planning/tasks/pull_over_path/proto/pull_over_path.proto index 542cbebdee7..efdf7863836 100644 --- a/modules/planning/tasks/pull_over_path/proto/pull_over_path.proto +++ b/modules/planning/tasks/pull_over_path/proto/pull_over_path.proto @@ -15,11 +15,11 @@ message PullOverPathConfig { DESTINATION = 1; } optional PiecewiseJerkPathConfig path_optimizer_config = 1; - optional double pull_over_road_edge_buffer = 2 [default = 0.15]; - optional double pull_over_weight = 3 [default = 10]; - optional double pull_over_approach_lon_distance_adjust_factor = 5 [default = 1.5]; - optional double pull_over_destination_to_adc_buffer = 6 [default = 25.0]; - optional double pull_over_destination_to_pathend_buffer = 7 [default = 10.0]; - optional PullOverDirection pull_over_direction = 8 [default = RIGHT_SIDE]; - optional PullOverPosition pull_over_position = 9 [default = DESTINATION]; + optional double pull_over_road_edge_buffer = 2 [default = 0.15]; //离道路边缘的最小缓冲距离（单位：米） + optional double pull_over_weight = 3 [default = 10]; // 靠边路径的代价权重:值越大越倾向选择靠边路径 + optional double pull_over_approach_lon_distance_adjust_factor = 5 [default = 1.5]; // 靠边路径接近段的纵向距离调节因子:动态调整停车前的减速段长度 + optional double pull_over_destination_to_adc_buffer = 6 [default = 25.0]; // 目标点与车辆之间最小前向距离：防止车辆过早停车或距离太短影响安全 + optional double pull_over_destination_to_pathend_buffer = 7 [default = 10.0]; // 目标点与路径末端的最小距离：确保停车点不太靠近路径末尾，避免急停或路径不足 + optional PullOverDirection pull_over_direction = 8 [default = RIGHT_SIDE]; // 靠边方向设定： + optional PullOverPosition pull_over_position = 9 [default = DESTINATION]; // 停车点选择方式设定 } \ No newline at end of file diff --git a/modules/planning/tasks/pull_over_path/pull_over_path.cc b/modules/planning/tasks/pull_over_path/pull_over_path.cc index e865d3b65cd..d0f093b9ad6 100644 --- a/modules/planning/tasks/pull_over_path/pull_over_path.cc +++ b/modules/planning/tasks/pull_over_path/pull_over_path.cc @@ -72,6 +72,7 @@ apollo::common::Status PullOverPath::Process( } bool PullOverPath::DecidePathBounds(std::vector* boundary) { + // 获取当前规划上下文中的泊车状态对象 pull_over_status，可以用于读取/修改泊车相关状态 auto* pull_over_status = injector_->planning_context() ->mutable_planning_status() ->mutable_pull_over(); @@ -79,20 +80,25 @@ bool PullOverPath::DecidePathBounds(std::vector* boundary) { if (!plan_pull_over_path) { return false; } + // 为 boundary 添加一个新的 PathBoundary 实例，并引用为 path_bound 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_)) { AERROR << "Failed to initialize path boundaries."; return false; } + // 根据道路边界进行粗略裁剪 if (!GetBoundaryFromRoads(*reference_line_info_, &path_bound)) { AERROR << "Failed to decide a rough boundary based on road boundary."; return false; } RecordDebugInfo(path_bound, "pull_over_road", reference_line_info_); + // 将边界的坐标系从“车道中心”为基准转换为“参考线”为基准 PathBoundsDeciderUtil::ConvertBoundarySAxisFromLaneCenterToRefLine( *reference_line_info_, &path_bound); @@ -102,6 +108,7 @@ bool PullOverPath::DecidePathBounds(std::vector* boundary) { "path"; return false; } + // 根据配置决定是向左泊车还是向右泊车。如果配置是“左右都行”，则计算左右边界距离来自动决定方向 bool is_pull_over_right = true; if (config_.pull_over_direction() == PullOverPathConfig::BOTH_SIDE) { double adc_to_left_bound = @@ -115,11 +122,13 @@ bool PullOverPath::DecidePathBounds(std::vector* boundary) { is_pull_over_right = true; } // 2. Update boundary by lane boundary for pull_over + // 进一步裁剪边界：基于车道边界 UpdatePullOverBoundaryByLaneBoundary(is_pull_over_right, &path_bound); RecordDebugInfo(path_bound, "pull_over_lane", reference_line_info_); std::string blocking_obstacle_id = ""; PathBound temp_path_bound = path_bound; + // 根据静态障碍物进一步缩小路径边界。记录被挡住的障碍物 ID 以及路径最窄宽度 if (!PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles( *reference_line_info_, init_sl_state_, &path_bound, &blocking_obstacle_id, &path_narrowest_width)) { @@ -128,6 +137,7 @@ bool PullOverPath::DecidePathBounds(std::vector* boundary) { return false; } // If already found a pull-over position, simply check if it's valid. + // 检查当前已知的泊车目标点是否仍在当前路径边界内，返回对应的索引。如果不在范围内，返回 -1 int curr_idx = -1; if (pull_over_status->has_position()) { curr_idx = PathBoundsDeciderUtil::IsPointWithinPathBound( @@ -135,6 +145,7 @@ bool PullOverPath::DecidePathBounds(std::vector* boundary) { pull_over_status->position().y(), path_bound); } // If haven't found a pull-over position, search for one. + // 如果还没有找到泊车点，就在裁剪后的路径边界中搜索可行泊车点 if (curr_idx < 0) { pull_over_status->Clear(); pull_over_status->set_plan_pull_over_path(true); @@ -145,8 +156,10 @@ bool PullOverPath::DecidePathBounds(std::vector* boundary) { } const auto& vehicle_param = VehicleConfigHelper::GetConfig().vehicle_param(); + // 获取泊车点对应的边界索引 curr_idx = std::get<3>(pull_over_configuration); // If have found a pull-over position, update planning-context + // 将泊车点的信息（位置和朝向）更新到 pull_over_status 中 pull_over_status->mutable_position()->set_x( std::get<0>(pull_over_configuration)); pull_over_status->mutable_position()->set_y( @@ -163,10 +176,12 @@ bool PullOverPath::DecidePathBounds(std::vector* boundary) { << pull_over_status->theta() << "]"; } // Trim path-bound properly + // 修剪路径边界为泊车区域所需的长度：将边界修剪为以泊车点为终点、保留一定后备点数的长度 while (static_cast(path_bound.size()) - 1 > curr_idx + FLAGS_num_extra_tail_bound_point) { path_bound.pop_back(); } + // 修剪后段边界，保持泊车区域边界一致 for (size_t idx = curr_idx + 1; idx < path_bound.size(); ++idx) { path_bound[idx].l_lower.l = path_bound[curr_idx].l_lower.l; path_bound[idx].l_upper.l = path_bound[curr_idx].l_upper.l; @@ -273,37 +288,51 @@ bool PullOverPath::AssessPath(std::vector* candidate_path_data, return true; } +/// @brief +/// @param reference_line_info +/// @param path_bound +/// @return bool PullOverPath::GetBoundaryFromRoads( const ReferenceLineInfo& reference_line_info, PathBoundary* const path_bound) { // Sanity checks. + // 合法性检查 CHECK_NOTNULL(path_bound); ACHECK(!path_bound->empty()); + const ReferenceLine& reference_line = reference_line_info.reference_line(); + // 获取车辆当前位置（s = init_sl_state_.first[0]）所在的车道宽度 double adc_lane_width = PathBoundsDeciderUtil::GetADCLaneWidth( reference_line, init_sl_state_.first[0]); // Go through every point, update the boudnary based on the road boundary. + // 初始化左右侧道路宽度，以备某些点获取失败时使用 double past_road_left_width = adc_lane_width / 2.0; double past_road_right_width = adc_lane_width / 2.0; int path_blocked_idx = -1; for (size_t i = 0; i < path_bound->size(); ++i) { // 1. Get road boundary. double curr_s = (*path_bound)[i].s; + // 初始化当前点的道路边界宽度和参考线到车道中心的偏移 double curr_road_left_width = 0.0; double curr_road_right_width = 0.0; double refline_offset_to_lane_center = 0.0; + // 获取当前 s 位置参考线相对于车道中心线的横向偏移 reference_line.GetOffsetToMap(curr_s, &refline_offset_to_lane_center); + // 尝试获取当前 s 处的道路左、右宽度 if (!reference_line.GetRoadWidth(curr_s, &curr_road_left_width, &curr_road_right_width)) { AWARN << "Failed to get lane width at s = " << curr_s; curr_road_left_width = past_road_left_width; curr_road_right_width = past_road_right_width; } else { + // 当前参考线可能不是完全居中，需要根据偏移进行左右边界调整 curr_road_left_width += refline_offset_to_lane_center; curr_road_right_width -= refline_offset_to_lane_center; + // 记录当前宽度，为下一点失败时备用 past_road_left_width = curr_road_left_width; past_road_right_width = curr_road_right_width; } + // 计算左右界限（以中心线为 0） double curr_left_bound = curr_road_left_width; double curr_right_bound = -curr_road_right_width; ADEBUG << "At s = " << curr_s @@ -313,16 +342,18 @@ bool PullOverPath::GetBoundaryFromRoads( << refline_offset_to_lane_center; // 2. Update into path_bound. + // 更新 path_bound 当前点的左右边界 if (!PathBoundsDeciderUtil::UpdatePathBoundaryWithBuffer( curr_left_bound, curr_right_bound, BoundType::ROAD, BoundType::ROAD, "", "", &path_bound->at(i))) { path_blocked_idx = static_cast(i); } + // 一旦发现某点被阻塞，停止后续处理 if (path_blocked_idx != -1) { break; } } - + // 将从阻塞点之后的所有 path_bound 点删掉，保持边界合法 PathBoundsDeciderUtil::TrimPathBounds(path_blocked_idx, path_bound); return true; } @@ -353,6 +384,10 @@ void PullOverPath::UpdatePullOverBoundaryByLaneBoundary( } } +/// @brief +/// @param path_bound 路径边界 path_bound（每个点包含 s, l_upper, l_lower） +/// @param pull_over_configuration 泊车位置信息 pull_over_configuration（x, y, θ, 索引） +/// @return bool PullOverPath::SearchPullOverPosition( const PathBound& path_bound, std::tuple* const pull_over_configuration) { @@ -452,7 +487,7 @@ bool PullOverPath::SearchPullOverPosition( << curr_road_left_width << "] curr_road_right_width[" << curr_road_right_width << "]"; if (curr_road_right_width - (curr_right_bound + adc_half_width) > - config_.pull_over_road_edge_buffer()) { + config_.pull_over_road_edge_buffer()) { // 0.15 AERROR << "Not close enough to road-edge. Not feasible for pull-over."; is_feasible_window = false; break; @@ -462,6 +497,7 @@ bool PullOverPath::SearchPullOverPosition( ADEBUG << "left_bound[" << left_bound << "] right_bound[" << right_bound << "]"; if (left_bound - right_bound < pull_over_space_width) { + // 至少要能横向放下整个车辆（含一定 buffer） AERROR << "Not wide enough to fit ADC. Not feasible for pull-over."; is_feasible_window = false; break; @@ -543,7 +579,9 @@ bool PullOverPath::FindNearestPullOverS(double* pull_over_s) { .min_turn_radius(); const double adjust_factor = config_.pull_over_approach_lon_distance_adjust_factor(); + // 设定拉车靠边所需的最小纵向距离 const double pull_over_distance = min_turn_radius * 2 * adjust_factor; + // 目标 pull over 的参考线 s 值 就是在当前车尾位置往前偏移 pull_over_distance 米 *pull_over_s = adc_end_s + pull_over_distance; return true; } @@ -561,6 +599,7 @@ bool PullOverPath::FindDestinationPullOverS(const PathBound& path_bound, // Check if destination is some distance away from ADC. ADEBUG << "Destination s[" << destination_s << "] adc_end_s[" << adc_end_s << "]"; + // 车辆在离终点不足 25 米的距离内不进行靠边决策 if (destination_s - adc_end_s < config_.pull_over_destination_to_adc_buffer()) { AERROR << "Destination is too close to ADC. distance[" @@ -569,6 +608,7 @@ bool PullOverPath::FindDestinationPullOverS(const PathBound& path_bound, } // Check if destination is within path-bounds searching scope. + // 车辆靠边目标点必须距离 path_bound 末尾不少于 10 米 const double destination_to_pathend_buffer = config_.pull_over_destination_to_pathend_buffer(); if (destination_s + destination_to_pathend_buffer >= path_bound.back().s) { diff --git a/modules/planning/tasks/reuse_path/reuse_path.cc b/modules/planning/tasks/reuse_path/reuse_path.cc index c8ba43415f8..aabaffacaf9 100644 --- a/modules/planning/tasks/reuse_path/reuse_path.cc +++ b/modules/planning/tasks/reuse_path/reuse_path.cc @@ -60,7 +60,10 @@ apollo::common::Status ReusePath::Process( return Status::OK(); } - +/// @brief 判断当前路径是否可以被重用，包括车道变更（lane change）、路径规划失败、障碍物碰撞等情况 +/// @param frame 当前信息 +/// @param reference_line_info 参考路径的信息 +/// @return bool ReusePath::IsPathReusable(Frame* frame, ReferenceLineInfo* const reference_line_info) { // active path reuse during change_lane only @@ -68,11 +71,13 @@ bool ReusePath::IsPathReusable(Frame* frame, ->mutable_planning_status() ->mutable_change_lane(); ADEBUG << "lane change status: " << lane_change_status->ShortDebugString(); + // 如果当前路径不是车道变更路径且配置中没有启用车道保持过程中重用路径，则跳过重用路径的判断 if (!reference_line_info->IsChangeLanePath() && !config_.enable_reuse_path_in_lane_follow()) { ADEBUG << "skipping reusing path: not in lane_change"; return false; } + // 如果当前路径是车道变更路径，但车道变更状态不是“正在变更车道”，则不能重用路径 if (reference_line_info->IsChangeLanePath() && lane_change_status->status() != ChangeLaneStatus::IN_CHANGE_LANE) { return false; @@ -82,18 +87,27 @@ bool ReusePath::IsPathReusable(Frame* frame, // 2. collision // 3. failed to trim previous path // 4. speed optimization failed on previous path + // 停止重用当前路径 + //如果路径重新规划（replan）了； + //如果发生了碰撞； + //如果上次路径修剪失败； + //如果之前的路径速度优化失败 bool speed_optimization_successful = false; + // 获取最新的历史帧信息 history_frame const auto& history_frame = injector_->frame_history()->Latest(); + // 如果存在历史帧，则判断历史路径的速度优化是否成功（通过轨迹类型是否为 SPEED_FALLBACK） if (history_frame) { const auto history_trajectory_type = history_frame->reference_line_info().front().trajectory_type(); speed_optimization_successful = (history_trajectory_type != ADCTrajectory::SPEED_FALLBACK); + // 如果历史帧的当前规划路径为空，则返回 false if (history_frame->current_frame_planned_path().empty()) { return false; } } if (path_reusable_) { + // 如果当前帧没有重新规划路径（is_replan()），并且速度优化成功且没有碰撞（通过 IsCollisionFree 函数检查），则可以重用路径 if (!frame->current_frame_planned_trajectory().is_replan() && speed_optimization_successful && IsCollisionFree(reference_line_info)) { ADEBUG << "reuse path"; @@ -109,7 +123,7 @@ bool ReusePath::IsPathReusable(Frame* frame, ->mutable_planning_status() ->mutable_path_decider(); static constexpr int kWaitCycle = -2; // wait 2 cycle - + // 获取前方静态障碍物的周期计数器，并判断是否忽略了前方的障碍物（ignore_blocking_obstacle） const int front_static_obstacle_cycle_counter = mutable_path_decider_status->front_static_obstacle_cycle_counter(); const bool ignore_blocking_obstacle = @@ -120,6 +134,7 @@ bool ReusePath::IsPathReusable(Frame* frame, // 1. blocking obstacle disappeared or moving far away // 2. trimming successful // 3. no statical obstacle collision. + // 如果前方的静态障碍物已消失或移动得很远，或者我们可以忽略这个障碍物，并且速度优化成功且没有碰撞，则启用路径重用 if ((front_static_obstacle_cycle_counter <= kWaitCycle || ignore_blocking_obstacle) && speed_optimization_successful && IsCollisionFree(reference_line_info)) { @@ -130,23 +145,35 @@ bool ReusePath::IsPathReusable(Frame* frame, } return path_reusable_; } - +/// @brief 判断车辆与前方障碍物的距离是否足够远，如果足够远，就可以忽略该障碍物 +/// @param reference_line_info 参考线的信息，它包含了参考路径（reference line）和路径上的各种信息 +/// @return bool ReusePath::IsIgnoredBlockingObstacle( ReferenceLineInfo* const reference_line_info) { + // 获取的是参考线对象 const ReferenceLine& reference_line = reference_line_info->reference_line(); + // 一个用于判断障碍物是否可以忽略的距离缓冲区，单位是米（30米） static constexpr double kSDistBuffer = 30.0; // meter + // 一个时间缓冲区，单位是秒（3秒） static constexpr int kTimeBuffer = 3; // second // vehicle speed + // 获取当前车辆的线速度（速度），单位是米每秒（m/s） double adc_speed = injector_->vehicle_state()->linear_velocity(); + // 计算最终的缓冲区距离 double final_s_buffer = std::max(kSDistBuffer, kTimeBuffer * adc_speed); // current vehicle s position + // adc_position_sl 是当前车辆在参考线上的 s 和 l 坐标，s 是沿路径的前进距离，l 是偏移量 common::SLPoint adc_position_sl; + // 获取车辆在参考线上的位置 GetADCSLPoint(reference_line, &adc_position_sl); // blocking obstacle start s - double blocking_obstacle_start_s; + double blocking_obstacle_start_s; // 前方阻塞障碍物的起始 s 坐标 + // GetBlockingObstacleS 用于获取阻塞障碍物的 s 坐标 + // 如果障碍物的起始 s 值存在，并且障碍物距离车辆的距离大于 final_s_buffer（即车辆和障碍物之间的距离大于缓冲区） if (GetBlockingObstacleS(reference_line_info, &blocking_obstacle_start_s) && // distance to blocking obstacle (blocking_obstacle_start_s - adc_position_sl.s() > final_s_buffer)) { + // 如果满足条件，说明当前障碍物距离车辆足够远，可以忽略该障碍物 ADEBUG << "blocking obstacle distance: " << blocking_obstacle_start_s - adc_position_sl.s(); return true; @@ -154,46 +181,61 @@ bool ReusePath::IsIgnoredBlockingObstacle( return false; } } - +/// @brief 获取前方阻塞障碍物的 s 坐标（参考线上的位置），通过查找阻塞障碍物的 ID，获取该障碍物的位置信息，并返回障碍物起始位置的 s 值 +/// @param reference_line_info 包含参考路径信息的对象，它提供了参考线和路径决策的相关信息 +/// @param blocking_obstacle_s 一个指针，函数通过它返回阻塞障碍物的 s 坐标 +/// @return bool ReusePath::GetBlockingObstacleS( ReferenceLineInfo* const reference_line_info, double* blocking_obstacle_s) { + // 获取的是路径决策器的状态信息，它存储了与路径选择和规划相关的各种信息 auto* mutable_path_decider_status = injector_->planning_context() ->mutable_planning_status() ->mutable_path_decider(); // get blocking obstacle ID (front_static_obstacle_id) + // blocking_obstacle_ID 获取当前阻塞障碍物的 ID。front_static_obstacle_id 是阻塞障碍物的唯一标识符，它用于区分不同的障碍物 const std::string& blocking_obstacle_ID = mutable_path_decider_status->front_static_obstacle_id(); + // indexed_obstacles 是参考线信息中包含的障碍物列表（以 ID 为索引）。path_decision()->obstacles() 获取当前路径决策中的所有障碍物 const IndexedList& indexed_obstacles = reference_line_info->path_decision()->obstacles(); + // Find(blocking_obstacle_ID) 方法通过 ID 查找并返回对应的阻塞障碍物。如果找不到障碍物，则返回 nullptr const auto* blocking_obstacle = indexed_obstacles.Find(blocking_obstacle_ID); - + // 如果 blocking_obstacle 为 nullptr，表示未找到阻塞障碍物 if (blocking_obstacle == nullptr) { return false; } - + // PerceptionSLBoundary() 获取障碍物的 SL 边界（即障碍物在参考线上的起始和终止位置） const auto& obstacle_sl = blocking_obstacle->PerceptionSLBoundary(); + // obstacle_sl.start_s() 获取该障碍物的起始 s 坐标，并将其赋值给传入的 blocking_obstacle_s 指针 *blocking_obstacle_s = obstacle_sl.start_s(); ADEBUG << "blocking obstacle distance: " << obstacle_sl.start_s(); return true; } - +/// @brief 获取车辆的当前位置，并将该位置从笛卡尔坐标系转换为参考路径上的 S-L 坐标系，最后将结果存储在 adc_position_sl 中 +/// @param reference_line +/// @param adc_position_sl void ReusePath::GetADCSLPoint(const ReferenceLine& reference_line, common::SLPoint* adc_position_sl) { + // 车辆在笛卡尔坐标系中的位置 common::math::Vec2d adc_position = {injector_->vehicle_state()->x(), injector_->vehicle_state()->y()}; reference_line.XYToSL(adc_position, adc_position_sl); } - +/// @brief +/// @param reference_line_info +/// @return bool ReusePath::IsCollisionFree(ReferenceLineInfo* const reference_line_info) { const ReferenceLine& reference_line = reference_line_info->reference_line(); static constexpr double kMinObstacleArea = 1e-4; const double kSBuffer = 0.5; // current vehicle sl position common::SLPoint adc_position_sl; + // 设置参数，获取自车的SL坐标 GetADCSLPoint(reference_line, &adc_position_sl); // current obstacles std::vector obstacle_polygons; + // 遍历当前的障碍物，忽略掉所有动态、虚拟的障碍物 for (auto obstacle : reference_line_info->path_decision()->obstacles().Items()) { // filtered all non-static objects and virtual obstacle @@ -206,7 +248,7 @@ bool ReusePath::IsCollisionFree(ReferenceLineInfo* const reference_line_info) { } continue; } - + // 忽略掉自车后面的障碍物以及过小的障碍物 const auto& obstacle_sl = obstacle->PerceptionSLBoundary(); // Ignore obstacles behind ADC if ((obstacle_sl.end_s() < adc_position_sl.s() - kSBuffer) || @@ -238,6 +280,7 @@ bool ReusePath::IsCollisionFree(ReferenceLineInfo* const reference_line_info) { return false; } // path end point + // 将上一段轨迹的终点投影到SL坐标系下 common::SLPoint path_end_position_sl; common::math::Vec2d path_end_position = {history_path.back().x(), history_path.back().y()}; @@ -297,7 +340,10 @@ bool ReusePath::NotShortPath(const DiscretizedPath& current_path) { // TODO(shu): use gflag return current_path.size() >= config_.short_path_threshold(); } - +/// @brief +/// @param frame 当前的帧数据（类型为 Frame*），可能包含当前车辆状态、路径规划点等信息 +/// @param reference_line_info 包含参考线（reference_line）和相关路径数据（path_data）的信息 +/// @return bool ReusePath::TrimHistoryPath(Frame* frame, ReferenceLineInfo* const reference_line_info) { const ReferenceLine& reference_line = reference_line_info->reference_line(); @@ -306,7 +352,7 @@ bool ReusePath::TrimHistoryPath(Frame* frame, ADEBUG << "no history frame"; return false; } - + // 从历史帧中获取规划起点（PlanningStartPoint）并获取对应的路径点（PathPoint） const common::TrajectoryPoint history_planning_start_point = history_frame->PlanningStartPoint(); common::PathPoint history_init_path_point = @@ -315,26 +361,30 @@ bool ReusePath::TrimHistoryPath(Frame* frame, << history_init_path_point.x() << "], y[" << history_init_path_point.y() << "], s: [" << history_init_path_point.s() << "]"; - + // 获取当前帧的规划起点，并提取路径点信息，输出当前路径点的坐标和弧长信息用于调试 const common::TrajectoryPoint planning_start_point = frame->PlanningStartPoint(); common::PathPoint init_path_point = planning_start_point.path_point(); ADEBUG << "init_path_point x:[" << std::setprecision(9) << init_path_point.x() << "], y[" << init_path_point.y() << "], s: [" << init_path_point.s() << "]"; - + // 获取历史帧的规划路径（history_path） const DiscretizedPath& history_path = history_frame->current_frame_planned_path(); + // 存储修剪后的路径 DiscretizedPath trimmed_path; common::SLPoint adc_position_sl; // current vehicle sl position + // 将车辆当前位置转换为 SL 坐标系（弧长 s 和横向位置 l） GetADCSLPoint(reference_line, &adc_position_sl); ADEBUG << "adc_position_sl.s(): " << adc_position_sl.s(); ADEBUG << "history_path.size(): " << history_path.size(); + // 遍历历史路径，找到第一个弧长 s 大于 0 的点，设定为路径的起始点。输出该起始点的索引 size_t path_start_index = 0; for (size_t i = 0; i < history_path.size(); ++i) { // find previous init point if (history_path[i].s() > 0) { + // // 找到上周期轨迹规划的起点索引 path_start_index = i; break; } @@ -342,25 +392,28 @@ bool ReusePath::TrimHistoryPath(Frame* frame, ADEBUG << "!!!path_start_index[" << path_start_index << "]"; // get current s=0 + // 将当前路径点转换为 SL 坐标系，并初始化一个布尔变量 inserted_init_point，用于标记是否已经插入了当前的起始点 common::SLPoint init_path_position_sl; reference_line.XYToSL(init_path_point, &init_path_position_sl); bool inserted_init_point = false; +// 从 path_start_index 开始，遍历历史路径中的每一个点，将其从笛卡尔坐标（x, y）转换为 SL 坐标。 for (size_t i = path_start_index; i < history_path.size(); ++i) { common::SLPoint path_position_sl; common::math::Vec2d path_position = {history_path[i].x(), history_path[i].y()}; reference_line.XYToSL(path_position, &path_position_sl); - + // 计算上一帧中每个路径点与当前起始点之间的弧长差 updated_s double updated_s = path_position_sl.s() - init_path_position_sl.s(); // insert init point + // 如果 updated_s > 0 且当前还没有插入起始点，就将当前路径点 init_path_point 插入到 trimmed_path 中，并设置其弧长为 0 if (updated_s > 0 && !inserted_init_point) { trimmed_path.emplace_back(init_path_point); trimmed_path.back().set_s(0); inserted_init_point = true; } - + // 然后将历史路径中的每个点插入到 trimmed_path trimmed_path.emplace_back(history_path[i]); // if (i < 50) { @@ -371,6 +424,7 @@ bool ReusePath::TrimHistoryPath(Frame* frame, // history_path[i].y() // << "]. s[" << history_path[i].s() << "]"; // } + // 更新每个点的弧长值 s trimmed_path.back().set_s(updated_s); } diff --git a/modules/planning/tasks/rule_based_stop_decider/rule_based_stop_decider.cc b/modules/planning/tasks/rule_based_stop_decider/rule_based_stop_decider.cc index fca85fe4913..fc2873f9a8d 100644 --- a/modules/planning/tasks/rule_based_stop_decider/rule_based_stop_decider.cc +++ b/modules/planning/tasks/rule_based_stop_decider/rule_based_stop_decider.cc @@ -50,17 +50,28 @@ bool RuleBasedStopDecider::Init( return Decider::LoadConfig(&config_); } +/// @brief 根据一系列规则（rule-based）来决定是否在特定位置设置停止点（Stop），以保证车辆行驶的安全性与合规性 +/// @param frame 当前规划帧（包含所有规划过程信息，如决策、障碍物、参考线等） +/// @param reference_line_info 当前参考线上的信息（包含路径、车辆位置、参考点等） +/// @return apollo::common::Status RuleBasedStopDecider::Process( Frame *const frame, ReferenceLineInfo *const reference_line_info) { // 1. Rule_based stop for side pass onto reverse lane + // 靠边通行（side-pass）时在逆向车道设置停止点 + // 如果车辆需要靠边绕行，而且需要临时进入逆向车道，该逻辑判断是否需要在进入之前设置一个停止点 + // 目的：防止突然并入逆行车道造成冲突，特别是在能见度或可行驶空间受限时 StopOnSidePass(frame, reference_line_info); // 2. Rule_based stop for urgent lane change + // 检查紧急变道的情况 + // 目的：判断是否有因为路线限制或障碍物导致的“必须立刻变道”的情况 if (config_.enable_lane_change_urgency_checking()) { CheckLaneChangeUrgency(frame); } // 3. Rule_based stop at path end position + // 在路径终点处添加停止点 + // 检查当前参考路径的终点，如果路径长度不足或前方没有可行驶空间，则设置停止点 AddPathEndStop(frame, reference_line_info); return Status::OK(); @@ -115,14 +126,19 @@ void RuleBasedStopDecider::CheckLaneChangeUrgency(Frame *const frame) { } } +/// @brief 在路径终点处添加一个停止点（stop decision），用于防止车辆在路径较短或规划不足时继续前进，确保安全 +/// @param frame 当前 Frame（整个规划周期的数据集合，包括决策、参考线等） +/// @param reference_line_info 当前的参考线信息 void RuleBasedStopDecider::AddPathEndStop( Frame *const frame, ReferenceLineInfo *const reference_line_info) { if (!reference_line_info->path_data().path_label().empty() && reference_line_info->path_data().frenet_frame_path().back().s() - reference_line_info->path_data().frenet_frame_path().front().s() < config_.short_path_length_threshold()) { + // 构造一个虚拟障碍物（stop wall）的 ID，例如 "STOP_PATH_END_" const std::string stop_wall_id = PATH_END_VO_ID_PREFIX + reference_line_info->path_data().path_label(); + // 此处设置停止点 不是因为具体障碍物，而是因为路径终点规则 std::vector wait_for_obstacles; util::BuildStopDecision( stop_wall_id, @@ -132,6 +148,37 @@ void RuleBasedStopDecider::AddPathEndStop( } } +// 参考线 +// void RuleBasedStopDecider::AddReferenceEndStop(Frame *const frame, ReferenceLineInfo *const reference_line_info) { +// CHECK_NOTNULL(reference_line_info); +// const double ref_end_s = reference_line_info->reference_line().reference_points().back().s() - +// reference_line_info->reference_line().reference_points().front().s(); +// const double vehicle_end_s = reference_line_info->AdcSlBoundary().end_s(); +// const double distance_to_ref_end = ref_end_s - vehicle_end_s; +// common::math::Vec2d xy_point(frame->goal().x(), frame->goal().y()); + +// if (!reference_line_info->reference_line().IsOnLane(xy_point)) { +// return; +// } + +// common::SLPoint goal_sl; +// reference_line_info->reference_line().XYToSL(frame->goal().heading(), xy_point, &goal_sl); + +// if (goal_sl.s() < reference_line_info->AdcSlBoundary().start_s()) { +// return; +// } + +// const std::string stop_wall_id = "REFERENCE_LINE_END"; +// std::vector wait_for_obstacles; +// const double end_s = std::min(ref_end_s - kReferenceEndStopBuffer, goal_sl.s() - kGoalEndStopBuffer); +// double ref_stop_s = +// apollo::common::math::Clamp(end_s, reference_line_info->reference_line().reference_points().front().s(), +// reference_line_info->reference_line().reference_points().back().s()); + +// util::BuildStopDecision(stop_wall_id, ref_stop_s, 0.0, StopReasonCode::STOP_REASON_REFERENCE_END, +// wait_for_obstacles, "RuleBasedStopDecider", frame, reference_line_info); +// } + void RuleBasedStopDecider::StopOnSidePass( Frame *const frame, ReferenceLineInfo *const reference_line_info) { static bool check_clear; diff --git a/modules/planning/tasks/speed_bounds_decider/speed_bounds_decider.cc b/modules/planning/tasks/speed_bounds_decider/speed_bounds_decider.cc index 55629ddf58a..c12409e70eb 100644 --- a/modules/planning/tasks/speed_bounds_decider/speed_bounds_decider.cc +++ b/modules/planning/tasks/speed_bounds_decider/speed_bounds_decider.cc @@ -50,55 +50,82 @@ bool SpeedBoundsDecider::Init( return Decider::LoadConfig(&config_); } +/// @brief 在路径上生成 ST 图（空间-时间图）边界和速度限制信息，为后续速度优化器提供输入 +/// @param frame 当前规划周期的帧数据，包含起点、历史轨迹、决策等全局信息 +/// @param reference_line_info 当前参考线的规划信息（如路径、障碍物决策、参考线等） +/// @return Status SpeedBoundsDecider::Process( Frame *const frame, ReferenceLineInfo *const reference_line_info) { // retrieve data from frame and reference_line_info - const PathData &path_data = reference_line_info->path_data(); - const TrajectoryPoint &init_point = frame->PlanningStartPoint(); - const ReferenceLine &reference_line = reference_line_info->reference_line(); - PathDecision *const path_decision = reference_line_info->path_decision(); + // path_data：表示当前参考线上的路径信息 + const PathData &path_data = reference_line_info->path_data(); // 规划的路径点序列 + // init_point：车辆当前的起始状态 + const TrajectoryPoint &init_point = frame->PlanningStartPoint(); // 规划的起点状态（位置、速度、加速度等） + // reference_line：参考线本身，包含 lane 信息 + const ReferenceLine &reference_line = reference_line_info->reference_line(); // 车道中心线或全局路径 + // path_decision：路径上的障碍物决策结果对象，用于访问和修改障碍物的属性 + PathDecision *const path_decision = reference_line_info->path_decision(); // 存储路径上的障碍物决策结果（如绕行、停车等） // 1. Map obstacles into st graph + // 步骤1：映射障碍物到 ST 图 + // 记录开始时间，创建 STBoundaryMapper 对象用于将障碍物映射到ST图（即时空图） auto time1 = std::chrono::system_clock::now(); + // config_：配置参数（如总时间 total_time） + // path_data：路径数据，用于计算障碍物在路径上的投影 + // path_data.discretized_path().Length()：路径总长度（s轴范围） + // total_time：规划时间范围（t轴范围） STBoundaryMapper boundary_mapper(config_, reference_line, path_data, path_data.discretized_path().Length(), config_.total_time(), injector_); - - if (!FLAGS_use_st_drivable_boundary) { + // 根据标志位 FLAGS_use_st_drivable_boundary（默认为 false），清除之前的 ST 边界数据，避免历史数据干扰 + if (!FLAGS_use_st_drivable_boundary) { // 默认为false path_decision->EraseStBoundaries(); } - + // 将所有障碍物投影到ST Graph上 + // ST 图记录了障碍物在 时间（t） 和 路径纵向位置（s） 上的占据区域 if (boundary_mapper.ComputeSTBoundary(path_decision).code() == ErrorCode::PLANNING_ERROR) { const std::string msg = "Mapping obstacle failed."; AERROR << msg; + // 若映射失败，返回错误状态 PLANNING_ERROR return Status(ErrorCode::PLANNING_ERROR, msg); } + + // 记录结束时间，并计算ST边界计算的时间（单位为毫秒），输出调试信息 auto time2 = std::chrono::system_clock::now(); std::chrono::duration diff = time2 - time1; ADEBUG << "Time for ST Boundary Mapping = " << diff.count() * 1000 << " msec."; - + // 所有的障碍物的st_boundary送入到一个boundaries vector之中进行保存 std::vector boundaries; + // 遍历路径决策中的所有障碍物，获取每个障碍物的ST边界 + // 遍历所有障碍物，筛选有效的 ST 边界，并根据边界类型标记障碍物是否阻塞路径 for (auto *obstacle : path_decision->obstacles().Items()) { const auto &id = obstacle->Id(); const auto &st_boundary = obstacle->path_st_boundary(); + // 如果ST边界不为空，判断该边界的类型 if (!st_boundary.IsEmpty()) { + // 如果边界类型是 KEEP_CLEAR，表示该障碍物不会阻塞路径，设置为非阻塞 if (st_boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) { path_decision->Find(id)->SetBlockingObstacle(false); } else { path_decision->Find(id)->SetBlockingObstacle(true); } st_boundary.PrintDebug("_obs_st_bounds"); + // 所有非空 ST 边界被存入 boundaries 容器供后续使用 boundaries.push_back(&st_boundary); } } + + // 获取速度回退的最小 s 位置（可能是路径上的某个点，用于设置速度限制的回退距离） + // 计算 ST 边界中所有障碍物的最小 s 值，作为速度规划的安全回退距离 + // 确保车辆在遇到障碍物时能提前减 const double min_s_on_st_boundaries = SetSpeedFallbackDistance(path_decision); // 2. Create speed limit along path + // 通过 SpeedLimitDecider 生成路径上的速度限制（如弯道限速、障碍物限速） SpeedLimitDecider speed_limit_decider(config_, reference_line, path_data); - SpeedLimit speed_limit; if (!speed_limit_decider .GetSpeedLimits(path_decision->obstacles(), &speed_limit) @@ -109,43 +136,59 @@ Status SpeedBoundsDecider::Process( } // 3. Get path_length as s axis search bound in st graph + // 获取路径的长度（s 轴上的范围） const double path_data_length = path_data.discretized_path().Length(); // 4. Get time duration as t axis search bound in st graph + // 获取配置中的总时间，用作ST图的 t 轴搜索范围 const double total_time_by_conf = config_.total_time(); // Load generated st graph data back to frame + // 将生成的ST图数据加载到 frame 中 + // 获取当前参考线下的 ST 图数据结构，用于向优化器传递 ST 边界与速度限制信息 StGraphData *st_graph_data = reference_line_info_->mutable_st_graph_data(); // Add a st_graph debug info and save the pointer to st_graph_data for // optimizer logging + // 创建调试信息并保存 st_graph_data 的指针，以便日志记录器使用 + // 获取调试信息对象，并为本次规划新建一个 STGraphDebug 实例 auto *debug = reference_line_info_->mutable_debug(); STGraphDebug *st_graph_debug = debug->mutable_planning_data()->add_st_graph(); +// 将计算的ST图数据加载到 st_graph_data 中，包含边界、最小 s 值、初始点、速度限制、巡航速度等信息 +// 将 ST 边界、速度限制、巡航速度等数据加载到 st_graph_data 对象中 +// boundaries：障碍物的 ST 边界集合 +// min_s_on_st_boundaries：安全回退距离 +// speed_limit：路径速度限制 +// cruise_speed：巡航速度（通常来自配置或路网限速） st_graph_data->LoadData(boundaries, min_s_on_st_boundaries, init_point, speed_limit, reference_line_info->GetCruiseSpeed(), path_data_length, total_time_by_conf, st_graph_debug); // Create and record st_graph debug info + // 将 ST 图数据记录到调试信息中，便于可视化或日志分 RecordSTGraphDebug(*st_graph_data, st_graph_debug); return Status::OK(); } - +/// @brief 根据路径决策中存在的障碍物的 st_boundary 来设置速度回退的距离（min_s_on_st_boundaries） +/// @param path_decision 一个指向 PathDecision 类型的常量指针 path_decision，该参数包含了当前路径决策的信息 +/// @return double SpeedBoundsDecider::SetSpeedFallbackDistance( PathDecision *const path_decision) { // Set min_s_on_st_boundaries to guide speed fallback. static constexpr double kEpsilon = 1.0e-6; double min_s_non_reverse = std::numeric_limits::infinity(); double min_s_reverse = std::numeric_limits::infinity(); - +// 循环遍历 path_decision 中的每一个障碍物。path_decision->obstacles() 返回一个障碍物列表，Items() 是获取障碍物项的方法，obstacle 是指向每个障碍物的指针 for (auto *obstacle : path_decision->obstacles().Items()) { + // 获取当前障碍物的 st_boundary，即障碍物的 s-t 边界 const auto &st_boundary = obstacle->path_st_boundary(); - + // 如果当前障碍物的 st_boundary 是空的（即没有边界数据），则跳过该障碍物，继续下一个循环 if (st_boundary.IsEmpty()) { continue; } - + // 分别获取当前障碍物 st_boundary 左下角和右下角的 s 坐标，并计算它们中较小的值 const auto left_bottom_point_s = st_boundary.bottom_left_point().s(); const auto right_bottom_point_s = st_boundary.bottom_right_point().s(); const auto lowest_s = std::min(left_bottom_point_s, right_bottom_point_s); @@ -158,7 +201,7 @@ double SpeedBoundsDecider::SetSpeedFallbackDistance( min_s_non_reverse = lowest_s; } } - + // 确保 min_s_reverse 和 min_s_non_reverse 不会小于零。即，最小的 s 值不能为负数 min_s_reverse = std::max(min_s_reverse, 0.0); min_s_non_reverse = std::max(min_s_non_reverse, 0.0); diff --git a/modules/planning/tasks/speed_bounds_decider/speed_limit_decider.cc b/modules/planning/tasks/speed_bounds_decider/speed_limit_decider.cc index f4e4ae9f2c5..737b04b82a9 100644 --- a/modules/planning/tasks/speed_bounds_decider/speed_limit_decider.cc +++ b/modules/planning/tasks/speed_bounds_decider/speed_limit_decider.cc @@ -41,13 +41,17 @@ SpeedLimitDecider::SpeedLimitDecider(const SpeedBoundsDeciderConfig& config, path_data_(path_data), vehicle_param_(common::VehicleConfigHelper::GetConfig().vehicle_param()) { } - +/// @brief +/// @param obstacles +/// @param speed_limit_data 存储每个路径点的计算出的速度限制（类型为 SpeedLimit） +/// @return Status SpeedLimitDecider::GetSpeedLimits( const IndexedList& obstacles, SpeedLimit* const speed_limit_data) const { CHECK_NOTNULL(speed_limit_data); - + // 路径已经离散化为多个小段，每个段表示一个路径点 const auto& discretized_path = path_data_.discretized_path(); + // 路径在 Frenet 坐标系下的表示形式，通常用于规划和跟踪 const auto& frenet_path = path_data_.frenet_frame_path(); PrintCurves print_curve; for (uint32_t i = 0; i < discretized_path.size(); ++i) { @@ -61,14 +65,16 @@ Status SpeedLimitDecider::GetSpeedLimits( } // (1) speed limit from map + // 根据参考线的当前位置获取速度限制，通常参考线会记录道路的限速信息 double speed_limit_from_reference_line = reference_line_.GetSpeedLimitFromS(reference_line_s); print_curve.AddPoint("speed_limit_from_ref", path_s, speed_limit_from_reference_line); // (2) speed limit from path curvature // -- 2.1: limit by centripetal force (acceleration) + // 根据路径曲率（kappa，即路径的弯曲程度）计算速度限制，主要考虑离心加速度对速度的影响。较大的曲率会导致较小的安全速度限制 const double speed_limit_from_centripetal_acc = - std::sqrt(speed_bounds_config_.max_centric_acceleration_limit() / + std::sqrt(speed_bounds_config_.max_centric_acceleration_limit() / // 0.8 std::fmax(std::fabs(discretized_path.at(i).kappa()), speed_bounds_config_.minimal_kappa())); print_curve.AddPoint("speed_limit_from_centripetal_acc", path_s, @@ -97,6 +103,7 @@ Status SpeedLimitDecider::GetSpeedLimits( // TODO(all): potential problem here; // frenet and cartesian coordinates are mixed. + // // 判断障碍物和车辆的前后位置关系 const double vehicle_front_s = reference_line_s + vehicle_param_.front_edge_to_center(); const double vehicle_back_s = @@ -110,7 +117,7 @@ Status SpeedLimitDecider::GetSpeedLimits( vehicle_back_s > obstacle_front_s) { continue; } - + // // 判断障碍物是否在车辆左右侧，并且是否在安全范围内 const auto& nudge_decision = ptr_obstacle->LateralDecision().nudge(); // Please notice the differences between adc_l and frenet_point_l @@ -155,7 +162,7 @@ Status SpeedLimitDecider::GetSpeedLimits( speed_limit_from_nearby_obstacles})); } else { curr_speed_limit = - std::fmax(speed_bounds_config_.lowest_speed(), + std::fmax(speed_bounds_config_.lowest_speed(), // 0.1 std::min({speed_limit_from_reference_line, speed_limit_from_centripetal_acc})); } diff --git a/modules/planning/tasks/speed_bounds_decider/st_boundary_mapper.cc b/modules/planning/tasks/speed_bounds_decider/st_boundary_mapper.cc index 48c325df9a3..6ecfa9266b4 100644 --- a/modules/planning/tasks/speed_bounds_decider/st_boundary_mapper.cc +++ b/modules/planning/tasks/speed_bounds_decider/st_boundary_mapper.cc @@ -61,10 +61,17 @@ STBoundaryMapper::STBoundaryMapper( planning_max_distance_(planning_distance), planning_max_time_(planning_time), injector_(injector) {} - +/// @brief +/// @param path_decision 路径上的障碍物决策信息，包含每个障碍物的状态、决策结果等。 +/// @return Status STBoundaryMapper::ComputeSTBoundary(PathDecision* path_decision) const { // Sanity checks. + // 1.参数有效性检查 + // 确保 planning_max_time_（规划最大时间）大于 0，确保时间配置有效 + //断言检查规划时间 planning_max_time_ 必须大于 0，确保时间参数合法 CHECK_GT(planning_max_time_, 0.0); + // 检查路径点的数量是否少于 2。如果路径点数量不足，输出错误信息并返回 PLANNING_ERROR 状态 + // 检查离散化路径点数量是否足够生成 ST 图。若路径点少于 2 个，直接报错返回 if (path_data_.discretized_path().size() < 2) { AERROR << "Fail to get params because of too few path points. path points " "size: " @@ -73,45 +80,73 @@ Status STBoundaryMapper::ComputeSTBoundary(PathDecision* path_decision) const { "Fail to get params because of too few path points"); } + // 2.遍历路径决策中的每个障碍物，计算其 ST 边界 // Go through every obstacle. - Obstacle* stop_obstacle = nullptr; - ObjectDecisionType stop_decision; - double min_stop_s = std::numeric_limits::max(); + // 记录最近的停车障碍物 + // stop_obstacle：记录距离最近的停车障碍物 + Obstacle* stop_obstacle = nullptr; // 后面需要更新的 + // 记录停车决策 + ObjectDecisionType stop_decision; // 后面需要更新的 + // 用于存储最小停车点的 s 值，初始化为最大值 + // min_stop_s：记录所有停车决策中最小的停车距离（s 坐标） + double min_stop_s = std::numeric_limits::max(); // 后面需要更新的 + // 遍历路径决策中的每个障碍物（path_decision->obstacles().Items() 返回所有障碍物），并通过 Find 方法获取障碍物对象 ptr_obstacle + // 循环逻辑：遍历路径决策中的所有障碍物，并通过 Find 方法获取完整障碍物对象 for (const auto* ptr_obstacle_item : path_decision->obstacles().Items()) { Obstacle* ptr_obstacle = path_decision->Find(ptr_obstacle_item->Id()); ACHECK(ptr_obstacle != nullptr); + // 无纵向决策的障碍物处理 // If no longitudinal decision has been made, then plot it onto ST-graph. + // 如果该障碍物没有纵向决策（即没有决定是否停车、是否超车等），则将其时空边界绘制到 ST 图中，调用 ComputeSTBoundary 递归处理该障碍物，然后继续处理下一个障碍物 +// 若障碍物 无纵向决策（如未标记停车/跟随），调用 ComputeSTBoundary 生成基础 ST 边界（根据障碍物运动预测） if (!ptr_obstacle->HasLongitudinalDecision()) { ComputeSTBoundary(ptr_obstacle); continue; } +// 处理停车决策 // If there is a longitudinal decision, then fine-tune boundary. + // 如果障碍物有纵向决策，则获取其决策，存储在 decision 中。该决策可能包括停车、跟随、超车、让行等 const auto& decision = ptr_obstacle->LongitudinalDecision(); + // 如果决策包含停车 (has_stop()) if (decision.has_stop()) { // 1. Store the closest stop fence info. // TODO(all): store ref. s value in stop decision; refine the code then. + // 将停车点信息存储起来。通过 XYToSL 方法将停车点从 XY 坐标转换为参考线上的 s 和 l 坐标，存储在 stop_sl_point 中 common::SLPoint stop_sl_point; + // 坐标转换：将停车点从笛卡尔坐标系（XY）转换为参考线坐标系（SL） reference_line_.XYToSL(decision.stop().stop_point(), &stop_sl_point); + // 获取停车点的 s 值，存储为 stop_s const double stop_s = stop_sl_point.s(); - + // 如果当前障碍物的停车点 stop_s 小于记录的最小停车点 min_stop_s + // 记录最小停车距离：确保车辆在最近的停车点前停车，避免碰撞 if (stop_s < min_stop_s) { - stop_obstacle = ptr_obstacle; - min_stop_s = stop_s; - stop_decision = decision; + stop_obstacle = ptr_obstacle; // 更新最近停车障碍物 + min_stop_s = stop_s; // 更新最小停车距离 + stop_decision = decision; // 保存停车决策 } + // 根据决策类型（跟随、超车、让行），调用 ComputeSTBoundaryWithDecision 细化 ST 边界 } else if (decision.has_follow() || decision.has_overtake() || decision.has_yield()) { // 2. Depending on the longitudinal overtake/yield decision, // fine-tune the upper/lower st-boundary of related obstacles. + // 如果决策包含跟随、超车或让行（has_follow()、has_overtake()、has_yield()），则根据这些决策调整时空边界。 ComputeSTBoundaryWithDecision(ptr_obstacle, decision); - } else if (!decision.has_ignore()) { + } + // 对非忽略（ignore）且未处理的决策类型发出警告，提醒开发者补充逻辑 + else if (!decision.has_ignore()) { // 3. Ignore those unrelated obstacles. + // 如果决策是 ignore（忽略）以外的其他类型，则记录警告，说明当前决策没有被处理或没有映射 AWARN << "No mapping for decision: " << decision.DebugString(); } } + + // 5. 如果有停车障碍物，映射停车决策到 ST 图 + // 如果找到停车决策（stop_obstacle 非空），则调用 MapStopDecision 函数来处理停车决策并更新相应的时空边界 if (stop_obstacle) { + // MapStopDecision 将停车点转换为 ST 图中的垂直线（时间轴上的固定 s 值） + // 在 ST 图中标记停车位置，规划模块会生成减速至停车的轨迹 bool success = MapStopDecision(stop_obstacle, stop_decision); if (!success) { const std::string msg = "Fail to MapStopDecision."; @@ -123,79 +158,126 @@ Status STBoundaryMapper::ComputeSTBoundary(PathDecision* path_decision) const { return Status::OK(); } +/// @brief // 将障碍物的停止决策映射到ST图边界 +/// @param stop_obstacle // 需要处理的障碍物指针 +/// @param stop_decision // 包含停止决策的对象 +/// @return bool STBoundaryMapper::MapStopDecision( Obstacle* stop_obstacle, const ObjectDecisionType& stop_decision) const { + // 检查决策必须包含停止信息，否则触发断言 DCHECK(stop_decision.has_stop()) << "Must have stop decision"; + // 创建SL坐标点存储停止点 common::SLPoint stop_sl_point; + // 将笛卡尔坐标的停止点转换为参考线上的SL坐标 reference_line_.XYToSL(stop_decision.stop().stop_point(), &stop_sl_point); +// 初始化ST图中的停止点s值 double st_stop_s = 0.0; + // 计算调整后的参考s值：减去车辆前边缘到中心的距离（将停止点转换到车辆中心参考系） const double stop_ref_s = stop_sl_point.s() - vehicle_param_.front_edge_to_center(); +// 判断停止点是否在路径末端之后 if (stop_ref_s > path_data_.frenet_frame_path().back().s()) { + // 计算延伸后的s值：离散路径末端s + 超出部分的s差值 st_stop_s = path_data_.discretized_path().back().s() + (stop_ref_s - path_data_.frenet_frame_path().back().s()); } else { + // 尝试获取路径上对应参考s值的路径点 PathPoint stop_point; if (!path_data_.GetPathPointWithRefS(stop_ref_s, &stop_point)) { - return false; + return false; // 获取失败则返回错误 } - st_stop_s = stop_point.s(); + st_stop_s = stop_point.s(); // 使用路径点的s值 } +// 计算s轴最小边界（不小于0） const double s_min = std::fmax(0.0, st_stop_s); + // 计算s轴最大边界（取规划最大距离/参考线长度的较大值） const double s_max = std::fmax( s_min, std::fmax(planning_max_distance_, reference_line_.Length())); +// 创建ST边界点对集合 std::vector> point_pairs; + // 添加下边界：时间0时刻从s_min到s_max的水平线 point_pairs.emplace_back(STPoint(s_min, 0.0), STPoint(s_max, 0.0)); + // 添加上边界：最大规划时间时刻的斜线（s_max增加缓冲值） point_pairs.emplace_back( STPoint(s_min, planning_max_time_), STPoint(s_max + speed_bounds_config_.boundary_buffer(), planning_max_time_)); + // 创建ST边界对象 auto boundary = STBoundary(point_pairs); + // 设置边界类型为停止 boundary.SetBoundaryType(STBoundary::BoundaryType::STOP); + // 设置特征长度（用于边界处理） boundary.SetCharacteristicLength(speed_bounds_config_.boundary_buffer()); + // 设置边界ID与障碍物ID一致 boundary.set_id(stop_obstacle->Id()); + // 将边界对象关联到障碍物 stop_obstacle->set_path_st_boundary(boundary); return true; } - +/// @brief 计算并更新与某个障碍物（obstacle）相关的时间-空间（ST）边界 +/// @param obstacle void STBoundaryMapper::ComputeSTBoundary(Obstacle* obstacle) const { - if (FLAGS_use_st_drivable_boundary) { +// 若启用 use_st_drivable_boundary 标志（通常表示使用预定义的可行区域边界），则跳过自定义 ST 边界计算 + if (FLAGS_use_st_drivable_boundary) { // 默认为false return; } - std::vector lower_points; - std::vector upper_points; + // 计算重叠边界点 + // 存储计算出的时间-空间边界的下边界点 + std::vector lower_points; // 时空下边界点（s 较小侧） + // 存储计算出的时间-空间边界的上边界点 + std::vector upper_points; // 时空上边界点（s 较大侧） + // 计算出重叠的边界点 + // discretized_path：离散化的车辆规划路径 + // obstacle：当前处理的障碍物对象 + // 功能： + // 预测障碍物在路径上的运动轨迹，计算其占据的时空区域 + // 通过几何投影或运动模型，生成边界点序列（例如：障碍物在不同时间 t 占据的最小和最大纵向位置 s） if (!GetOverlapBoundaryPoints(path_data_.discretized_path(), *obstacle, &upper_points, &lower_points)) { return; } - + // 使用之前计算得到的上下边界点创建一个 STBoundary 对象 boundary + // 根据上下边界点创建 STBoundary 对象，表示障碍物的时空占据区域 + // CreateInstance：工厂方法，根据点集构造边界（可能进行插值或凸包计算） auto boundary = STBoundary::CreateInstance(lower_points, upper_points); + // 将该障碍物的 ID 设置给 boundary 对象，确保边界与特定的障碍物关联 + // set_id：绑定边界与障碍物的唯一标识，便于后续跟踪 boundary.set_id(obstacle->Id()); // TODO(all): potential bug here. + // 若两种边界同时存在，可能覆盖优先级逻辑，需确认设计是否符合预期 + // 未处理两者均为空的情况，可能导致边界类型未初始化（需结合上下文确认是否合理） const auto& prev_st_boundary = obstacle->path_st_boundary(); const auto& ref_line_st_boundary = obstacle->reference_line_st_boundary(); + // 优先继承路径 ST 边界类型，次优先继承参考线 ST 边界类型 if (!prev_st_boundary.IsEmpty()) { boundary.SetBoundaryType(prev_st_boundary.boundary_type()); } else if (!ref_line_st_boundary.IsEmpty()) { boundary.SetBoundaryType(ref_line_st_boundary.boundary_type()); } - + // 将计算得到的 boundary 设置为障碍物的路径 ST 边界，更新障碍物的状态 obstacle->set_path_st_boundary(boundary); } - +/// @brief 根据路径点和障碍物的轨迹计算可能的重叠区域，并返回这些区域的上下边界点 +/// @param path_points 离散化的路径点序列，表示自车规划路径 +/// @param obstacle 障碍物对象，包含位置、尺寸、预测轨迹等信息 +/// @param upper_points 保存重叠区域的上边界 障碍物在 ST 图上的上边界点（s 值较大的一侧） +/// @param lower_points 保存重叠区域的下边界 障碍物在 ST 图上的下边界点（s 值较小的一侧） +/// @return 返回一个 bool 类型，表示是否找到了有效的重叠边界点 bool STBoundaryMapper::GetOverlapBoundaryPoints( const std::vector& path_points, const Obstacle& obstacle, std::vector* upper_points, std::vector* lower_points) const { // Sanity checks. + // 检查输出容器是否为空 DCHECK(upper_points->empty()); DCHECK(lower_points->empty()); + // 检查路径点是否为空 if (path_points.empty()) { AERROR << "No points in path_data_.discretized_path()."; return false; @@ -204,43 +286,54 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints( const auto* planning_status = injector_->planning_context() ->mutable_planning_status() ->mutable_change_lane(); - + // 根据是否处于换道，调整l_buffer + // 若处于换道状态 (IN_CHANGE_LANE)，使用较小的缓冲距离 0.3，因换道需要更紧凑的避让 + // 否则使用默认值 0.4，提供更大的安全裕度 double l_buffer = planning_status->status() == ChangeLaneStatus::IN_CHANGE_LANE - ? speed_bounds_config_.lane_change_obstacle_nudge_l_buffer() - : FLAGS_nonstatic_obstacle_nudge_l_buffer; + ? speed_bounds_config_.lane_change_obstacle_nudge_l_buffer() // 0.3 + : FLAGS_nonstatic_obstacle_nudge_l_buffer; // 0.4 // Draw the given obstacle on the ST-graph. + // 获取障碍物的轨迹 const auto& trajectory = obstacle.Trajectory(); + // 获取障碍物的长度 const double obstacle_length = obstacle.Perception().length(); + // 获取障碍物的宽度 const double obstacle_width = obstacle.Perception().width(); if (trajectory.trajectory_point().empty()) { + // 处理静态障碍物 + // 判断是否有重叠 bool box_check_collision = false; // For those with no predicted trajectories, just map the obstacle's // current position to ST-graph and always assume it's static. + // 如果障碍物不是静态的，但没有预测轨迹，打印警告信息 if (!obstacle.IsStatic()) { AWARN << "Non-static obstacle[" << obstacle.Id() << "] has NO prediction trajectory." << obstacle.Perception().ShortDebugString(); } - - const Box2d& obs_box = obstacle.PerceptionBoundingBox(); - - for (const auto& curr_point_on_path : path_points) { + // 获取障碍物的包围盒（Box2d），用于碰撞检测 + const Box2d& obs_box = obstacle.PerceptionBoundingBox(); + // 遍历路径上的每个点，检查每个路径点是否与障碍物的包围盒发生重叠 + // 遍历路径点检查碰撞 + for (const auto& curr_point_on_path : path_points) { + // 遍历离散路径点，若当前点超过了规划最大距离，退出 if (curr_point_on_path.s() > planning_max_distance_) { break; } + // 如果发生重叠，设置 box_check_collision 为 true 并跳出循环 if (CheckOverlap(curr_point_on_path, obs_box, l_buffer)) { box_check_collision = true; break; } } - +// 若发生碰撞，计算边界点 if (box_check_collision) { const double backward_distance = -vehicle_param_.front_edge_to_center(); const double forward_distance = obs_box.length(); - + // 遍历路径点，检查是否与障碍物的多边形（Polygon2d）发生重叠 for (const auto& curr_point_on_path : path_points) { if (curr_point_on_path.s() > planning_max_distance_) { break; @@ -248,6 +341,9 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints( const Polygon2d& obs_polygon = obstacle.PerceptionPolygon(); if (CheckOverlap(curr_point_on_path, obs_polygon, l_buffer)) { // If there is overlapping, then plot it on ST-graph. + // 计算重叠区域的 s 值（路径上的位置）。low_s 为重叠区的开始位置，high_s 为重叠区的结束位置 + //low_s：路径点 s 值 + 车头到中心的负向距离（覆盖车体后方） + //high_s：路径点 s 值 + 障碍物长度（覆盖车体前方） double low_s = std::fmax(0.0, curr_point_on_path.s() + backward_distance); double high_s = std::fmin(planning_max_distance_, @@ -255,6 +351,8 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints( // It is an unrotated rectangle appearing on the ST-graph. // TODO(jiacheng): reconsider the backward_distance, it might be // unnecessary, but forward_distance is indeed meaningful though. + // 将重叠区域的上下边界点添加到 upper_points 和 lower_points 中，并退出循环 + // // 添加边界点 lower_points->emplace_back(low_s, 0.0); lower_points->emplace_back(low_s, planning_max_time_); upper_points->emplace_back(high_s, 0.0); @@ -264,11 +362,15 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints( } } } else { + // 处理动态障碍物 // For those with predicted trajectories (moving obstacles): // 1. Subsample to reduce computation time. + // 如果障碍物有预测轨迹（动态障碍物），则对路径点进行采样，减少计算量 + // 若路径点数目过多，降采样 const int default_num_point = 50; DiscretizedPath discretized_path; if (path_points.size() > 2 * default_num_point) { + // 按比例降采样 const auto ratio = path_points.size() / default_num_point; std::vector sampled_path_points; for (size_t i = 0; i < path_points.size(); ++i) { @@ -282,38 +384,55 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints( } // 2. Go through every point of the predicted obstacle trajectory. + // 根据障碍物的速度和路径点间隔，计算用于轨迹点的步长（trajectory_step），即每次检查的轨迹点数量 + // 计算轨迹点步长 double trajectory_time_interval = obstacle.Trajectory().trajectory_point()[1].relative_time(); +// 根据障碍物速度、车辆宽度和轨迹时间间隔，动态调整步长，确保在高速下不会漏检 +// 步长 = 车辆宽度 / (障碍物速度 * 轨迹时间间隔)，取整且至少为1 int trajectory_step = std::min( FLAGS_trajectory_check_collision_time_step, std::max(vehicle_param_.width() / obstacle.speed() / trajectory_time_interval, 1.0)); + + + // 初始化轨迹点的碰撞状态 + // 遍历轨迹点检测碰撞 bool trajectory_point_collision_status = false; int previous_index = 0; - + // 遍历障碍物的每个轨迹点，并计算其形状（多边形） for (int i = 0; i < trajectory.trajectory_point_size(); i = std::min(i + trajectory_step, trajectory.trajectory_point_size() - 1)) { + // 获取轨迹点对应的障碍物形状 const auto& trajectory_point = trajectory.trajectory_point(i); Polygon2d obstacle_shape = obstacle.GetObstacleTrajectoryPolygon(trajectory_point); - + // 跳过负时间的轨迹点 double trajectory_point_time = trajectory_point.relative_time(); static constexpr double kNegtiveTimeThreshold = -1.0; + // 跳过小于阈值时间的轨迹点 if (trajectory_point_time < kNegtiveTimeThreshold) { continue; } + // 检查路径点是否与当前轨迹点的障碍物形状发生重叠 + // 检测碰撞并更新边界点 bool collision = CheckOverlapWithTrajectoryPoint( discretized_path, obstacle_shape, upper_points, lower_points, l_buffer, default_num_point, obstacle_length, obstacle_width, trajectory_point_time); + // 如果碰撞状态发生变化，则回溯检查之前的轨迹点 + // 碰撞状态变化时回溯检查 if ((trajectory_point_collision_status ^ collision) && i != 0) { + // 向前回溯轨迹点，找到精确碰撞时刻 // Start retracing track points forward int index = i - 1; while ((trajectory_point_collision_status ^ collision) && index > previous_index) { + // 回溯并更新碰撞状态 + // 更新碰撞状态 const auto& point = trajectory.trajectory_point(index); trajectory_point_time = point.relative_time(); obstacle_shape = obstacle.GetObstacleTrajectoryPolygon(point); @@ -327,12 +446,15 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints( } trajectory_point_collision_status = !trajectory_point_collision_status; } + // 如果到达最后一个轨迹点，退出循环 if (i == trajectory.trajectory_point_size() - 1) break; + // 更新状态和索引 previous_index = i; } } // Sanity checks and return. + // 按时间 t 对边界点排序，确保在 ST 图上按时间顺序连接 std::sort(lower_points->begin(), lower_points->end(), [](const STPoint& a, const STPoint& b) { return a.t() < b.t(); @@ -344,7 +466,17 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints( DCHECK_EQ(lower_points->size(), upper_points->size()); return (lower_points->size() > 1 && upper_points->size() > 1); } - +/// @brief 检查给定的轨迹点是否与障碍物发生重叠。如果发生重叠，函数将使用更高的分辨率定位与障碍物重叠的具体区间，并返回重叠区域的上下边界 +/// @param discretized_path 轨迹的离散化路径，包含了路径的多个点，每个点都有位置和时间信息 +/// @param obstacle_shape 障碍物的多边形形状，用于与轨迹点进行碰撞检测 +/// @param upper_points 重叠区域的上边界 +/// @param lower_points 重叠区域的下边界 +/// @param l_buffer 一个缓冲区，通常用于防止因计算误差导致的假重叠 +/// @param default_num_point 默认的路径分割点数，用于更精细的分辨率 +/// @param obstacle_length 障碍物的长度，用于估计重叠区域的大小 +/// @param obstacle_width 障碍物的宽度，用于估计重叠区域的大小 +/// @param trajectory_point_time 轨迹点的时间，表示车辆经过该点时的时间 +/// @return bool STBoundaryMapper::CheckOverlapWithTrajectoryPoint( const DiscretizedPath& discretized_path, const Polygon2d& obstacle_shape, @@ -355,7 +487,9 @@ bool STBoundaryMapper::CheckOverlapWithTrajectoryPoint( const double obstacle_length, const double obstacle_width, const double trajectory_point_time) const { + // 车辆前端到车辆中心的距离，用于控制路径点的间隔。每次检查的间隔为 step_length const double step_length = vehicle_param_.front_edge_to_center(); + // 路径的长度，限制在最大轨迹长度（speed_bounds_config_.max_trajectory_len()）和轨迹实际长度之间的较小值 auto path_len = std::min(speed_bounds_config_.max_trajectory_len(), discretized_path.Length()); // Go through every point of the ADC's path. @@ -364,6 +498,7 @@ bool STBoundaryMapper::CheckOverlapWithTrajectoryPoint( discretized_path.Evaluate(path_s + discretized_path.front().s()); if (CheckOverlap(curr_adc_path_point, obstacle_shape, l_buffer)) { // Found overlap, start searching with higher resolution + // 高分辨率查找重叠区间 const double backward_distance = -step_length; const double forward_distance = vehicle_param_.length() + vehicle_param_.width() + @@ -371,12 +506,12 @@ bool STBoundaryMapper::CheckOverlapWithTrajectoryPoint( const double default_min_step = 0.1; // in meters const double fine_tuning_step_length = std::fmin( default_min_step, discretized_path.Length() / default_num_point); - + // 双向细化搜索 bool find_low = false; bool find_high = false; - double low_s = std::fmax(0.0, path_s + backward_distance); + double low_s = std::fmax(0.0, path_s + backward_distance); // 向后搜索 double high_s = - std::fmin(discretized_path.Length(), path_s + forward_distance); + std::fmin(discretized_path.Length(), path_s + forward_distance); // 向前搜索 // Keep shrinking by the resolution bidirectionally until finally // locating the tight upper and lower bounds. @@ -403,6 +538,7 @@ bool STBoundaryMapper::CheckOverlapWithTrajectoryPoint( } } } + // 保存重叠区间 if (find_high && find_low) { lower_points->emplace_back( low_s - speed_bounds_config_.point_extension(), @@ -416,84 +552,118 @@ bool STBoundaryMapper::CheckOverlapWithTrajectoryPoint( } return false; } - +/// @brief 它用于根据给定的障碍物（obstacle）和决策（decision）来计算ST（时空）边界 +/// @param obstacle 一个指向 Obstacle 对象的指针，表示障碍物 +/// @param decision 一个 ObjectDecisionType 类型的常量引用，表示决策 void STBoundaryMapper::ComputeSTBoundaryWithDecision( Obstacle* obstacle, const ObjectDecisionType& decision) const { +// 使用 DCHECK 检查 decision 必须包含 follow、yield 或 overtake 之一，表示决策的类型 +// 如果决策不符合要求，则打印出调试信息 DCHECK(decision.has_follow() || decision.has_yield() || decision.has_overtake()) << "decision is " << decision.DebugString() << ", but it must be follow or yield or overtake."; +// 定义存储ST边界下界点与上界点的容器 + std::vector lower_points; // 时空边界下界点集合 + std::vector upper_points; // 时空边界上界点集合 - std::vector lower_points; - std::vector upper_points; - +// 检查是否启用动态边界且障碍物已初始化路径ST边界 if (FLAGS_use_st_drivable_boundary && obstacle->is_path_st_boundary_initialized()) { + // 若条件满足，直接获取障碍物现有的路径ST边界数据 const auto& path_st_boundary = obstacle->path_st_boundary(); - lower_points = path_st_boundary.lower_points(); - upper_points = path_st_boundary.upper_points(); + lower_points = path_st_boundary.lower_points(); // 复制下界点 + upper_points = path_st_boundary.upper_points(); // 复制上界点 } else { + // 否则通过计算获取重叠区域的边界点 if (!GetOverlapBoundaryPoints(path_data_.discretized_path(), *obstacle, &upper_points, &lower_points)) { - return; + return; // 若无法获取边界点则提前退出 } } - + // 使用 lower_points 和 upper_points 创建一个 STBoundary 实例，表示时空边界 auto boundary = STBoundary::CreateInstance(lower_points, upper_points); // get characteristic_length and boundary_type. - STBoundary::BoundaryType b_type = STBoundary::BoundaryType::UNKNOWN; - double characteristic_length = 0.0; - if (decision.has_follow()) { - characteristic_length = std::fabs(decision.follow().distance_s()); - AINFO << "characteristic_length: " << characteristic_length; + // 初始化边界类型 b_type 为 UNKNOWN + STBoundary::BoundaryType b_type = STBoundary::BoundaryType::UNKNOWN; // 边界类型 + // 初始化 characteristic_length（特征长度）为 0 + double characteristic_length = 0.0; // 特征长度（安全距离） + + // 根据决策类型处理边界 + if (decision.has_follow()) { // 跟随决策 + characteristic_length = std::fabs(decision.follow().distance_s()); // 获取跟随距离 + AINFO << "characteristic_length: " << characteristic_length; // 记录日志 + // 沿S轴扩展边界（增加安全距离） boundary = STBoundary::CreateInstance(lower_points, upper_points) .ExpandByS(characteristic_length); - b_type = STBoundary::BoundaryType::FOLLOW; - } else if (decision.has_yield()) { - characteristic_length = std::fabs(decision.yield().distance_s()); + b_type = STBoundary::BoundaryType::FOLLOW; // 标记为跟随类型 + } else if (decision.has_yield()) { // 让行决策 + characteristic_length = std::fabs(decision.yield().distance_s()); // 获取让行距离 boundary = STBoundary::CreateInstance(lower_points, upper_points) - .ExpandByS(characteristic_length); - b_type = STBoundary::BoundaryType::YIELD; - } else if (decision.has_overtake()) { - characteristic_length = std::fabs(decision.overtake().distance_s()); - b_type = STBoundary::BoundaryType::OVERTAKE; - } else { + .ExpandByS(characteristic_length); // 扩展边界 + b_type = STBoundary::BoundaryType::YIELD; // 标记为让行类型 + } else if (decision.has_overtake()) { // 超车决策 + characteristic_length = std::fabs(decision.overtake().distance_s()); // 获取超车距离 + b_type = STBoundary::BoundaryType::OVERTAKE; // 标记为超车类型 + } else { // 非法决策类型处理 + // 如果决策既不是 follow、yield 也不是 overtake，则触发断言错误，打印出决策的调试信息 DCHECK(false) << "Obj decision should be either yield or overtake: " << decision.DebugString(); } - boundary.SetBoundaryType(b_type); - boundary.set_id(obstacle->Id()); - boundary.SetCharacteristicLength(characteristic_length); - obstacle->set_path_st_boundary(boundary); -} + // 设置边界的类型为之前计算出的 b_type + // 配置最终边界属性 + boundary.SetBoundaryType(b_type); // 设置边界类型 + // 设置边界的 ID 为障碍物的 ID + boundary.set_id(obstacle->Id()); // 关联障碍物ID + // 设置边界的特征长度 + boundary.SetCharacteristicLength(characteristic_length); // 设置特征长度 + // 将计算得到的时空边界设置回障碍物中 + obstacle->set_path_st_boundary(boundary); // 将计算后的边界保存到障碍物对象 +} +/// @brief 检查两个矩形框（一个表示障碍物，另一个表示自动驾驶车辆的边界框）是否有重叠 +/// @param path_point 当前路径点，包含了车辆的位置 (x, y) 和朝向角度 theta +/// @param obs_box 障碍物的边界框，类型为 Box2d +/// @param l_buffer 一个缓冲区长度，用来扩展车辆的边界框 +/// @return 返回一个布尔值，表示两者是否重叠 bool STBoundaryMapper::CheckOverlap(const PathPoint& path_point, const Box2d& obs_box, const double l_buffer) const { // Convert reference point from center of rear axis to center of ADC. + // 车辆几何中心在车体坐标系下的坐标 Vec2d ego_center_map_frame((vehicle_param_.front_edge_to_center() - vehicle_param_.back_edge_to_center()) * 0.5, (vehicle_param_.left_edge_to_center() - vehicle_param_.right_edge_to_center()) * 0.5); + // 将后轴中心变换到几何中心 ego_center_map_frame.SelfRotate(path_point.theta()); ego_center_map_frame.set_x(ego_center_map_frame.x() + path_point.x()); ego_center_map_frame.set_y(ego_center_map_frame.y() + path_point.y()); // Compute the ADC bounding box. + // 计算车辆的boundingbox Box2d adc_box(ego_center_map_frame, path_point.theta(), vehicle_param_.length(), vehicle_param_.width() + l_buffer * 2); // Check whether ADC bounding box overlaps with obstacle bounding box. + // 碰撞检测 return obs_box.HasOverlap(adc_box); } - +/// @brief 首先通过路径点计算出自车在地图上的位置和朝向，然后根据自车的尺寸（包括缓冲区）计算出自车的包围盒， +//并将其转化为多边形。最后，通过与障碍物多边形的重叠检测来判断是否存在碰撞或障碍 +/// @param path_point 路径点，表示自车当前位置及其朝向（theta） +/// @param obs_polygon 障碍物多边形，表示障碍物的位置和形状 +/// @param l_buffer 一个额外的缓冲区长度，用来扩展自车的宽度 +/// @return bool STBoundaryMapper::CheckOverlap(const PathPoint& path_point, const Polygon2d& obs_polygon, const double l_buffer) const { // Convert reference point from center of rear axis to center of ADC. + // 目的是将参考点从自车后轴中心转换为自车的中心 + // 计算自车中心在车辆坐标系中的位置（ego_center_map_frame） Vec2d ego_center_map_frame((vehicle_param_.front_edge_to_center() - vehicle_param_.back_edge_to_center()) * 0.5, @@ -509,6 +679,8 @@ bool STBoundaryMapper::CheckOverlap(const PathPoint& path_point, vehicle_param_.length(), vehicle_param_.width() + l_buffer * 2); // Check whether ADC polygon overlaps with obstacle polygon. + // 检查自车的多边形是否与障碍物的多边形重叠 + // 将自车的包围盒（adc_box）转化为一个多边形（adc_polygon），该多边形表示自车的形状 Polygon2d adc_polygon(adc_box); return obs_polygon.HasOverlap(adc_polygon); } diff --git a/modules/planning/tasks/speed_decider/speed_decider.cc b/modules/planning/tasks/speed_decider/speed_decider.cc index 1d167955efb..743a8c35089 100644 --- a/modules/planning/tasks/speed_decider/speed_decider.cc +++ b/modules/planning/tasks/speed_decider/speed_decider.cc @@ -46,13 +46,16 @@ using apollo::perception::PerceptionObstacle; bool SpeedDecider::Init(const std::string& config_dir, const std::string& name, const std::shared_ptr& injector) { + // 1.调用父类 Task 的初始化 if (!Task::Init(config_dir, name, injector)) { return false; } // Load the config this task. + // 2.加载 SpeedDecider 自己的配置 if (!Task::LoadConfig(&config_)) { return false; } + // 3.读取 follow_distance 函数配置 for (const auto& follow_function : config_.follow_distance_scheduler().follow_distance()) { follow_distance_function_.emplace_back( @@ -67,10 +70,13 @@ bool SpeedDecider::Init(const std::string& config_dir, const std::string& name, common::Status SpeedDecider::Execute(Frame* frame, ReferenceLineInfo* reference_line_info) { + // 1.调用父类 Task 的 Execute Task::Execute(frame, reference_line_info); + // 2.初始化起始点和边界 init_point_ = frame_->PlanningStartPoint(); adc_sl_boundary_ = reference_line_info_->AdcSlBoundary(); reference_line_ = &reference_line_info_->reference_line(); + // 3.获取物体决策并判断是否成功 if (!MakeObjectDecision(reference_line_info->speed_data(), reference_line_info->path_decision()) .ok()) { @@ -218,36 +224,42 @@ bool SpeedDecider::IsFollowTooClose(const Obstacle& obstacle) const { Status SpeedDecider::MakeObjectDecision( const SpeedData& speed_profile, PathDecision* const path_decision) const { + // 1.检查速度配置有效性 if (speed_profile.size() < 2) { const std::string msg = "dp_st_graph failed to get speed profile."; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } - + // 2.遍历路径决策中的所有障碍物 for (const auto* obstacle : path_decision->obstacles().Items()) { auto* mutable_obstacle = path_decision->Find(obstacle->Id()); const auto& boundary = mutable_obstacle->path_st_boundary(); - + // 忽略决策障碍物 + // 3.判断障碍物是否需要忽略 if (boundary.IsEmpty() || boundary.max_s() < 0.0 || boundary.max_t() < 0.0 || boundary.min_t() >= speed_profile.back().t()) { AppendIgnoreDecision(mutable_obstacle); continue; } + // 4.处理已经有纵向决策的障碍物 if (obstacle->HasLongitudinalDecision()) { AppendIgnoreDecision(mutable_obstacle); continue; } // for Virtual obstacle, skip if center point NOT "on lane" + // 5.处理虚拟障碍物 if (obstacle->IsVirtual()) { const auto& obstacle_box = obstacle->PerceptionBoundingBox(); + // Vec2d 转换成 sl, 判断sl点是否在lane上 if (!reference_line_->IsOnLane(obstacle_box.center())) { continue; } } // always STOP for pedestrian + // 6.判断是否需要停车（如行人） if (config_.is_stop_for_pedestrain() && CheckStopForPedestrian(*mutable_obstacle)) { ObjectDecisionType stop_decision; @@ -258,7 +270,7 @@ Status SpeedDecider::MakeObjectDecision( } continue; } - + // 7.获取障碍物的 ST 位置信息 auto location = GetSTLocation(path_decision, speed_profile, boundary); if (!FLAGS_use_st_drivable_boundary) { @@ -268,7 +280,7 @@ Status SpeedDecider::MakeObjectDecision( } } } - + // 8.判断障碍物的位置，并做出相应决策 switch (location) { case BELOW: if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) { @@ -332,9 +344,11 @@ Status SpeedDecider::MakeObjectDecision( return Status(ErrorCode::PLANNING_ERROR, msg); } break; + // 9.其他未知障碍物 default: AERROR << "Unknown position:" << location; } + // 10.忽略障碍物 AppendIgnoreDecision(mutable_obstacle); } @@ -534,6 +548,7 @@ bool SpeedDecider::CheckStopForPedestrian(const Obstacle& obstacle) const { } const auto& obstacle_sl_boundary = obstacle.PerceptionSLBoundary(); + // 忽略后方存在的障碍物 if (obstacle_sl_boundary.end_s() < adc_sl_boundary_.start_s()) { return false; } diff --git a/modules/routing/common/routing_gflags.h b/modules/routing/common/routing_gflags.h index 80d19a5d8de..043c234ea55 100644 --- a/modules/routing/common/routing_gflags.h +++ b/modules/routing/common/routing_gflags.h @@ -18,9 +18,9 @@ #include "gflags/gflags.h" -DECLARE_string(routing_conf_file); -DECLARE_string(routing_node_name); +DECLARE_string(routing_conf_file); // Routing模块配置文件的路径 +DECLARE_string(routing_node_name); // Routing模块的节点名称 -DECLARE_double(min_length_for_lane_change); -DECLARE_bool(enable_change_lane_in_result); -DECLARE_uint32(routing_response_history_interval_ms); +DECLARE_double(min_length_for_lane_change); // 在变道前，在当前车道上行驶的最短距离 +DECLARE_bool(enable_change_lane_in_result); // 导航结果是否允许变道 +DECLARE_uint32(routing_response_history_interval_ms); // 路由请求的响应时长 diff --git a/modules/routing/core/black_list_range_generator.cc b/modules/routing/core/black_list_range_generator.cc index 95a7680c85d..8255900d1b3 100644 --- a/modules/routing/core/black_list_range_generator.cc +++ b/modules/routing/core/black_list_range_generator.cc @@ -40,6 +40,7 @@ double MoveSBackward(double s, double lower_bound) { AERROR << "Illegal s: " << s << ", lower bound: " << lower_bound; return s; } + // S_GAP_FOR_BLACK：是一个全局常量（通常 ~0.2m、0.3m，避免路径拼接过于紧密），表示黑名单裁剪的安全间距 if (s - S_GAP_FOR_BLACK > lower_bound) { return (s - S_GAP_FOR_BLACK); } else { @@ -47,20 +48,31 @@ double MoveSBackward(double s, double lower_bound) { } } +/// @brief 递归地收集与给定节点 node 相邻的“平行”车道节点（拓扑节点），即从当前节点出发，沿着“向左或向右”的出边遍历，找到所有平行的节点并存入 node_set 中 +/// @param node +/// @param node_set void GetOutParallelLane(const TopoNode* node, std::unordered_set* const node_set) { + // 遍历当前节点的所有“向左”或“向右”的出边 for (const auto* edge : node->OutToLeftOrRightEdge()) { + //取这条边的目标节点 const auto* to_node = edge->ToNode(); + // 如果目标节点尚未加入集合，防止重复访问（防止死循环） if (node_set->count(to_node) == 0) { - node_set->emplace(to_node); - GetOutParallelLane(to_node, node_set); + node_set->emplace(to_node); // 加入集合 + GetOutParallelLane(to_node, node_set);// 递归遍历目标节点的平行节点 } } } +// 从当前节点出发，沿着“入向左右平行边”（InFromLeftOrRightEdge()）递归访问，收集所有从左边或右边与当前车道平行，且连接到当前车道的上游车道节点 +/// @brief 递归获取所有“入向”的平行车道节点 +/// @param node +/// @param node_set void GetInParallelLane(const TopoNode* node, std::unordered_set* const node_set) { for (const auto* edge : node->InFromLeftOrRightEdge()) { + // 取该入边对应的起始节点（上游节点） const auto* from_node = edge->FromNode(); if (node_set->count(from_node) == 0) { node_set->emplace(from_node); @@ -70,23 +82,31 @@ void GetInParallelLane(const TopoNode* node, } // for new navigator +// 把 request 里的 黑名单道路，转换成 黑名单区间，放进 range_manager 里 void AddBlackMapFromRoad(const routing::RoutingRequest& request, const TopoGraph* graph, TopoRangeManager* const range_manager) { + // 获取路段上的所有节点 for (const auto& road_id : request.blacklisted_road()) { std::unordered_set road_nodes_set; + // 通过 TopoGraph 的方法，把一个 road_id 映射到 这个路段上的所有车道节点 graph->GetNodesByRoadId(road_id, &road_nodes_set); + // 对每个节点，添加黑名单区间 for (const auto& node : road_nodes_set) { + // 对于该道路上的所有 TopoNode，把它 从头到尾（[0.0, node->Length()]）都标记为黑名单 range_manager->Add(node, 0.0, node->Length()); } } } // for new navigator +// 根据 RoutingRequest 中的黑名单车道，生成对应的黑名单范围，然后放到 range_manager 中 void AddBlackMapFromLane(const routing::RoutingRequest& request, const TopoGraph* graph, TopoRangeManager* const range_manager) { + // 每个元素（lane）通常有：id()：车道的唯一标识 start_s()、end_s()：黑名单范围的起点和终点 for (const auto& lane : request.blacklisted_lane()) { + // 根据车道 ID，去 拓扑图 TopoGraph 中找对应的 TopoNode const auto* node = graph->GetNode(lane.id()); if (node) { range_manager->Add(node, lane.start_s(), lane.end_s()); @@ -94,70 +114,118 @@ void AddBlackMapFromLane(const routing::RoutingRequest& request, } } +// 根据给定的车道 node，将它的外侧平行车道上对应裁剪位置附近（由 cut_ratio 指定的比例点）标记为黑名单，用以避免规划时驶入这些区域 +/// @brief 给定一个拓扑节点 node，基于它的“外侧平行车道”，在 range_manager 中添加对应的黑名单区间 +/// @param node 当前的车道拓扑节点 +/// @param cut_ratio 裁剪比例，0~1之间，表示在对应车道长度上的位置比例 +/// @param range_manager 用来管理车道黑名单区间的管理器指针 void AddBlackMapFromOutParallel(const TopoNode* node, double cut_ratio, TopoRangeManager* const range_manager) { + // 无序集合 par_node_set，用于存放与当前车道 node 外侧平行的所有车道节点 std::unordered_set par_node_set; + // 将当前车道的外侧平行车道节点放入 par_node_set GetOutParallelLane(node, &par_node_set); + // 从集合中移除自己（node），防止自己重复加入黑名单 par_node_set.erase(node); for (const auto* par_node : par_node_set) { + // 计算裁剪位置 par_cut_s，即该车道长度乘以裁剪比例 cut_ratio double par_cut_s = cut_ratio * par_node->Length(); + // 在 range_manager 中添加黑名单区间，这个区间是一个点区间（起点和终点都等于 par_cut_s），表示将此处标记为“黑名单 range_manager->Add(par_node, par_cut_s, par_cut_s); } } +/// @brief 针对**“入向”的平行车道**进行处理 +/// @param node +/// @param cut_ratio +/// @param range_manager void AddBlackMapFromInParallel(const TopoNode* node, double cut_ratio, TopoRangeManager* const range_manager) { std::unordered_set par_node_set; - GetInParallelLane(node, &par_node_set); - par_node_set.erase(node); + GetInParallelLane(node, &par_node_set); // 获取所有入向的平行车道节点 + par_node_set.erase(node); // 去除自身节点，避免重复 for (const auto* par_node : par_node_set) { - double par_cut_s = cut_ratio * par_node->Length(); - range_manager->Add(par_node, par_cut_s, par_cut_s); + // cut_ratio 是一个[0,1]的比例，表示当前车道节点上的位置占总长度的比例。乘以该节点长度获得对应的S坐标 + double par_cut_s = cut_ratio * par_node->Length(); // 计算对应的s位置 + range_manager->Add(par_node, par_cut_s, par_cut_s); // 只添加单点区间 } } } // namespace +/// @brief 根据 路由请求 中提供的信息（如黑名单车道、黑名单路段等），在 TopoRangeManager 中登记不可通行的范围 +/// @param request +/// @param graph +/// @param range_manager void BlackListRangeGenerator::GenerateBlackMapFromRequest( const routing::RoutingRequest& request, const TopoGraph* graph, TopoRangeManager* const range_manager) const { + // 从请求中的 黑名单车道 添加不可通行范围 AddBlackMapFromLane(request, graph, range_manager); + // 从请求中的 黑名单道路 添加不可通行范围 AddBlackMapFromRoad(request, graph, range_manager); + // 对所有黑名单范围进行 排序并合并 range_manager->SortAndMerge(); } +/// @brief +/// @param src_node 起点（起始 Lane） +/// @param dest_node 终点（终止 Lane） +/// @param start_s 在起点的 s 坐标 +/// @param end_s 在终点的 s 坐标 +/// @param range_manager 用于管理黑名单范围（可行驶区域裁剪） void BlackListRangeGenerator::AddBlackMapFromTerminal( const TopoNode* src_node, const TopoNode* dest_node, double start_s, double end_s, TopoRangeManager* const range_manager) const { + // start_length 是起点 Lane 的长度 double start_length = src_node->Length(); + // end_length 是终点 Lane 的长度 double end_length = dest_node->Length(); +// 定义了一个很小的数 0.01，作为“允许误差范围” static constexpr double kEpsilon = 1e-2; + + // 校正起点 s 值：如果 start_s 比起点 lane 长度略大（差值 ≤ 0.01），就直接当作 start_length const double start_s_adjusted = (start_s > start_length && start_s - start_length <= kEpsilon) ? start_length : start_s; + // 校正终点 s 值： 如果 end_s 比终点 Lane 长度略大，就用 end_length const double end_s_adjusted = (end_s > end_length && end_s - end_length <= kEpsilon) ? end_length : end_s; - + + // 检查起点 s 是否合法 if (start_s_adjusted < 0.0 || start_s_adjusted > start_length) { AERROR << "Illegal start_s: " << start_s << ", length: " << start_length; return; } + // 检查终点 s 是否合法 if (end_s_adjusted < 0.0 || end_s_adjusted > end_length) { AERROR << "Illegal end_s: " << end_s << ", length: " << end_length; return; } + // 计算起点的黑名单裁剪位置 + // 调用 MoveSBackward()，把 start_s_adjusted 往“后”稍微移动，得到裁剪 s 值 + // 这里第二个参数 0.0 说明是移动到起点位置 double start_cut_s = MoveSBackward(start_s_adjusted, 0.0); + + // 把起点裁剪点加入黑名单，在 src_node 里，把起点处的 s 范围裁掉（加到黑名单） range_manager->Add(src_node, start_cut_s, start_cut_s); + + // 处理起点的“并行段”黑名单，进一步把起点 lane 的并行段（起点部分）也加入黑名单 AddBlackMapFromOutParallel(src_node, start_cut_s / start_length, range_manager); - + + // 计算终点的黑名单裁剪位置 + // 调用 MoveSForward()，把 end_s_adjusted 往“前”稍微移动，得到裁剪 s double end_cut_s = MoveSForward(end_s_adjusted, end_length); + // 把终点裁剪点加入黑名单，在终点 Lane 里，也把裁剪位置加到黑名单 range_manager->Add(dest_node, end_cut_s, end_cut_s); + // 处理终点的“并行段”黑名单，进一步把终点 lane 的并行段（入口部分）也加入黑名单 AddBlackMapFromInParallel(dest_node, end_cut_s / end_length, range_manager); + // 最后合并范围，合并（排序 & 合并）所有已添加的黑名单范围，避免重叠或重复 range_manager->SortAndMerge(); } diff --git a/modules/routing/core/navigator.cc b/modules/routing/core/navigator.cc index 307af8120c1..f0275ac5384 100644 --- a/modules/routing/core/navigator.cc +++ b/modules/routing/core/navigator.cc @@ -30,7 +30,10 @@ using apollo::common::ErrorCode; bool ShowRequestInfo(const routing::RoutingRequest& request, const TopoGraph* graph) { + // 遍历请求中的所有路径点（waypoint） for (const auto& wp : request.waypoint()) { + + // 检查每个 waypoint 是否存在 const auto* node = graph->GetNode(wp.id()); if (node == nullptr) { AERROR << "Way node is not found in topo graph! ID: " << wp.id(); @@ -41,31 +44,47 @@ bool ShowRequestInfo(const routing::RoutingRequest& request, << " length: " << node->Length(); } +// 遍历 Blacklisted Lanes for (const auto& bl : request.blacklisted_lane()) { + + // 检查和打印黑名单车道信息 const auto* node = graph->GetNode(bl.id()); if (node == nullptr) { AERROR << "Black list node is not found in topo graph! ID: " << bl.id(); return false; } + // 日志打印每个 waypoint 信息：包括车道 ID、s 值、位置信息 (x, y)，以及车道长度 AINFO << "Black point:\tlane id: " << bl.id() << " start_s: " << bl.start_s() << " end_s: " << bl.end_s() << " length: " << node->Length(); } +// 如果所有 waypoints 和 blacklisted_lanes 都能在图里找到，返回 true return true; } +/// @brief +/// @param request 用户的路由请求，包含一系列 waypoint +/// @param graph 全局道路拓扑结构的 TopoGraph +/// @param way_nodes 对应于 waypoint 的 TopoNode 节点指针列表 +/// @param way_s 每个 waypoint 在其所属 lane 上的纵向坐标（s） +/// @return bool GetWayNodes(const routing::RoutingRequest& request, const TopoGraph* graph, std::vector* const way_nodes, std::vector* const way_s) { for (const auto& point : request.waypoint()) { + // point.id()：每个 waypoint 中的 lane id（对应车道 ID） + // 从 TopoGraph 的索引里查找这个 lane id 对应的 TopoNode const auto* cur_node = graph->GetNode(point.id()); if (cur_node == nullptr) { AERROR << "Cannot find way point in graph! Id: " << point.id(); return false; } + + // way_nodes：保存对应的 TopoNode 节点指针 way_nodes->push_back(cur_node); + // way_s：保存该 waypoint 在 lane 上的纵向坐标 s way_s->push_back(point.s()); } return true; @@ -94,21 +113,32 @@ void PrintDebugData(const std::vector& nodes) { } // namespace +/// @brief 通过拓扑图文件初始化导航器对象 +/// @param topo_file_path Navigator::Navigator(const std::string& topo_file_path) { + // 存储从文件加载的拓扑图数据 Graph graph; if (!cyber::common::GetProtoFromFile(topo_file_path, &graph)) { AERROR << "Failed to read topology graph from " << topo_file_path; return; } +// 创建一个新的 TopoGraph 对象，并用智能指针 graph_ 管理 graph_.reset(new TopoGraph()); + // 加载边以及节点 if (!graph_->LoadGraph(graph)) { AINFO << "Failed to init navigator graph failed! File path: " << topo_file_path; return; } + + // 创建并初始化 BlackListRangeGenerator 对象，用于生成“黑名单”区域（通常是路由中需避开的区域） black_list_generator_.reset(new BlackListRangeGenerator); + + // 创建并初始化 ResultGenerator 对象，用于生成路由结果（路径规划的最终结果） result_generator_.reset(new ResultGenerator); + + // 标记导航器状态为“已准备好”，表示初始化成功，可以开始执行路由任务 is_ready_ = true; AINFO << "The navigator is ready."; } @@ -119,11 +149,20 @@ bool Navigator::IsReady() const { return is_ready_; } void Navigator::Clear() { topo_range_manager_.Clear(); } +/// @brief +/// @param request 路由请求（包含起点、终点和途经点） +/// @param graph 拓扑图指针 +/// @param way_nodes 从请求 waypoint 中解析到的拓扑节点（TopoNode）列表 +/// @param way_s 对应节点在 lane 上的纵向坐标（s 坐标） +/// @return bool Navigator::Init(const routing::RoutingRequest& request, const TopoGraph* graph, std::vector* const way_nodes, std::vector* const way_s) { + // 清空导航器内部上一条规划残留的数据（如黑名单、上次结果等） Clear(); + + //解析 Waypoint，生成搜索起止点 if (!GetWayNodes(request, graph_.get(), way_nodes, way_s)) { AERROR << "Failed to find search terminal point in graph!"; return false; @@ -151,26 +190,47 @@ bool Navigator::MergeRoute( return true; } + +/// @brief 给定一系列路点（way_nodes 和它们的 s），分段 进行路径搜索，每段使用 AStarStrategy，最终合并成完整的路径 +/// @param graph +/// @param way_nodes +/// @param way_s +/// @param result_nodes +/// @return bool Navigator::SearchRouteByStrategy( const TopoGraph* graph, const std::vector& way_nodes, const std::vector& way_s, std::vector* const result_nodes) const { + // 初始化搜索策略 std::unique_ptr strategy_ptr; strategy_ptr.reset(new AStarStrategy(FLAGS_enable_change_lane_in_result)); - + + // result_nodes：最终结果 result_nodes->clear(); + // node_vec：分段搜索的所有结果，后续再做合并 std::vector node_vec; + + // 遍历 相邻路点对，逐段搜索路径 for (size_t i = 1; i < way_nodes.size(); ++i) { const auto* way_start = way_nodes[i - 1]; const auto* way_end = way_nodes[i]; double way_start_s = way_s[i - 1]; double way_end_s = way_s[i]; - + + // 生成 full_range_manager（包含黑名单）：后续会裁剪 + // topo_range_manager_：可行驶范围 TopoRangeManager full_range_manager = topo_range_manager_; + // AddBlackMapFromTerminal：把这段的黑名单也加进去，避免搜索到不合法路段 + // 针对路径段的起点和终点做“黑名单范围”裁剪（可行驶范围限制），并且还包括它们的“并行段”拓展 black_list_generator_->AddBlackMapFromTerminal( way_start, way_end, way_start_s, way_end_s, &full_range_manager); - + + // 构建 子图（SubTopoGraph）？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？ + // 以范围（RangeMap）生成一个子图（只包含当前段落可用的路段） SubTopoGraph sub_graph(full_range_manager.RangeMap()); + + // 获取 子图中的起点、终点 + // GetSubNodeWithS：保证起点/终点位于可行驶段中（可处理 s 范围） const auto* start = sub_graph.GetSubNodeWithS(way_start, way_start_s); if (start == nullptr) { AERROR << "Sub graph node is nullptr, origin node id: " @@ -184,6 +244,7 @@ bool Navigator::SearchRouteByStrategy( return false; } + // 在子图中使用 A 路径搜索 std::vector cur_result_nodes; if (!strategy_ptr->Search(graph, &sub_graph, start, end, &cur_result_nodes)) { @@ -191,11 +252,13 @@ bool Navigator::SearchRouteByStrategy( << " to " << end->LaneId(); return false; } - + + // 拼接段落路径 node_vec.insert(node_vec.end(), cur_result_nodes.begin(), cur_result_nodes.end()); } - + + // 合并分段路径结果 if (!MergeRoute(node_vec, result_nodes)) { AERROR << "Failed to merge route."; return false; @@ -205,6 +268,7 @@ bool Navigator::SearchRouteByStrategy( bool Navigator::SearchRoute(const routing::RoutingRequest& request, routing::RoutingResponse* const response) { + // 检查请求的有效性 if (!ShowRequestInfo(request, graph_.get())) { SetErrorCode(ErrorCode::ROUTING_ERROR_REQUEST, "Error encountered when reading request point!", @@ -212,13 +276,19 @@ bool Navigator::SearchRoute(const routing::RoutingRequest& request, return false; } +// Init完成后 +// 检查规划器是否就绪：如果 Navigator 内部（比如拓扑图等）尚未加载完毕，也直接返回 if (!IsReady()) { SetErrorCode(ErrorCode::ROUTING_ERROR_NOT_READY, "Navigator is not ready!", response->mutable_status()); return false; } + + // 初始化导航请求 std::vector way_nodes; std::vector way_s; + // 将 request 中的 waypoint 转换成实际的图中的 TopoNode 指针 + // 计算对应 s 坐标 if (!Init(request, graph_.get(), &way_nodes, &way_s)) { SetErrorCode(ErrorCode::ROUTING_ERROR_NOT_READY, "Failed to initialize navigator!", response->mutable_status()); @@ -237,9 +307,15 @@ bool Navigator::SearchRoute(const routing::RoutingRequest& request, response->mutable_status()); return false; } + + // 修正首尾的 s 坐标：修正第一段和最后一段的起止 s，和请求 waypoint 对齐 result_nodes.front().SetStartS(request.waypoint().begin()->s()); result_nodes.back().SetEndS(request.waypoint().rbegin()->s()); + +// 生成最终可通行区域（PassageRegion） +// 过搜索出来的 result_nodes，结合地图信息，生成完整的可通行区域（PassageRegion） +// 在 response 中填充所有可行段落的 LaneSegment，最终生成完整的 RoutingResponse if (!result_generator_->GeneratePassageRegion( graph_->MapVersion(), request, result_nodes, topo_range_manager_, response)) { diff --git a/modules/routing/graph/sub_topo_graph.cc b/modules/routing/graph/sub_topo_graph.cc index 891b5ea691d..269e5027dd9 100644 --- a/modules/routing/graph/sub_topo_graph.cc +++ b/modules/routing/graph/sub_topo_graph.cc @@ -35,39 +35,73 @@ bool IsCloseEnough(double s1, double s2) { return std::fabs(s1 - s2) < MIN_DIFF_LENGTH; } +/// @brief 将输入的原始区间集合（origin_range）按顺序合并重叠或相邻的区间，并剪裁到车道有效区间内，最后输出到 block_range +/// @param topo_node +/// @param origin_range +/// @param block_range void MergeBlockRange(const TopoNode* topo_node, const std::vector& origin_range, std::vector* block_range) { + // 复制输入的原始区间 origin_range 到本地 sorted_origin_range std::vector sorted_origin_range; sorted_origin_range.insert(sorted_origin_range.end(), origin_range.begin(), origin_range.end()); + // 对 sorted_origin_range 进行排序（默认排序按 NodeSRange 的起始位置排序，前提是 NodeSRange 有对应的 operator< 实现） sort(sorted_origin_range.begin(), sorted_origin_range.end()); + + // 初始化循环索引，准备遍历排序后的区间列表 int cur_index = 0; int total_size = static_cast(sorted_origin_range.size()); + + // 取当前索引区间作为合并区间的初始值 while (cur_index < total_size) { NodeSRange range(sorted_origin_range[cur_index]); ++cur_index; + + // 内层循环试图把所有与当前 range 重叠或相邻的区间合并进 range 中 + // MergeRangeOverlap：如果参数区间和 range 有重叠或紧邻，合并它们，扩展 range，返回 true while (cur_index < total_size && range.MergeRangeOverlap(sorted_origin_range[cur_index])) { ++cur_index; } + + // 判断合并后的区间是否完全在车道有效区间外（车道起点到终点） if (range.EndS() < topo_node->StartS() || range.StartS() > topo_node->EndS()) { continue; } + + // 将区间裁剪到车道有效范围内（保证区间起点不小于车道起点，终点不超过车道终点） range.SetStartS(std::max(topo_node->StartS(), range.StartS())); range.SetEndS(std::min(topo_node->EndS(), range.EndS())); + + // 将合并且裁剪后的区间加入输出的阻塞区间集合中 block_range->push_back(std::move(range)); } } +/* +例如，假设阻塞区间是 [3,5] 和 [7,9]，车道起点0，终点10，则 all_value 为 [0,3,5,7,9,10]。 + +则有效区间就是 [0,3]、[5,7]、[9,10]，这些区间是可通行的 +*/ +/// @brief 根据传入的原始区间（origin_range），结合车道的起止位置，生成一个排好序且有效的区间集合（valid_range） +/// @param topo_node +/// @param origin_range +/// @param valid_range void GetSortedValidRange(const TopoNode* topo_node, const std::vector& origin_range, std::vector* valid_range) { + // 用于存放合并后的“阻塞区间” std::vector block_range; + // 对原始区间进行合并，得到一组合并后的区间，避免区间重叠和碎片化 MergeBlockRange(topo_node, origin_range, &block_range); + // 获取该拓扑节点的起点和终点位置的 s 值（车道上的起止坐标） double start_s = topo_node->StartS(); double end_s = topo_node->EndS(); + + // 构造一个数值序列 all_value： 用来从阻塞区间划分出有效区间的边界点 + // 包含： 起点 start_s 阻塞区间的所有起点和终点（交替加入） 终点 end_s std::vector all_value; all_value.push_back(start_s); for (const auto& range : block_range) { @@ -75,8 +109,11 @@ void GetSortedValidRange(const TopoNode* topo_node, all_value.push_back(range.EndS()); } all_value.push_back(end_s); + + // 遍历 all_value，每两个点作为一对区间端点，构造一个 NodeSRange（有效区间） for (size_t i = 0; i < all_value.size(); i += 2) { NodeSRange new_range(all_value[i], all_value[i + 1]); + // 将这些区间添加到输出参数 valid_range 中 valid_range->push_back(std::move(new_range)); } } @@ -91,21 +128,37 @@ bool IsReachable(const TopoNode* from_node, const TopoNode* to_node) { } // namespace +/* +根据黑名单区间提取出每个车道的有效子区间； +基于这些有效区间初始化对应的子节点； +初始化子节点之间的子边； +添加潜在的子边连接（可能是非直接相邻但逻辑上相连的） +*/ +/// @brief 基于传入的“黑名单区间映射”（black_map）初始化子拓扑图（SubTopoGraph）的主要步骤 +/// @param black_map 一个 unordered_map，key 是指向原始 TopoNode 的指针，value 是该车道对应的多个可用区间（NodeSRange）列表 SubTopoGraph::SubTopoGraph( const std::unordered_map>& black_map) { std::vector valid_range; + // 遍历黑名单映射表里的每个原始车道节点及其区间 for (const auto& map_iter : black_map) { valid_range.clear(); + // 对当前车道的多个区间进行排序和过滤，得到排好序且有效的区间集合 valid_range GetSortedValidRange(map_iter.first, map_iter.second, &valid_range); + // 初始化这个车道对应的 子节点： 这里的“子节点”是指拓扑图中原节点的子分段，按区间划分成更细粒度的节点 InitSubNodeByValidRange(map_iter.first, valid_range); } + // 继续遍历黑名单映射表中的每个原始车道节点 for (const auto& map_iter : black_map) { + // 初始化它们之间的连接边（子边）：基于原拓扑图中车道节点的连接关系，将连接映射到子节点层级 InitSubEdge(map_iter.first); } + // 再次遍历每个原始车道节点 for (const auto& map_iter : black_map) { + // 添加“潜在的子边”，即可能的非标准连接，或为了更丰富路径规划考虑而加入的连接 + // 潜在边可以是跨车道跳转、特殊车道合流等路径规划需要的连接 AddPotentialEdge(map_iter.first); } } @@ -167,30 +220,42 @@ const TopoNode* SubTopoGraph::GetSubNodeWithS(const TopoNode* topo_node, return sorted_vec[index].GetTopoNode(); } +// 根据给定的 valid_range（一组有效区间）把一个大 TopoNode 拆分成多个子节点 (sub_topo_node)，并且在这些子节点之间建立顺序连接的边 void SubTopoGraph::InitSubNodeByValidRange( const TopoNode* topo_node, const std::vector& valid_range) { // Attention: no matter topo node has valid_range or not, // create map value first; + // 当前 topo_node 对应的所有子节点及其区间 auto& sub_node_vec = sub_node_range_sorted_map_[topo_node]; + // 方便快速查找子节点的集合 auto& sub_node_set = sub_node_map_[topo_node]; std::vector sub_node_sorted_vec; + // 遍历所有有效区间，生成子节点 for (const auto& range : valid_range) { + // 跳过过小的区间（小于 MIN_INTERNAL_FOR_NODE，避免生成碎片节点） if (range.Length() < MIN_INTERNAL_FOR_NODE) { continue; } std::shared_ptr sub_topo_node_ptr; + // 用原始的 topo_node 和这个区间构造一个新的子节点对象 sub_topo_node_ptr.reset(new TopoNode(topo_node, range)); + // 将子节点指针和区间保存到对应的容器中 sub_node_vec.emplace_back(sub_topo_node_ptr.get(), range); + // topo_nodes_ 是整个子图管理的所有子节点的集合，保存所有子节点的 shared_ptr，保证生命周期 sub_node_set.insert(sub_topo_node_ptr.get()); + // sub_node_sorted_vec 临时保存所有生成的子节点指针，用于下一步连接 sub_node_sorted_vec.push_back(sub_topo_node_ptr.get()); topo_nodes_.push_back(std::move(sub_topo_node_ptr)); } - + + // 连接相邻的子节点 for (size_t i = 1; i < sub_node_sorted_vec.size(); ++i) { auto* pre_node = sub_node_sorted_vec[i - 1]; auto* next_node = sub_node_sorted_vec[i]; + // 判断两个节点的区间是否“足够接近”（IsCloseEnough），避免出现间隙过大断开 if (IsCloseEnough(pre_node->EndS(), next_node->StartS())) { + // 如果接近，则构造一条从前一个子节点到后一个子节点的有向边，代表子节点之间的连接关系 Edge edge; edge.set_from_lane_id(topo_node->LaneId()); edge.set_to_lane_id(topo_node->LaneId()); @@ -198,6 +263,7 @@ void SubTopoGraph::InitSubNodeByValidRange( edge.set_cost(0.0); std::shared_ptr topo_edge_ptr; topo_edge_ptr.reset(new TopoEdge(edge, pre_node, next_node)); + // 将这条边加入前驱节点的出边和后继节点的入边列表 pre_node->AddOutEdge(topo_edge_ptr.get()); next_node->AddInEdge(topo_edge_ptr.get()); topo_edges_.push_back(std::move(topo_edge_ptr)); @@ -205,14 +271,20 @@ void SubTopoGraph::InitSubNodeByValidRange( } } +// 拿到原始节点的所有子节点后，为它们“继承”原始节点的入边和出边，建立起子节点之间对应的连接关系 +//为给定的一个原始节点（topo_node）的所有子节点（sub_nodes）初始化其入边和出边 void SubTopoGraph::InitSubEdge(const TopoNode* topo_node) { std::unordered_set sub_nodes; + // 获取该原始节点对应的所有子节点集合 if (!GetSubNodes(topo_node, &sub_nodes)) { return; } - + + // 遍历所有子节点，对每个子节点分别调用两个初始化边的方法 for (auto* sub_node : sub_nodes) { + // 根据原始节点所有的入边（InFromAllEdge()），给该子节点初始化入边 InitInSubNodeSubEdge(sub_node, topo_node->InFromAllEdge()); + // 根据原始节点所有的出边（OutToAllEdge()），给该子节点初始化出边 InitOutSubNodeSubEdge(sub_node, topo_node->OutToAllEdge()); } } @@ -271,9 +343,15 @@ void SubTopoGraph::InitOutSubNodeSubEdge( } } +/// @brief 根据给定的原始节点 node，从内部的 sub_node_map_ 容器里查找对应的所有子节点集合，并把这些子节点放到 sub_nodes 输出参数里 +/// @param node +/// @param sub_nodes +/// @return bool SubTopoGraph::GetSubNodes( const TopoNode* node, std::unordered_set* const sub_nodes) const { + // 在 sub_node_map_ 中查找 node 作为 key 的记录 + // sub_node_map_ 是一个从原始节点指针映射到该节点所有子节点集合的哈希表 const auto& iter = sub_node_map_.find(node); if (iter == sub_node_map_.end()) { return false; diff --git a/modules/routing/graph/topo_graph.cc b/modules/routing/graph/topo_graph.cc index 492f1b4d7dc..a2fe9f4261a 100644 --- a/modules/routing/graph/topo_graph.cc +++ b/modules/routing/graph/topo_graph.cc @@ -28,15 +28,27 @@ void TopoGraph::Clear() { } bool TopoGraph::LoadNodes(const Graph& graph) { + // 拓扑图中没有节点 if (graph.node().empty()) { AERROR << "No nodes found in topology graph."; return false; } + + // 遍历graph中所有节点：一个节点代表一条lane for (const auto& node : graph.node()) { + // 使用节点的车道ID (lane_id()) 作为键，映射到当前已加载的节点数量（即新节点的索引） + // 映射便于根据车道ID快速定位节点在 topo_nodes_ 中的位置 node_index_map_[node.lane_id()] = static_cast(topo_nodes_.size()); + + // 创建一个新的 TopoNode 对象，使用当前遍历的 node protobuf 数据初始化 std::shared_ptr topo_node; topo_node.reset(new TopoNode(node)); + + // 将该 TopoNode 的裸指针插入以道路ID (road_id()) 为键的 road_node_map_ 容器中 + // 该映射用于快速根据道路ID查询所有属于该道路的拓扑节点 road_node_map_[node.road_id()].insert(topo_node.get()); + + // 把智能指针 topo_node 添加到成员变量 topo_nodes_ 中，topo_nodes_ 是存储所有拓扑节点的容器 topo_nodes_.push_back(std::move(topo_node)); } return true; @@ -44,38 +56,56 @@ bool TopoGraph::LoadNodes(const Graph& graph) { // Need to execute load_nodes() firstly bool TopoGraph::LoadEdges(const Graph& graph) { +// 拓扑图中没有边，不过没关系 if (graph.edge().empty()) { AINFO << "0 edges found in topology graph, but it's fine"; return true; } + + // 遍历 graph 中每一条边 for (const auto& edge : graph.edge()) { + // 获取该边的起点车道ID（from_lane_id）和终点车道ID（to_lane_id） const std::string& from_lane_id = edge.from_lane_id(); const std::string& to_lane_id = edge.to_lane_id(); + // 检查这两个车道ID是否都出现在已加载的节点映射表中 if (node_index_map_.count(from_lane_id) != 1 || node_index_map_.count(to_lane_id) != 1) { return false; } + + // 根据索引获取 from_node 和 to_node 的原始指针 + // 这两个节点对象构成边的两端 std::shared_ptr topo_edge; TopoNode* from_node = topo_nodes_[node_index_map_[from_lane_id]].get(); TopoNode* to_node = topo_nodes_[node_index_map_[to_lane_id]].get(); + + // 创建一个新的 TopoEdge 对象，使用当前 protobuf 中的 edge 数据以及起点和终点节点初始化 topo_edge.reset(new TopoEdge(edge, from_node, to_node)); + + // 将该边加入到 from_node 的“出边”列表（即这个节点出去的边） from_node->AddOutEdge(topo_edge.get()); + // 同时加入到 to_node 的“入边”列表（即这个节点进来的边） to_node->AddInEdge(topo_edge.get()); + // 把该边对象加入 topo_edges_ 容器中，保存所有边的集合 topo_edges_.push_back(std::move(topo_edge)); } return true; } bool TopoGraph::LoadGraph(const Graph& graph) { + // 清空当前拓扑图数据，重置成员变量 Clear(); map_version_ = graph.hdmap_version(); map_district_ = graph.hdmap_district(); + // 加载拓扑节点（路网中的关键点） if (!LoadNodes(graph)) { AERROR << "Failed to load nodes from topology graph."; return false; } + + // 加载拓扑边（连接节点的道路段） if (!LoadEdges(graph)) { AERROR << "Failed to load edges from topology graph."; return false; @@ -88,14 +118,29 @@ const std::string& TopoGraph::MapVersion() const { return map_version_; } const std::string& TopoGraph::MapDistrict() const { return map_district_; } +/// @brief +/// @param id 一个车道 ID（string），比如 lane_id +/// @return const TopoNode* TopoGraph::GetNode(const std::string& id) const { + // node_index_map_： std::unordered_map + // 从 lane_id 映射到索引，快速找到这个 ID 在 topo_nodes_ 中的位置 const auto& iter = node_index_map_.find(id); if (iter == node_index_map_.end()) { return nullptr; } + // topo_nodes_： std::vector> 保存了所有的 TopoNode 对象。每个 TopoNode 用智能指针管理 return topo_nodes_[iter->second].get(); } +/* +node_index_map_["lane_123"] = 5; +topo_nodes_[5] = std::make_unique("lane_123", ...); + +const TopoNode* node = topo_graph.GetNode("lane_123"); + +topo_nodes_[5].get(); +*/ + void TopoGraph::GetNodesByRoadId( const std::string& road_id, std::unordered_set* const node_in_road) const { diff --git a/modules/routing/graph/topo_node.cc b/modules/routing/graph/topo_node.cc index 57a866a5400..9f30bd92764 100644 --- a/modules/routing/graph/topo_node.cc +++ b/modules/routing/graph/topo_node.cc @@ -254,50 +254,70 @@ bool TopoNode::IsOverlapEnough(const TopoNode* sub_node, } void TopoNode::AddInEdge(const TopoEdge* edge) { + // 检查该边的终点是不是当前节点（即这是否是一条“入边”） if (edge->ToNode() != this) { return; } + + // 如果这条边的起点（FromNode）已经在入边映射中存在，说明这条边已经添加过，也直接返回避免重复添加 if (in_edge_map_.count(edge->FromNode()) != 0) { return; } switch (edge->Type()) { + // 对于一条从其他车道左转进入当前车道的边 case TET_LEFT: - in_from_right_edge_set_.insert(edge); + // 这里的命名基于当前节点“看到”的入边角度 —— 别人左转进来，相对我是从右侧来 + in_from_right_edge_set_.insert(edge); // 在当前节点看来，它是从“右侧”进入的，因此加入 in_from_right_edge_set_ in_from_left_or_right_edge_set_.insert(edge); break; case TET_RIGHT: + // 如果别人右转进入当前节点，当前节点看到的方向是“从左侧”进入 in_from_left_edge_set_.insert(edge); in_from_left_or_right_edge_set_.insert(edge); break; default: + // 对于直行或无法分类的边，认为是“从前方进入”的，加入 in_from_pre_edge_set_ in_from_pre_edge_set_.insert(edge); break; } + // 不管类型是什么，所有入边都加入总集合 in_from_all_edge_set_ in_from_all_edge_set_.insert(edge); + // 将这条入边存入映射表中，方便根据起点节点快速访问边信息 in_edge_map_[edge->FromNode()] = edge; } +/// @brief 接收一条指向其他节点的边（TopoEdge）指针，尝试将它加入当前节点的“出边”集合中 +/// @param edge void TopoNode::AddOutEdge(const TopoEdge* edge) { + // 检查这条边的起点是否确实是当前这个节点 if (edge->FromNode() != this) { return; } + + // 如果这条边的终点已经存在于 out_edge_map_ 中（说明之前已经添加过），也不重复添加，直接返回 if (out_edge_map_.count(edge->ToNode()) != 0) { return; } + + // 根据边的类型 Type() 做分类。Apollo 中边类型一般包括直行、左转、右转等 switch (edge->Type()) { case TET_LEFT: - out_to_left_edge_set_.insert(edge); - out_to_left_or_right_edge_set_.insert(edge); + out_to_left_edge_set_.insert(edge); // 左转边集合 + out_to_left_or_right_edge_set_.insert(edge); // 左右转边集合 break; case TET_RIGHT: - out_to_right_edge_set_.insert(edge); - out_to_left_or_right_edge_set_.insert(edge); + out_to_right_edge_set_.insert(edge); // 右转边集合 + out_to_left_or_right_edge_set_.insert(edge); // 左右转边集合 break; default: - out_to_suc_edge_set_.insert(edge); + out_to_suc_edge_set_.insert(edge); // 其他类型的边（如直行），加入 out_to_suc_edge_set_（继承边集合）中 break; } + + // 统一处理 + // 所有出边的统一集合（无论左转、右转还是直行），方便统一访问 out_to_all_edge_set_.insert(edge); + // 从当前节点到其他节点的出边映射表，键是终点节点，值是对应的边 out_edge_map_[edge->ToNode()] = edge; } diff --git a/modules/routing/graph/topo_range.cc b/modules/routing/graph/topo_range.cc index 5ebba2c5daf..e1b93c4e4de 100644 --- a/modules/routing/graph/topo_range.cc +++ b/modules/routing/graph/topo_range.cc @@ -53,14 +53,20 @@ void NodeSRange::SetStartS(double start_s) { start_s_ = start_s; } void NodeSRange::SetEndS(double end_s) { end_s_ = end_s; } +/// @brief 将两个区间合并，如果有重叠或相邻，则合并到当前区间中 +/// @param other +/// @return bool NodeSRange::MergeRangeOverlap(const NodeSRange& other) { if (!IsValid() || !other.IsValid()) { return false; } + // 判断两个区间是否不相交 if (other.StartS() > EndS() || other.EndS() < StartS()) { return false; } + // 将当前区间的结束位置更新为两个区间的最大结束位置 SetEndS(std::max(EndS(), other.EndS())); + // 将当前区间的起始位置更新为两个区间的最小起始位置 SetStartS(std::min(StartS(), other.StartS())); return true; } diff --git a/modules/routing/proto/topo_graph.proto b/modules/routing/proto/topo_graph.proto index 9c831d39b7e..197bd67edb2 100644 --- a/modules/routing/proto/topo_graph.proto +++ b/modules/routing/proto/topo_graph.proto @@ -4,15 +4,18 @@ package apollo.routing; import "modules/common_msgs/map_msgs/map_geometry.proto"; +// 曲线上的一个点 message CurvePoint { optional double s = 1; } +// 曲线上的一段 message CurveRange { optional CurvePoint start = 1; optional CurvePoint end = 2; } +// 车道上的一个节点，包含了所属车道，道路，长度，曲线起止点，中心线等信息 message Node { optional string lane_id = 1; optional double length = 2; @@ -24,6 +27,7 @@ message Node { optional string road_id = 8; } +// 连接车道之间的边，包含了起止车道id，代价和方向等信息 message Edge { enum DirectionType { FORWARD = 0; @@ -37,9 +41,10 @@ message Edge { optional DirectionType direction_type = 4; } +// 完整地图的Topo结构，这其中包含了多个Node和Edge message Graph { - optional string hdmap_version = 1; - optional string hdmap_district = 2; + optional string hdmap_version = 1; // 拓扑地图对应的地图版本 + optional string hdmap_district = 2; // 托普地图对应的地图区域 repeated Node node = 3; repeated Edge edge = 4; } diff --git a/modules/routing/routing.cc b/modules/routing/routing.cc index 88d379d1704..f7bf2696934 100644 --- a/modules/routing/routing.cc +++ b/modules/routing/routing.cc @@ -34,11 +34,18 @@ std::string Routing::Name() const { return FLAGS_routing_node_name; } Routing::Routing() : monitor_logger_buffer_(common::monitor::MonitorMessageItem::ROUTING) {} +/// @brief 路由模块的初始化 +/// @return apollo::common::Status Routing::Init() { + // 获取路由用的拓扑图文件路径，RoutingMapFile() 通常返回一个包含拓扑信息的地图文件路径字符串。 const auto routing_map_file = apollo::hdmap::RoutingMapFile(); AINFO << "Use routing topology graph path: " << routing_map_file; + + // 创建并初始化 Navigator 对象，传入拓扑图路径 + // Navigator 是负责路径查找、导航决策的核心类，基于拓扑图执行路由算法 navigator_ptr_.reset(new Navigator(routing_map_file)); + // 基础地图包含详细的车道线、交通标志等信息 hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr(); ACHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile(); @@ -56,23 +63,35 @@ apollo::common::Status Routing::Start() { return apollo::common::Status::OK(); } +/// @brief 补全或扩展 RoutingRequest 中缺失的车道信息（lane_id 和 s 坐标），同时生成额外候选请求 +/// @param routing_request 一个原始的 RoutingRequest，可能部分 waypoint 没有 lane_id +/// @return 一组新的 RoutingRequest，每条请求都有完整的 lane_id 信息（以及额外候选车道生成的新请求） std::vector Routing::FillLaneInfoIfMissing( const routing::RoutingRequest& routing_request) { + // 最终结果 std::vector fixed_requests; + // 记录候选车道，索引是 waypoint 的下标 std::unordered_map> additional_lane_waypoint_map; + // 拷贝一份，用于补齐当前请求 routing::RoutingRequest fixed_request(routing_request); + + // 遍历所有 Waypoints for (int i = 0; i < routing_request.waypoint_size(); ++i) { LaneWaypoint lane_waypoint(routing_request.waypoint(i)); if (lane_waypoint.has_id()) { continue; } + //如果该 waypoint 已经有 lane_id，开始寻找最近的 lane // fill lane info when missing + // 查找最近车道（多圈扩大半径搜索） + // 将 waypoint 转成 ENU 坐标系的点 const auto point = common::util::PointFactory::ToPointENU(lane_waypoint.pose()); std::vector> lanes; // look for lanes with bigger radius if not found + // 从 0.3m 半径开始，循环扩大半径（步进 0.3m，共 20 次，最大 6m）查找附近的 lanes constexpr double kRadius = 0.3; for (int i = 0; i < 20; ++i) { hdmap_->GetLanes(point, kRadius + i * kRadius, &lanes); @@ -80,21 +99,27 @@ std::vector Routing::FillLaneInfoIfMissing( break; } } + + // 处理找不到 lane 的情况 if (lanes.empty()) { AERROR << "Failed to find nearest lane from map at position: " << point.DebugString(); return fixed_requests; // return empty vector } + + // 记录匹配到的 lanes for (size_t j = 0; j < lanes.size(); ++j) { double s = 0.0; double l = 0.0; lanes[j]->GetProjection({point.x(), point.y()}, &s, &l); if (j == 0) { + //对于第一个匹配到的 lane： 直接更新到 fixed_request 的 waypoint auto waypoint_info = fixed_request.mutable_waypoint(i); waypoint_info->set_id(lanes[j]->id().id()); waypoint_info->set_s(s); } else { // additional candidate lanes + // 对于后续的 lane：生成一个新的 LaneWaypoint，放到 additional_lane_waypoint_map 中，表示这是该 waypoint 的候选车道（重叠部分） LaneWaypoint new_lane_waypoint(lane_waypoint); new_lane_waypoint.set_id(lanes[j]->id().id()); new_lane_waypoint.set_s(s); @@ -102,10 +127,16 @@ std::vector Routing::FillLaneInfoIfMissing( } } } + + // 生成所有可能的“请求组合” // first routing_request + // 先把基础补齐的 fixed_request 放入结果列表 fixed_requests.push_back(fixed_request); // additional routing_requests because of lane overlaps + // 遍历 additional_lane_waypoint_map + // 按照候选车道，克隆当前所有 fixed_requests 并在对应 waypoint 中替换新的车道信息，形成新的请求 + // 逐步扩展请求组合，考虑所有多车道重叠的可能性 for (const auto& m : additional_lane_waypoint_map) { size_t cur_size = fixed_requests.size(); for (size_t i = 0; i < cur_size; ++i) { @@ -126,21 +157,34 @@ std::vector Routing::FillLaneInfoIfMissing( return fixed_requests; } +/// @brief +/// @param routing_request 来自用户的路径请求 +/// @param routing_response 输出结果，存放路径规划成功后的完整路径 +/// @return bool Routing::Process( const std::shared_ptr& routing_request, routing::RoutingResponse* const routing_response) { + // 如果请求为空，发出警告并提前返回 true if (nullptr == routing_request) { AWARN << "Routing request is empty!"; return true; } + // 指针指向了一段内存 CHECK_NOTNULL(routing_response); + // 输出完整路径请求信息，方便调试或日志查看 AINFO << "Get new routing request:" << routing_request->DebugString(); +// 处理请求点缺失车道线信息的情况：补全每个请求点的 lane_id +// 如果请求中只有 GPS 或路径点，没有 lane_id，这个函数会通过 HDMap 反查补齐 const auto& fixed_requests = FillLaneInfoIfMissing(*routing_request); + + // 遍历所有补全后的请求，尝试规划路径 double min_routing_length = std::numeric_limits::max(); for (const auto& fixed_request : fixed_requests) { routing::RoutingResponse routing_response_temp; if (navigator_ptr_->SearchRoute(fixed_request, &routing_response_temp)) { + // 如果搜索成功，比较其路径长度 + // 保留最短路径作为最终的 routing_response const double routing_length = routing_response_temp.measurement().distance(); if (routing_length < min_routing_length) { @@ -149,6 +193,8 @@ bool Routing::Process( } } } + + if (min_routing_length < std::numeric_limits::max()) { monitor_logger_buffer_.INFO("Routing success!"); return true; diff --git a/modules/routing/routing_component.cc b/modules/routing/routing_component.cc index f9ee366b895..18985976c4d 100644 --- a/modules/routing/routing_component.cc +++ b/modules/routing/routing_component.cc @@ -15,26 +15,31 @@ *****************************************************************************/ #include "modules/routing/routing_component.h" - +// C++标准库，通常用来支持 std::move, std::pair 等工具 #include - +// 包含了与 adapter 和 routing 相关的 gflags 参数定义 #include "modules/common/adapters/adapter_gflags.h" #include "modules/routing/common/routing_gflags.h" +// 告诉编译器，这个参数变量存在，可以在当前文件中使用 DECLARE_string(flagfile); namespace apollo { namespace routing { bool RoutingComponent::Init() { + // 创建一个空的路由配置 protobuf 对象 RoutingConfig routing_conf; + + // 从组件配置文件（通常是 protobuf 格式的 .conf 文件）中读取配置，填充到 routing_conf ACHECK(cyber::ComponentBase::GetProtoConfig(&routing_conf)) << "Unable to load routing conf file: " << cyber::ComponentBase::ConfigFilePath(); AINFO << "Config file: " << cyber::ComponentBase::ConfigFilePath() << " is loaded."; - + + // 创建一个 Cyber RT 通信的写者 response_writer_，用于发布路由规划结果 apollo::cyber::proto::RoleAttributes attr; attr.set_channel_name(routing_conf.topic_config().routing_response_topic()); auto qos = attr.mutable_qos_profile(); @@ -43,8 +48,10 @@ bool RoutingComponent::Init() { apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE); qos->set_durability( apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL); + // 用 node_->CreateWriter(attr) 创建写者 response_writer_ = node_->CreateWriter(attr); +// 创建另一个写者 response_history_writer_，发布历史的路由响应消息，话题为 routing_response_history_topic apollo::cyber::proto::RoleAttributes attr_history; attr_history.set_channel_name( routing_conf.topic_config().routing_response_history_topic()); @@ -55,13 +62,15 @@ bool RoutingComponent::Init() { apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE); qos_history->set_durability( apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL); - response_history_writer_ = node_->CreateWriter(attr_history); + + // 获取当前组件的 weak_ptr 智能指针，避免 lambda 捕获时产生循环引用导致内存泄漏 std::weak_ptr self = std::dynamic_pointer_cast(shared_from_this()); + timer_.reset(new ::apollo::cyber::Timer( - FLAGS_routing_response_history_interval_ms, + FLAGS_routing_response_history_interval_ms, // 1000 ms [self, this]() { auto ptr = self.lock(); if (ptr) { @@ -75,18 +84,27 @@ bool RoutingComponent::Init() { }, false)); timer_->Start(); - + + // routing_（内部的路由核心算法实例）的初始化和启动函数 return routing_.Init().ok() && routing_.Start().ok(); } bool RoutingComponent::Proc( const std::shared_ptr& request) { + // 新建一个空的 RoutingResponse 用于存放结果 auto response = std::make_shared(); + // routing_ 是一个成员变量，类型是 Routing，真正执行路径规划逻辑 if (!routing_.Process(request, response.get())) { return false; } + + // 给响应加上通用头部信息（时间戳、模块名等） common::util::FillHeader(node_->Name(), response.get()); + + // 发布路由结果 response_writer_->Write(response); + + // 存储最新响应（用于定时器回放历史） { std::lock_guard guard(mutex_); response_ = std::move(response); diff --git a/modules/routing/topo_creator/topo_creator.cc b/modules/routing/topo_creator/topo_creator.cc index c29fa6465ca..bfd121e9d27 100644 --- a/modules/routing/topo_creator/topo_creator.cc +++ b/modules/routing/topo_creator/topo_creator.cc @@ -14,13 +14,19 @@ * limitations under the License. *****************************************************************************/ +// 处理文件读写 #include "cyber/common/file.h" +// 处理高精地图相关的工具类 #include "modules/map/hdmap/hdmap_util.h" +// 路由模块中定义的命令行参数（gflags） #include "modules/routing/common/routing_gflags.h" +// 创建路由拓扑图的核心逻辑 #include "modules/routing/topo_creator/graph_creator.h" int main(int argc, char **argv) { + // 初始化 Google 日志系统，用程序名作为日志标签 google::InitGoogleLogging(argv[0]); + // 解析命令行参数并赋值给 GFlags 变量，例如 --routing_conf_file google::ParseCommandLineFlags(&argc, &argv, true); apollo::routing::RoutingConfig routing_conf; @@ -31,10 +37,14 @@ int main(int argc, char **argv) { AINFO << "Conf file: " << FLAGS_routing_conf_file << " is loaded."; +// base_map: 基础地图路径 const auto base_map = apollo::hdmap::BaseMapFile(); +// routing_map: 路由地图路径（输出路径） const auto routing_map = apollo::hdmap::RoutingMapFile(); - + +// 使用基础地图路径、输出路径、配置参数构造一个 GraphCreator 对象 apollo::routing::GraphCreator creator(base_map, routing_map, routing_conf); +// 生成路由拓扑图 ACHECK(creator.Create()) << "Create routing topo failed!"; AINFO << "Create routing topo successfully from " << base_map << " to " diff --git a/scripts/apollo.bashrc b/scripts/apollo.bashrc index 7304a7c53b8..9cb4a2dbdfc 100644 --- a/scripts/apollo.bashrc +++ b/scripts/apollo.bashrc @@ -16,8 +16,8 @@ # limitations under the License. ############################################################################### -TOP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd -P)" -APOLLO_ROOT_DIR="${TOP_DIR}" +TOP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd -P)" #当前脚本所在目录的上一级目录的绝对路径 +APOLLO_ROOT_DIR="${TOP_DIR}" # Apollo 项目的根目录被设置为当前脚本所在目录的上一级目录 APOLLO_IN_DOCKER=false # If inside docker container if [ -f /.dockerenv ]; then diff --git a/scripts/apollo_build.sh b/scripts/apollo_build.sh index a1d5b4d1ff4..8faa81e8ec2 100644 --- a/scripts/apollo_build.sh +++ b/scripts/apollo_build.sh @@ -19,16 +19,20 @@ set -e TOP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd -P)" +## 加载 Apollo 基础环境 source "${TOP_DIR}/scripts/apollo_base.sh" function main() { + # 调用 site_restore 函数（该函数定义在 apollo_base.sh），可能用于 恢复某些 Apollo 依赖项或环境设置 site_restore + # 解析传入的命令行参数 parse_cmdline_arguments "$@" + # 运行 Bazel 进行构建 run_bazel "Build" if [ -z "${SHORTHAND_TARGETS}" ]; then SHORTHAND_TARGETS="apollo" fi success "Done building ${SHORTHAND_TARGETS}. Enjoy!" } - +# 运行 main 函数，传入所有命令行参数 main "$@"