【一看就会】Autoware.universe的“规划”部分源码梳理【一】

标签：const goal universe route 路径 lanelet 源码 Autoware auto

文章目录

• 前言
• 一、planning实现的几个功能
• • 1.规划起点到目标点的路线
  • 2.规划跟随路线的轨迹
  • 3.确保车辆不与障碍物发生碰撞
  • 4.确保车辆遵守交通规则
  • 5.规划车辆可行的轨迹
• 二、全局路径生成模块——mission_planner
• • 1.mission_planner.cpp
  • • 1.输入输出：
    • 2.代码流程：
    • 3.源码注释
  • 2.route_selector.cpp
  • • 1.代码流程
    • 2.输入输出
    • 3.源码注释
  • 3.goal_pose_visualizer
  • 4. service_utils.cpp
  • 5. utility_functions.cpp
  • 6. default_planner.cpp
  • • 1.代码流程图
    • • 1.核心逻辑
    • 2.输入输出
    • 3.源码注释
  • 7. arrival_checker.cpp
  • • 源码注释
• 总结

---

前言

规控不分家，control那边讲完了，这一系列将一下planning这部分。

提前设置一个问题，大家带着解决这个问题的思路去看下面的讲解：如何修改绕障轨迹

planing这部分在官方文档中已经给出了整体说明，肯定比我自己整理要好很多，见连接：https://autowarefoundation.github.io/autoware-documentation/galactic/design/autoware-architecture/planning/

这系列的文章便跟着官方说明文档，进行讲解。

补充一下文件启动流程：
1.planning_simulator.launch.xml
2.autoware.launch.xml
3.tier4_planning_component.launch.xml
4.planning.launch.xml
5.到了这里，和control那的模块节点启动不同，这里是继续启动了五个launch文件：mission_planning.launch.xml，scenario_planning.launch.xml，planning_validator.launch.xml，planning_evaluator.launch.xml，remaining_distance_time_calculator.launch.xml
6.在这五个launch文件里面，又分别启动了planning中别的launch文件。
启动流程就到这里了，再往下就没什么意思了。

---

一、planning实现的几个功能

在官方文档中，对planning实现的功能进行了简单说明，在此进行相关拓展。

1.规划起点到目标点的路线

目标：确定从车辆当前位置到目标目的地的最佳路径。

关键组件：

全局路径规划：使用高精度地图（如道路网络）计算最佳路线，考虑距离、交通状况和道路类型等因素。

路径点生成：生成一系列中间点（路径点），车辆需要依次经过这些点以到达目的地。

动态重规划：根据实时变化（如道路封闭、交通拥堵）不断更新路线。

在autoware中使用的路径规划算法是基于 Lanelet2 的图搜索算法——A*算法。

几种常见的路径搜索算法原理可以看我另外一篇博客。

2.规划跟随路线的轨迹

目标：生成一条平滑、可行驶的轨迹，使车辆能够沿着路线行驶。

关键组件：

局部轨迹规划：在短时间范围内生成详细的路径（如位置、速度、加速度）。

动态调整：根据障碍物、交通等动态因素实时调整轨迹。

平滑性：确保轨迹在运动学上是可行的（如避免急转弯、平滑的加速度曲线）。

3.确保车辆不与障碍物发生碰撞

目标：检测并避免与静态和动态障碍物（如行人、其他车辆、施工区域）发生碰撞。

关键组件：

障碍物检测：使用传感器（如LiDAR、摄像头、雷达）识别车辆周围的障碍物。

碰撞避免：规划与障碍物保持安全距离的轨迹。

风险评估：评估碰撞的可能性并相应调整轨迹。

4.确保车辆遵守交通规则

目标：确保车辆遵守交通法规，并在复杂环境中表现出可预测的行为。

关键组件：

交通信号灯遵守：检测并响应交通信号灯（如在红灯时停车，绿灯时通行）。

停车线和人行横道处理：在停车线前停车，并在人行横道前礼让行人。

车道遵守：遵循车道标线，避免非法变道，并遵守限速规定。

路权规则：在需要时礼让其他车辆或行人。

5.规划车辆可行的轨迹

目标：确保规划的轨迹在车辆的运动学和动力学上是可行的。

关键组件：

运动学可行性：确保车辆能够执行规划的轨迹，而不会超出其转弯半径、加速度或减速度限制。

动力学可行性：考虑车辆的动力学特性（如轮胎摩擦力、重量分布）以确保稳定性。

舒适性：规划平滑的轨迹以确保乘客舒适（如避免急加速或急刹车）。

二、全局路径生成模块——mission_planner

文件结构：文件夹下有七个cpp文件：
1.mission_planner.cpp：实现路径规划的主逻辑，负责生成和管理车辆的行驶路径。
2.route_selector.cpp：管理主路径（Main Route）和最小风险路径（MRM Route）的切换和恢复。
3. goal_pose_visualizer.cpp：可视化目标位姿，用于调试和监控。
4. service_utils.cpp：提供 ROS 服务调用的工具函数，处理异常和同步调用。
5. utility_functions.cpp：提供通用的工具函数，用于路径规划和管理的辅助操作。
6. default_planner.cpp：路径规划核心文件，实现默认的路径规划算法，基于 Lanelet2 地图生成路径。
7. arrival_checker.cpp：检查车辆是否到达目标点，并更新路径状态。

这部分的launch文件启动了三个节点：
mission_planner：主程序
route_selector
goal_pose_visualizer

接下来会依次介绍这几个文件功能。

主要关注：mission_planner.cpp和default_planner.cpp

1.mission_planner.cpp

1.输入输出：

输入

    订阅的输入：

        里程计 (~/input/odometry)：车辆当前的位置和速度信息。

        操作模式状态 (~/input/operation_mode_state)：车辆的操作模式（如自动驾驶模式是否启用）。

        矢量地图 (~/input/vector_map)：Lanelet2 格式的地图数据。

        修改后的目标点 (~/input/modified_goal)：用户或系统修改的目标点。

    服务请求：

        清除路径 (~/clear_route)：清除当前路径。

        设置 Lanelet 路径 (~/set_lanelet_route)：根据 Lanelet 地图生成路径。

        设置 Waypoint 路径 (~/set_waypoint_route)：根据一系列航点生成路径。

输出

    发布的输出：

        路径 (~/route)：规划好的路径（LaneletRoute 格式）。

        路径状态 (~/state)：当前路径的状态（如 UNSET、SET、ARRIVED 等）。

        路径标记 (~/debug/route_marker)：用于可视化的路径标记（如 RViz 中显示的路径）。

    服务响应：

        对路径操作（如设置路径、清除路径）的响应，包含操作是否成功的状态信息。

2.代码流程：

初始化
   |
   v
订阅输入数据（里程计、地图、操作模式等）
   |
   v
等待路径请求（服务或订阅）
   |
   v
路径规划（调用插件生成路径）
   |
   v
安全性检查（重新规划时）
   |
   v
发布路径和状态
   |
   v
检查是否到达目标点
   |
   v
更新路径状态

在这个流程中，会调用service_utils.cpp，route_selector.cpp，arrival_checker.cpp

3.源码注释

// Copyright 2019 Autoware Foundation
//
// 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 "mission_planner.hpp"

#include "service_utils.hpp"

#include <autoware_lanelet2_extension/utility/message_conversion.hpp>
#include <autoware_lanelet2_extension/utility/query.hpp>
#include <autoware_lanelet2_extension/utility/route_checker.hpp>
#include <autoware_lanelet2_extension/utility/utilities.hpp>

#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <lanelet2_core/geometry/LineString.h>

#include <algorithm>
#include <utility>

namespace autoware::mission_planner
{

// MissionPlanner 构造函数，初始化节点
MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
: Node("mission_planner", options),
  arrival_checker_(this),  // 初始化到达检查器
  plugin_loader_("autoware_mission_planner", "autoware::mission_planner::PlannerPlugin"),  // 插件加载器
  tf_buffer_(get_clock()),  // TF 缓冲区
  tf_listener_(tf_buffer_),  // TF 监听器
  odometry_(nullptr),  // 初始化里程计为空
  map_ptr_(nullptr)  // 初始化地图指针为空
{
  using std::placeholders::_1;
  using std::placeholders::_2;

  // 从参数服务器获取地图帧名称
  map_frame_ = declare_parameter<std::string>("map_frame");
  // 获取重新规划路径的时间阈值
  reroute_time_threshold_ = declare_parameter<double>("reroute_time_threshold");
  // 获取重新规划路径的最小长度
  minimum_reroute_length_ = declare_parameter<double>("minimum_reroute_length");

  // 创建默认路径规划器插件实例
  planner_ =
    plugin_loader_.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner");
  planner_->initialize(this);  // 初始化路径规划器

  // 设置 QoS 为持久化
  const auto durable_qos = rclcpp::QoS(1).transient_local();
  // 订阅里程计信息
  sub_odometry_ = create_subscription<Odometry>(
    "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1));
  // 订阅操作模式状态
  sub_operation_mode_state_ = create_subscription<OperationModeState>(
    "~/input/operation_mode_state", rclcpp::QoS(1),
    std::bind(&MissionPlanner::on_operation_mode_state, this, _1));
  // 订阅矢量地图
  sub_vector_map_ = create_subscription<LaneletMapBin>(
    "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1));
  // 发布路径标记
  pub_marker_ = create_publisher<MarkerArray>("~/debug/route_marker", durable_qos);

  // 注意：路由接口的回调组应该是互斥的
  // 订阅修改后的目标点
  sub_modified_goal_ = create_subscription<PoseWithUuidStamped>(
    "~/input/modified_goal", durable_qos, std::bind(&MissionPlanner::on_modified_goal, this, _1));
  // 创建清除路径服务
  srv_clear_route = create_service<ClearRoute>(
    "~/clear_route", service_utils::handle_exception(&MissionPlanner::on_clear_route, this));
  // 创建设置 Lanelet 路径服务
  srv_set_lanelet_route = create_service<SetLaneletRoute>(
    "~/set_lanelet_route",
    service_utils::handle_exception(&MissionPlanner::on_set_lanelet_route, this));
  // 创建设置 Waypoint 路径服务
  srv_set_waypoint_route = create_service<SetWaypointRoute>(
    "~/set_waypoint_route",
    service_utils::handle_exception(&MissionPlanner::on_set_waypoint_route, this));
  // 发布路径
  pub_route_ = create_publisher<LaneletRoute>("~/route", durable_qos);
  // 发布路径状态
  pub_state_ = create_publisher<RouteState>("~/state", durable_qos);

  // 节点初始化后，当准备好路由 API 时，将发布路由状态
  const auto period = rclcpp::Rate(10).period();
  data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); });

  // 配置日志记录器
  logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
}

// 发布位姿日志
void MissionPlanner::publish_pose_log(const Pose & pose, const std::string & pose_type)
{
  const auto & p = pose.position;
  RCLCPP_INFO(
    this->get_logger(), "%s pose - x: %f, y: %f, z: %f", pose_type.c_str(), p.x, p.y, p.z);
  const auto & quaternion = pose.orientation;
  RCLCPP_INFO(
    this->get_logger(), "%s orientation - qx: %f, qy: %f, qz: %f, qw: %f", pose_type.c_str(),
    quaternion.x, quaternion.y, quaternion.z, quaternion.w);
}

// 检查初始化状态
void MissionPlanner::check_initialization()
{
  auto logger = get_logger();
  auto clock = *get_clock();

  // 如果路径规划器未准备好，等待 Lanelet 地图
  if (!planner_->ready()) {
    RCLCPP_INFO_THROTTLE(logger, clock, 5000, "waiting lanelet map... Route API is not ready.");
    return;
  }
  // 如果里程计数据未收到，等待里程计
  if (!odometry_) {
    RCLCPP_INFO_THROTTLE(logger, clock, 5000, "waiting odometry... Route API is not ready.");
    return;
  }

  // 所有数据已准备好，API 可用
  RCLCPP_DEBUG(logger, "Route API is ready.");
  change_state(RouteState::UNSET);

  // 停止定时器回调
  data_check_timer_->cancel();
  data_check_timer_ = nullptr;
}

// 里程计回调函数
void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg)
{
  odometry_ = msg;

  // 注意：在其他状态下不要检查，因为目标可能会改变
  if (state_.state == RouteState::SET) {
    PoseStamped pose;
    pose.header = odometry_->header;
    pose.pose = odometry_->pose.pose;
    // 检查是否到达目标点
    if (arrival_checker_.is_arrived(pose)) {
      change_state(RouteState::ARRIVED);
    }
  }
}

// 操作模式状态回调函数
void MissionPlanner::on_operation_mode_state(const OperationModeState::ConstSharedPtr msg)
{
  operation_mode_state_ = msg;
}

// 地图回调函数
void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg)
{
  map_ptr_ = msg;
  lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
  lanelet::utils::conversion::fromBinMsg(*map_ptr_, lanelet_map_ptr_);
}

// 转换位姿到地图坐标系
Pose MissionPlanner::transform_pose(const Pose & pose, const Header & header)
{
  geometry_msgs::msg::TransformStamped transform;
  geometry_msgs::msg::Pose result;
  try {
    transform = tf_buffer_.lookupTransform(map_frame_, header.frame_id, tf2::TimePointZero);
    tf2::doTransform(pose, result, transform);
    return result;
  } catch (tf2::TransformException & error) {
    throw service_utils::TransformError(error.what());
  }
}

// 改变路径状态
void MissionPlanner::change_state(RouteState::_state_type state)
{
  state_.stamp = now();
  state_.state = state;
  pub_state_->publish(state_);
}

// 修改目标点回调函数
void MissionPlanner::on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg)
{
  RCLCPP_INFO(get_logger(), "Received modified goal.");

  if (state_.state != RouteState::SET) {
    RCLCPP_ERROR(get_logger(), "The route hasn't set yet. Cannot reroute.");
    return;
  }
  if (!planner_->ready()) {
    RCLCPP_ERROR(get_logger(), "The planner is not ready.");
    return;
  }
  if (!odometry_) {
    RCLCPP_ERROR(get_logger(), "The vehicle pose is not received.");
    return;
  }
  if (!current_route_) {
    RCLCPP_ERROR(get_logger(), "The route has not set yet.");
    return;
  }
  if (current_route_->uuid != msg->uuid) {
    RCLCPP_ERROR(get_logger(), "Goal uuid is incorrect.");
    return;
  }

  change_state(RouteState::REROUTING);
  const auto route = create_route(*msg);

  if (route.segments.empty()) {
    cancel_route();
    change_state(RouteState::SET);
    RCLCPP_ERROR(get_logger(), "The planned route is empty.");
  }

  change_route(route);
  change_state(RouteState::SET);
  RCLCPP_INFO(get_logger(), "Changed the route with the modified goal");
}

// 清除路径服务回调函数
void MissionPlanner::on_clear_route(
  const ClearRoute::Request::SharedPtr, const ClearRoute::Response::SharedPtr res)
{
  change_route();
  change_state(RouteState::UNSET);
  res->status.success = true;
}

// 设置 Lanelet 路径服务回调函数
void MissionPlanner::on_set_lanelet_route(
  const SetLaneletRoute::Request::SharedPtr req, const SetLaneletRoute::Response::SharedPtr res)
{
  using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response;
  const auto is_reroute = state_.state == RouteState::SET;

  if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state.");
  }
  if (!planner_->ready()) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready.");
  }
  if (!odometry_) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
  }
  if (is_reroute && !operation_mode_state_) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received.");
  }

  const bool is_autonomous_driving =
    operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS &&
                              operation_mode_state_->is_autoware_control_enabled
                          : false;

  if (is_reroute && is_autonomous_driving) {
    const auto reroute_availability = sub_reroute_availability_.takeData();
    if (!reroute_availability || !reroute_availability->availability) {
      throw service_utils::ServiceException(
        ResponseCode::ERROR_INVALID_STATE,
        "Cannot reroute as the planner is not in lane following.");
    }
  }

  change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING);
  const auto route = create_route(*req);

  if (route.segments.empty()) {
    cancel_route();
    change_state(is_reroute ? RouteState::SET : RouteState::UNSET);
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
  }

  if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) {
    cancel_route();
    change_state(RouteState::SET);
    throw service_utils::ServiceException(
      ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed.");
  }

  change_route(route);
  change_state(RouteState::SET);
  res->status.success = true;

  publish_pose_log(odometry_->pose.pose, "initial");
  publish_pose_log(req->goal_pose, "goal");
}

// 设置 Waypoint 路径服务回调函数
void MissionPlanner::on_set_waypoint_route(
  const SetWaypointRoute::Request::SharedPtr req, const SetWaypointRoute::Response::SharedPtr res)
{
  using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response;
  const auto is_reroute = state_.state == RouteState::SET;

  if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state.");
  }
  if (!planner_->ready()) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready.");
  }
  if (!odometry_) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
  }
  if (is_reroute && !operation_mode_state_) {
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received.");
  }

  const bool is_autonomous_driving =
    operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS &&
                              operation_mode_state_->is_autoware_control_enabled
                          : false;

  if (is_reroute && is_autonomous_driving) {
    const auto reroute_availability = sub_reroute_availability_.takeData();
    if (!reroute_availability || !reroute_availability->availability) {
      throw service_utils::ServiceException(
        ResponseCode::ERROR_INVALID_STATE,
        "Cannot reroute as the planner is not in lane following.");
    }
  }

  change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING);
  const auto route = create_route(*req);

  if (route.segments.empty()) {
    cancel_route();
    change_state(is_reroute ? RouteState::SET : RouteState::UNSET);
    throw service_utils::ServiceException(
      ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
  }

  if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) {
    cancel_route();
    change_state(RouteState::SET);
    throw service_utils::ServiceException(
      ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed.");
  }

  change_route(route);
  change_state(RouteState::SET);
  res->status.success = true;

  publish_pose_log(odometry_->pose.pose, "initial");
  publish_pose_log(req->goal_pose, "goal");
}

// 改变路径
void MissionPlanner::change_route()
{
  current_route_ = nullptr;
  planner_->clearRoute();
  arrival_checker_.set_goal();

  // TODO(Takagi, Isamu): 在这里发布一个空路径
  // pub_route_->publish();
  // pub_marker_->publish();
}

// 改变路径
void MissionPlanner::change_route(const LaneletRoute & route)
{
  PoseWithUuidStamped goal;
  goal.header = route.header;
  goal.pose = route.goal_pose;
  goal.uuid = route.uuid;

  current_route_ = std::make_shared<LaneletRoute>(route);
  planner_->updateRoute(route);
  arrival_checker_.set_goal(goal);

  pub_route_->publish(route);
  pub_marker_->publish(planner_->visualize(route));
}

// 取消路径
void MissionPlanner::cancel_route()
{
  // 恢复路径规划器状态
  if (current_route_) {
    planner_->updateRoute(*current_route_);
  }
}

// 创建 Lanelet 路径
LaneletRoute MissionPlanner::create_route(const SetLaneletRoute::Request & req)
{
  const auto & header = req.header;
  const auto & segments = req.segments;
  const auto & goal_pose = req.goal_pose;
  const auto & uuid = req.uuid;
  const auto & allow_goal_modification = req.allow_modification;

  return create_route(header, segments, goal_pose, uuid, allow_goal_modification);
}

// 创建 Waypoint 路径
LaneletRoute MissionPlanner::create_route(const SetWaypointRoute::Request & req)
{
  const auto & header = req.header;
  const auto & waypoints = req.waypoints;
  const auto & goal_pose = req.goal_pose;
  const auto & uuid = req.uuid;
  const auto & allow_goal_modification = req.allow_modification;

  return create_route(header, waypoints, goal_pose, uuid, allow_goal_modification);
}

// 创建路径
LaneletRoute MissionPlanner::create_route(const PoseWithUuidStamped & msg)
{
  const auto & header = msg.header;
  const auto & goal_pose = msg.pose;
  const auto & uuid = msg.uuid;
  const auto & allow_goal_modification = current_route_->allow_modification;

  return create_route(header, std::vector<Pose>(), goal_pose, uuid, allow_goal_modification);
}

// 创建 Lanelet 路径
LaneletRoute MissionPlanner::create_route(
  const Header & header, const std::vector<LaneletSegment> & segments, const Pose & goal_pose,
  const UUID & uuid, const bool allow_goal_modification)
{
  LaneletRoute route;
  route.header.stamp = header.stamp;
  route.header.frame_id = map_frame_;
  route.start_pose = odometry_->pose.pose;
  route.goal_pose = transform_pose(goal_pose, header);
  route.segments = segments;
  route.uuid = uuid;
  route.allow_modification = allow_goal_modification;
  return route;
}

// 创建 Waypoint 路径
LaneletRoute MissionPlanner::create_route(
  const Header & header, const std::vector<Pose> & waypoints, const Pose & goal_pose,
  const UUID & uuid, const bool allow_goal_modification)
{
  PlannerPlugin::RoutePoints points;
  points.push_back(odometry_->pose.pose);
  for (const auto & waypoint : waypoints) {
    points.push_back(transform_pose(waypoint, header));
  }
  points.push_back(transform_pose(goal_pose, header));

  LaneletRoute route = planner_->plan(points);
  route.header.stamp = header.stamp;
  route.header.frame_id = map_frame_;
  route.uuid = uuid;
  route.allow_modification = allow_goal_modification;
  return route;
}

// 检查重新规划路径的安全性
bool MissionPlanner::check_reroute_safety(
  const LaneletRoute & original_route, const LaneletRoute & target_route)
{
  if (
    original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ ||
    !lanelet_map_ptr_ || !odometry_) {
    RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Route, map or odometry is not set.");
    return false;
  }

  const auto current_velocity = odometry_->twist.twist.linear.x;

  // 如果车辆停止，不检查安全性
  if (current_velocity < 0.01) {
    return true;
  }

  auto hasSamePrimitives = [](
                             const std::vector<LaneletPrimitive> & original_primitives,
                             const std::vector<LaneletPrimitive> & target_primitives) {
    if (original_primitives.size() != target_primitives.size()) {
      return false;
    }

    for (const auto & primitive : original_primitives) {
      const auto has_same = [&](const auto & p) { return p.id == primitive.id; };
      const bool is_same =
        std::find_if(target_primitives.begin(), target_primitives.end(), has_same) !=
        target_primitives.end();
      if (!is_same) {
        return false;
      }
    }
    return true;
  };

  // =============================================================================================
  // 注意：目标路径是在车辆在原始路径上行驶时计算的，因此目标路径的第一条车道应在原始路径的车道中。
  // 所以共同段间隔匹配目标路径的开始。例外情况是如果车辆在交叉口车道上，getClosestLanelet() 可能不会返回原始路径中存在的车道。
  // 在这种情况下，共同段间隔不匹配目标车道的开始。
  // =============================================================================================
  const auto start_idx_opt =
    std::invoke([&]() -> std::optional<std::pair<size_t /* original */, size_t /* target */>> {
      for (size_t i = 0; i < original_route.segments.size(); ++i) {
        const auto & original_segment = original_route.segments.at(i).primitives;
        for (size_t j = 0; j < target_route.segments.size(); ++j) {
          const auto & target_segment = target_route.segments.at(j).primitives;
          if (hasSamePrimitives(original_segment, target_segment)) {
            return std::make_pair(i, j);
          }
        }
      }
      return std::nullopt;
    });
  if (!start_idx_opt.has_value()) {
    RCLCPP_ERROR(
      get_logger(), "Check reroute safety failed. Cannot find the start index of the route.");
    return false;
  }
  const auto [start_idx_original, start_idx_target] = start_idx_opt.value();

  // 找到与目标原语匹配的最后一个索引
  size_t end_idx_original = start_idx_original;
  size_t end_idx_target = start_idx_target;
  for (size_t i = 1; i < target_route.segments.size() - start_idx_target; ++i) {
    if (start_idx_original + i > original_route.segments.size() - 1) {
      break;
    }

    const auto & original_primitives =
      original_route.segments.at(start_idx_original + i).primitives;
    const auto & target_primitives = target_route.segments.at(start_idx_target + i).primitives;
    if (!hasSamePrimitives(original_primitives, target_primitives)) {
      break;
    }
    end_idx_original = start_idx_original + i;
    end_idx_target = start_idx_target + i;
  }

  // 在从主路径/MRM 到 MRM/主路径的第一次转换时，route_selector 请求的路径可能不是从当前车道开始的
  const bool ego_is_on_first_target_section = std::any_of(
    target_route.segments.front().primitives.begin(),
    target_route.segments.front().primitives.end(), [&](const auto & primitive) {
      const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id);
      return lanelet::utils::isInLanelet(target_route.start_pose, lanelet);
    });
  if (!ego_is_on_first_target_section) {
    RCLCPP_ERROR(
      get_logger(),
      "Check reroute safety failed. Ego is not on the first section of target route.");
    return false;
  }

  // 如果目标路径的前面不是共同段的前面，则预计目标路径的前面与另一条车道冲突
  double accumulated_length = 0.0;

  if (start_idx_target != 0 && start_idx_original > 1) {
    // 计算从当前位置到共同段开始的距离
    const auto current_pose = target_route.start_pose;
    const auto primitives = original_route.segments.at(start_idx_original - 1).primitives;
    lanelet::ConstLanelets start_lanelets;
    for (const auto & primitive : primitives) {
      const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id);
      start_lanelets.push_back(lanelet);
    }
    // 找到最接近的车道
    lanelet::ConstLanelet closest_lanelet;
    if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) {
      RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet.");
      return false;
    }

    const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline());
    const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position);
    const auto arc_coordinates = lanelet::geometry::toArcCoordinates(
      centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
    const double dist_to_current_pose = arc_coordinates.length;
    const double lanelet_length = lanelet::utils::getLaneletLength2d(closest_lanelet);
    accumulated_length = lanelet_length - dist_to_current_pose;
  } else {
    // 计算从当前位置到当前车道末端的距离
    const auto current_pose = target_route.start_pose;
    const auto primitives = original_route.segments.at(start_idx_original).primitives;
    lanelet::ConstLanelets start_lanelets;
    for (const auto & primitive : primitives) {
      const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id);
      start_lanelets.push_back(lanelet);
    }
    // 找到最接近的车道
    lanelet::ConstLanelet closest_lanelet;
    if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) {
      RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet.");
      return false;
    }

    const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline());
    const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position);
    const auto arc_coordinates = lanelet::geometry::toArcCoordinates(
      centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
    const double dist_to_current_pose = arc_coordinates.length;
    const double lanelet_length = lanelet::utils::getLaneletLength2d(closest_lanelet);
    accumulated_length = lanelet_length - dist_to_current_pose;
  }

  // 计算从 start_idx+1 到 end_idx 的距离
  for (size_t i = start_idx_original + 1; i <= end_idx_original; ++i) {
    const auto primitives = original_route.segments.at(i).primitives;
    if (primitives.empty()) {
      break;
    }

    std::vector<double> lanelets_length(primitives.size());
    for (size_t primitive_idx = 0; primitive_idx < primitives.size(); ++primitive_idx) {
      const auto & primitive = primitives.at(primitive_idx);
      const auto & lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id);
      lanelets_length.at(primitive_idx) = (lanelet::utils::getLaneletLength2d(lanelet));
    }
    accumulated_length += *std::min_element(lanelets_length.begin(), lanelets_length.end());
  }

  // 检查目标是否在目标终端车道内
  const auto & target_end_primitives = target_route.segments.at(end_idx_target).primitives;
  const auto & target_goal = target_route.goal_pose;
  for (const auto & target_end_primitive : target_end_primitives) {
    const auto lanelet = lanelet_map_ptr_->laneletLayer.get(target_end_primitive.id);
    if (lanelet::utils::isInLanelet(target_goal, lanelet)) {
      const auto target_goal_position =
        lanelet::utils::conversion::toLaneletPoint(target_goal.position);
      const double dist_to_goal = lanelet::geometry::toArcCoordinates(
                                    lanelet::utils::to2D(lanelet.centerline()),
                                    lanelet::utils::to2D(target_goal_position).basicPoint())
                                    .length;
      const double target_lanelet_length = lanelet::utils::getLaneletLength2d(lanelet);
      // 注意：`accumulated_length` 这里包含整个 target_end_primitive 的长度，因此需要减去从目标到 target_end_primitive 末端的剩余距离
      const double remaining_dist = target_lanelet_length - dist_to_goal;
      accumulated_length = std::max(accumulated_length - remaining_dist, 0.0);
      break;
    }
  }

  // 检查安全性
  const double safety_length =
    std::max(current_velocity * reroute_time_threshold_, minimum_reroute_length_);
  if (accumulated_length > safety_length) {
    return true;
  }

  RCLCPP_WARN(
    get_logger(),
    "Length of lane where original and B target (= %f) is less than safety length (= %f), so "
    "reroute is not safe.",
    accumulated_length, safety_length);
  return false;
}
}  // namespace autoware::mission_planner

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mission_planner::MissionPlanner)

2.route_selector.cpp

负责管理主路径（Main Route）和应急路径规划模块（MRM Route）的切换和恢复。

1.代码流程

初始化
   |
   v
订阅路径规划器的状态和路径
   |
   v
监听主路径和应急路径规划模块的服务请求
   |
   v
根据当前状态（是否处于应急风险模式）决定是否转发请求
   |
   v
在应急模式下中断主路径，激活应急路径规划模块
   |
   v
应急模式结束后，尝试恢复主路径
   |
   v
发布路径状态和路径

在功能实现中，依赖于service_utils.cpp 中的异常处理功能。

2.输入输出

输入

    服务请求：

        主路径接口：

            清除路径 (~/main/clear_route)。

            设置 Waypoint 路径 (~/main/set_waypoint_route)。

            设置 Lanelet 路径 (~/main/set_lanelet_route)。

        最小风险路径接口：

            清除路径 (~/mrm/clear_route)。

            设置 Waypoint 路径 (~/mrm/set_waypoint_route)。

            设置 Lanelet 路径 (~/mrm/set_lanelet_route)。

    订阅的输入：

        路径规划器的状态 (~/planner/state)。

        路径规划器的路径 (~/planner/route)。

输出

    发布的输出：

        主路径接口：

            路径状态 (~/main/state)。

            路径 (~/main/route)。

        最小风险路径接口：

            路径状态 (~/mrm/state)。

            路径 (~/mrm/route)。

    服务响应：

        对路径操作（如设置路径、清除路径）的响应，包含操作是否成功的状态信息。

3.源码注释

// Copyright 2024 The Autoware Contributors
//
// 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 "route_selector.hpp"

#include "service_utils.hpp"

#include <array>
#include <memory>
#include <random>

namespace autoware::mission_planner::uuid
{

// 生成一个随机的 16 字节 UUID
std::array<uint8_t, 16> generate_random_id()
{
  // 使用随机设备初始化随机数引擎
  static std::independent_bits_engine<std::mt19937, 8, uint8_t> engine(std::random_device{}());
  std::array<uint8_t, 16> id;
  // 生成 16 字节的随机数作为 UUID
  std::generate(id.begin(), id.end(), std::ref(engine));
  return id;
}

// 如果 UUID 为空，则生成一个随机 UUID
UUID generate_if_empty(const UUID & uuid)
{
  // 定义一个全零的 UUID
  constexpr std::array<uint8_t, 16> zero_uuid = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

  UUID result;
  // 如果输入的 UUID 是全零，则生成一个随机 UUID，否则使用输入的 UUID
  result.uuid = (uuid.uuid == zero_uuid) ? generate_random_id() : uuid.uuid;
  return result;
}

}  // namespace autoware::mission_planner::uuid

namespace autoware::mission_planner
{

// RouteInterface 构造函数，初始化时钟和状态
RouteInterface::RouteInterface(rclcpp::Clock::SharedPtr clock)
{
  clock_ = clock;
  state_.state = RouteState::UNKNOWN; // 初始状态为 UNKNOWN
}

// 获取当前路径状态
RouteState::_state_type RouteInterface::get_state() const
{
  return state_.state;
}

// 获取当前路径
std::optional<LaneletRoute> RouteInterface::get_route() const
{
  return route_;
}

// 清除当前路径
void RouteInterface::change_route()
{
  route_ = std::nullopt;
}

// 改变路径状态，并发布状态
void RouteInterface::change_state(RouteState::_state_type state)
{
  state_.stamp = clock_->now(); // 更新时间戳
  state_.state = state; // 更新状态
  pub_state_->publish(state_); // 发布状态
}

// 更新路径状态，并发布状态
void RouteInterface::update_state(const RouteState & state)
{
  state_ = state; // 更新状态
  pub_state_->publish(state_); // 发布状态
}

// 更新路径，并发布路径
void RouteInterface::update_route(const LaneletRoute & route)
{
  route_ = route; // 更新路径
  pub_route_->publish(route); // 发布路径
}

// RouteSelector 构造函数，初始化主路径和最小风险路径接口
RouteSelector::RouteSelector(const rclcpp::NodeOptions & options)
: Node("route_selector", options), main_(get_clock()), mrm_(get_clock())
{
  using std::placeholders::_1;
  using std::placeholders::_2;
  const auto service_qos = rmw_qos_profile_services_default; // 服务 QoS
  const auto durable_qos = rclcpp::QoS(1).transient_local(); // 持久化 QoS

  // 初始化主路径接口
  main_.srv_clear_route = create_service<ClearRoute>(
    "~/main/clear_route",
    service_utils::handle_exception(&RouteSelector::on_clear_route_main, this));
  main_.srv_set_waypoint_route = create_service<SetWaypointRoute>(
    "~/main/set_waypoint_route",
    service_utils::handle_exception(&RouteSelector::on_set_waypoint_route_main, this));
  main_.srv_set_lanelet_route = create_service<SetLaneletRoute>(
    "~/main/set_lanelet_route",
    service_utils::handle_exception(&RouteSelector::on_set_lanelet_route_main, this));
  main_.pub_state_ = create_publisher<RouteState>("~/main/state", durable_qos); // 发布主路径状态
  main_.pub_route_ = create_publisher<LaneletRoute>("~/main/route", durable_qos); // 发布主路径

  // 初始化最小风险路径接口
  mrm_.srv_clear_route = create_service<ClearRoute>(
    "~/mrm/clear_route", service_utils::handle_exception(&RouteSelector::on_clear_route_mrm, this));
  mrm_.srv_set_waypoint_route = create_service<SetWaypointRoute>(
    "~/mrm/set_waypoint_route",
    service_utils::handle_exception(&RouteSelector::on_set_waypoint_route_mrm, this));
  mrm_.srv_set_lanelet_route = create_service<SetLaneletRoute>(
    "~/mrm/set_lanelet_route",
    service_utils::handle_exception(&RouteSelector::on_set_lanelet_route_mrm, this));
  mrm_.pub_state_ = create_publisher<RouteState>("~/mrm/state", durable_qos); // 发布最小风险路径状态
  mrm_.pub_route_ = create_publisher<LaneletRoute>("~/mrm/route", durable_qos); // 发布最小风险路径

  // 初始化路径规划器接口
  group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // 创建回调组
  cli_clear_route_ = create_client<ClearRoute>("~/planner/clear_route", service_qos, group_); // 清除路径客户端
  cli_set_lanelet_route_ =
    create_client<SetLaneletRoute>("~/planner/set_lanelet_route", service_qos, group_); // 设置 Lanelet 路径客户端
  cli_set_waypoint_route_ =
    create_client<SetWaypointRoute>("~/planner/set_waypoint_route", service_qos, group_); // 设置 Waypoint 路径客户端
  sub_state_ = create_subscription<RouteState>(
    "~/planner/state", durable_qos, std::bind(&RouteSelector::on_state, this, _1)); // 订阅路径状态
  sub_route_ = create_subscription<LaneletRoute>(
    "~/planner/route", durable_qos, std::bind(&RouteSelector::on_route, this, _1)); // 订阅路径

  // 设置初始状态
  main_.change_state(RouteState::INITIALIZING); // 主路径状态为初始化中
  mrm_.change_state(RouteState::INITIALIZING); // 最小风险路径状态为初始化中
  initialized_ = false; // 初始化标志为 false
  mrm_operating_ = false; // 最小风险模式标志为 false
  main_request_ = std::monostate{}; // 主路径请求初始化为空
}

// 路径状态回调函数
void RouteSelector::on_state(const RouteState::ConstSharedPtr msg)
{
  // 如果路径规划器状态为 UNSET 且未初始化，则初始化主路径和最小风险路径状态
  if (msg->state == RouteState::UNSET && !initialized_) {
    main_.change_state(RouteState::UNSET);
    mrm_.change_state(RouteState::UNSET);
    initialized_ = true; // 标记为已初始化
  }

  // 根据是否处于最小风险模式，更新主路径或最小风险路径状态
  (mrm_operating_ ? mrm_ : main_).update_state(*msg);
}

// 路径回调函数
void RouteSelector::on_route(const LaneletRoute::ConstSharedPtr msg)
{
  // 根据是否处于最小风险模式，更新主路径或最小风险路径
  (mrm_operating_ ? mrm_ : main_).update_route(*msg);
}

// 清除主路径服务回调函数
void RouteSelector::on_clear_route_main(
  ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res)
{
  // 保存请求并清除旧路径，以便从最小风险模式恢复
  main_request_ = std::monostate{};
  main_.change_route();

  // 如果处于最小风险模式，仅改变状态
  if (mrm_operating_) {
    main_.change_state(RouteState::UNSET);
    res->status.success = true;
    return;
  }

  // 如果未处于最小风险模式，则转发请求
  res->status = service_utils::sync_call(cli_clear_route_, req);
}

// 设置 Waypoint 主路径服务回调函数
void RouteSelector::on_set_waypoint_route_main(
  SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res)
{
  // 保存请求并清除旧路径，以便从最小风险模式恢复
  req->uuid = uuid::generate_if_empty(req->uuid); // 如果 UUID 为空，则生成一个随机 UUID
  main_request_ = req;
  main_.change_route();

  // 如果处于最小风险模式，仅改变状态
  if (mrm_operating_) {
    main_.change_state(RouteState::INTERRUPTED);
    res->status.success = true;
    return;
  }

  // 如果未处于最小风险模式，则转发请求
  res->status = service_utils::sync_call(cli_set_waypoint_route_, req);
}

// 设置 Lanelet 主路径服务回调函数
void RouteSelector::on_set_lanelet_route_main(
  SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res)
{
  // 保存请求并清除旧路径，以便从最小风险模式恢复
  req->uuid = uuid::generate_if_empty(req->uuid); // 如果 UUID 为空，则生成一个随机 UUID
  main_request_ = req;
  main_.change_route();

  // 如果处于最小风险模式，仅改变状态
  if (mrm_operating_) {
    main_.change_state(RouteState::INTERRUPTED);
    res->status.success = true;
    return;
  }

  // 如果未处于最小风险模式，则转发请求
  res->status = service_utils::sync_call(cli_set_lanelet_route_, req);
}

// 清除最小风险路径服务回调函数
void RouteSelector::on_clear_route_mrm(
  ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res)
{
  // 尝试恢复主路径
  res->status = resume_main_route(req);

  // 如果恢复成功，则退出最小风险模式
  if (res->status.success) {
    mrm_operating_ = false;
    mrm_.change_state(RouteState::UNSET);
  }
}

// 设置 Waypoint 最小风险路径服务回调函数
void RouteSelector::on_set_waypoint_route_mrm(
  SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res)
{
  // 如果 UUID 为空，则生成一个随机 UUID
  req->uuid = uuid::generate_if_empty(req->uuid);
  // 转发请求
  res->status = service_utils::sync_call(cli_set_waypoint_route_, req);

  // 如果请求成功，则进入最小风险模式
  if (res->status.success) {
    mrm_operating_ = true;
    if (main_.get_state() != RouteState::UNSET) {
      main_.change_state(RouteState::INTERRUPTED); // 中断主路径
    }
  }
}

// 设置 Lanelet 最小风险路径服务回调函数
void RouteSelector::on_set_lanelet_route_mrm(
  SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res)
{
  // 如果 UUID 为空，则生成一个随机 UUID
  req->uuid = uuid::generate_if_empty(req->uuid);
  // 转发请求
  res->status = service_utils::sync_call(cli_set_lanelet_route_, req);

  // 如果请求成功，则进入最小风险模式
  if (res->status.success) {
    mrm_operating_ = true;
    if (main_.get_state() != RouteState::UNSET) {
      main_.change_state(RouteState::INTERRUPTED); // 中断主路径
    }
  }
}

// 恢复主路径
ResponseStatus RouteSelector::resume_main_route(ClearRoute::Request::SharedPtr req)
{
  // 创建一个 Lanelet 路径请求
  const auto create_lanelet_request = [](const LaneletRoute & route) {
    const auto r = std::make_shared<SetLaneletRoute::Request>();
    r->header = route.header;
    r->goal_pose = route.goal_pose;
    r->segments = route.segments;
    r->uuid = route.uuid;
    r->allow_modification = route.allow_modification;
    return r;
  };

  // 创建一个 Waypoint 路径请求
  const auto create_goal_request = [](const auto & request) {
    const auto r = std::make_shared<SetWaypointRoute::Request>();
    r->header = request->header;
    r->goal_pose = request->goal_pose;
    r->uuid = request->uuid;
    r->allow_modification = request->allow_modification;
    return r;
  };

  // 如果没有主路径请求，则清除路径
  if (std::holds_alternative<std::monostate>(main_request_)) {
    return service_utils::sync_call(cli_clear_route_, req);
  }

  // 如果有主路径请求，则尝试恢复主路径
  if (const auto route = main_.get_route()) {
    const auto r = create_lanelet_request(route.value());
    const auto status = service_utils::sync_call(cli_set_lanelet_route_, r);
    if (status.success) return status;
  }

  // 如果恢复失败，则使用目标点重新规划主路径
  if (const auto request = std::get_if<WaypointRequest>(&main_request_)) {
    const auto r = create_goal_request(*request);
    return service_utils::sync_call(cli_set_waypoint_route_, r);
  }
  if (const auto request = std::get_if<LaneletRequest>(&main_request_)) {
    const auto r = create_goal_request(*request);
    return service_utils::sync_call(cli_set_waypoint_route_, r);
  }
  throw std::logic_error("route_selector: unknown main route request");
}

}  // namespace autoware::mission_planner

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mission_planner::RouteSelector)

3.goal_pose_visualizer

这个主要作用是负责可视化目标位置。它订阅路由消息并发布目标位置的可视化信息。

主要和与 mission_planner.cpp 和 route_selector.cpp 交互，接收路由信息并发布目标位置。

// Copyright 2020 Tier IV, Inc. 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.

#ifndef GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_
#define GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_

#include <rclcpp/rclcpp.hpp> // ROS 2 C++ 客户端库

#include <autoware_planning_msgs/msg/lanelet_route.hpp> // Lanelet 路径消息类型
#include <geometry_msgs/msg/pose_stamped.hpp> // 位姿消息类型

namespace autoware::mission_planner
{

// GoalPoseVisualizer 类，用于可视化目标位姿
class GoalPoseVisualizer : public rclcpp::Node
{
public:
  // 构造函数，接受节点选项
  explicit GoalPoseVisualizer(const rclcpp::NodeOptions & node_options);

private:
  // 订阅 Lanelet 路径的订阅器
  rclcpp::Subscription<autoware_planning_msgs::msg::LaneletRoute>::SharedPtr sub_route_;
  // 发布目标位姿的发布器
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_goal_pose_;

  // Lanelet 路径的回调函数
  void echo_back_route_callback(
    const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr msg);
};

}  // namespace autoware::mission_planner

#endif  // GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_

4. service_utils.cpp

#include "service_utils.hpp" // 引入服务工具头文件

#include <string> // 引入字符串库

namespace service_utils // 命名空间：service_utils
{

// 创建一个表示服务未准备好的异常
ServiceException ServiceUnready(const std::string & message)
{
  // 返回一个 ServiceException 对象，状态为 SERVICE_UNREADY，附带错误信息
  return ServiceException(ResponseStatus::SERVICE_UNREADY, message, false);
}

// 创建一个表示坐标转换错误的异常
ServiceException TransformError(const std::string & message)
{
  // 返回一个 ServiceException 对象，状态为 TRANSFORM_ERROR，附带错误信息
  return ServiceException(ResponseStatus::TRANSFORM_ERROR, message, false);
};

}  // namespace service_utils

5. utility_functions.cpp

实现了四个工具函数：
convert_linear_ring_to_polygon：

    将线性环（LinearRing）转换为多边形（Polygon）。

    用于处理车辆或障碍物的几何形状。

insert_marker_array：

    将一个 MarkerArray 插入到另一个 MarkerArray 中。

    用于合并可视化数据。

convertCenterlineToPoints：

    将 Lanelet 的中心线转换为点集。

    用于提取车道中心线的几何信息。

convertBasicPoint3dToPose：

    将 Lanelet 的 BasicPoint3d 转换为 Pose，并设置方向。

    用于生成目标位姿或路径点。

// Copyright 2019-2024 Autoware Foundation
//
// 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 "utility_functions.hpp" // 引入工具函数头文件

#include <boost/geometry.hpp> // 引入 Boost Geometry 库，用于几何计算

#include <lanelet2_core/geometry/Lanelet.h> // 引入 Lanelet2 库，用于处理车道数据

// 将线性环（LinearRing）转换为多边形（Polygon）
autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon(
  autoware::universe_utils::LinearRing2d footprint)
{
  autoware::universe_utils::Polygon2d footprint_polygon; // 创建一个多边形对象
  // 将线性环的顶点依次添加到多边形的外环中
  boost::geometry::append(footprint_polygon.outer(), footprint[0]);
  boost::geometry::append(footprint_polygon.outer(), footprint[1]);
  boost::geometry::append(footprint_polygon.outer(), footprint[2]);
  boost::geometry::append(footprint_polygon.outer(), footprint[3]);
  boost::geometry::append(footprint_polygon.outer(), footprint[4]);
  boost::geometry::append(footprint_polygon.outer(), footprint[5]);
  boost::geometry::correct(footprint_polygon); // 修正多边形的几何结构
  return footprint_polygon; // 返回转换后的多边形
}

// 将一个 MarkerArray 插入到另一个 MarkerArray 中
void insert_marker_array(
  visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2)
{
  // 将 a2 的 markers 插入到 a1 的 markers 末尾
  a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
}

// 将 Lanelet 的中心线转换为点集
std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet)
{
  std::vector<geometry_msgs::msg::Point> centerline_points; // 创建一个点集
  // 遍历 Lanelet 的中心线，将每个点转换为 geometry_msgs::msg::Point
  for (const auto & point : lanelet.centerline()) {
    geometry_msgs::msg::Point center_point;
    center_point.x = point.basicPoint().x(); // 设置点的 x 坐标
    center_point.y = point.basicPoint().y(); // 设置点的 y 坐标
    center_point.z = point.basicPoint().z(); // 设置点的 z 坐标
    centerline_points.push_back(center_point); // 将点添加到点集中
  }
  return centerline_points; // 返回转换后的点集
}

// 将 Lanelet 的 BasicPoint3d 转换为 Pose，并设置方向
geometry_msgs::msg::Pose convertBasicPoint3dToPose(
  const lanelet::BasicPoint3d & point, const double lane_yaw)
{
  // 创建一个 Pose 对象
  geometry_msgs::msg::Pose pose;
  // 设置 Pose 的位置
  pose.position.x = point.x(); // 设置 x 坐标
  pose.position.y = point.y(); // 设置 y 坐标
  pose.position.z = point.z(); // 设置 z 坐标

  // 设置 Pose 的方向（四元数），根据给定的 yaw 角度
  pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw);

  return pose; // 返回转换后的 Pose
}

6. default_planner.cpp

这部分是路径规划的核心。
是一个基于 Lanelet2 地图的默认路径规划器，负责根据起点和目标点生成车辆的行驶路径。

1.代码流程图

初始化
   |
   v
加载地图
   |
   v
等待路径规划请求
   |
   v
生成路径
   |
   v
检查目标点有效性
   |
   v
校正目标点高度
   |
   v
返回规划的路径
   |
   v
发布可视化标记

1.核心逻辑

其中的核心函数：PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points);
其中路径生成的核心调用是：route_handler_.planPathLaneletsBetweenCheckpoints(
start_check_point, goal_check_point, &path_lanelets, param_.consider_no_drivable_lanes);

参数说明：

参数说明：

    start_check_point：起点位姿。

    goal_check_point：目标点位姿。

    path_lanelets：输出的路径车道。

    param_.consider_no_drivable_lanes：是否考虑不可行驶车道。

主要逻辑为：

    输入：

        起点和目标点的位姿。

        Lanelet2 地图数据。

    路径生成步骤：

        查找起点和目标点所在的车道：

            使用 Lanelet2 的查询工具（如 getClosestLanelet）找到起点和目标点所在的车道。

        搜索路径：

            使用图搜索算法（如 Dijkstra 或 A*）在 Lanelet2 地图的车道网络中搜索从起点车道到目标车道的路径。

        生成路径车道：

            将搜索到的车道序列转换为路径车道（lanelet::ConstLanelets）。

    路径优化：

        对生成的车道序列进行优化，确保路径的平滑性和连续性。

2.输入输出

输入

    地图数据：

        通过订阅 ~/input/vector_map 话题或直接传入 LaneletMapBin 消息，加载 Lanelet2 地图。

    路径规划请求：

        通过 plan 函数传入的 RoutePoints（起点和目标点的位姿列表）。

    车辆信息：

        通过 VehicleInfo 获取车辆的几何信息（如车轮距、悬垂等）。

    参数配置：

        通过 ROS 参数服务器配置路径规划器的行为（如目标点角度阈值、是否启用目标点位姿校正等）。

输出

    规划的路径：

        返回 LaneletRoute 消息，包含路径的起点、目标点和路径段。

    可视化标记：

        发布 MarkerArray 消息，用于在 RViz 中可视化路径和目标点足迹。

    调试信息：

        发布目标点足迹的可视化标记（~/debug/goal_footprint 话题）。

3.源码注释

// Copyright 2019-2024 Autoware Foundation
//
// 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 "default_planner.hpp" // 引入默认路径规划器头文件

#include "utility_functions.hpp" // 引入工具函数头文件

#include <autoware/motion_utils/trajectory/trajectory.hpp> // 引入运动规划工具库
#include <autoware/route_handler/route_handler.hpp> // 引入路径处理器库
#include <autoware/universe_utils/geometry/geometry.hpp> // 引入几何工具库
#include <autoware/universe_utils/math/normalization.hpp> // 引入数学工具库（归一化）
#include <autoware/universe_utils/math/unit_conversion.hpp> // 引入数学工具库（单位转换）
#include <autoware/universe_utils/ros/marker_helper.hpp> // 引入 ROS 可视化工具库
#include <autoware_lanelet2_extension/utility/message_conversion.hpp> // 引入 Lanelet2 消息转换工具
#include <autoware_lanelet2_extension/utility/query.hpp> // 引入 Lanelet2 查询工具
#include <autoware_lanelet2_extension/utility/utilities.hpp> // 引入 Lanelet2 工具库
#include <autoware_lanelet2_extension/visualization/visualization.hpp> // 引入 Lanelet2 可视化工具
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp> // 引入车辆信息工具库

#include <boost/geometry/algorithms/difference.hpp> // 引入 Boost Geometry 差异算法
#include <boost/geometry/algorithms/is_empty.hpp> // 引入 Boost Geometry 空集判断算法

#include <lanelet2_core/Forward.h> // 引入 Lanelet2 核心库
#include <lanelet2_core/LaneletMap.h> // 引入 Lanelet2 地图库
#include <lanelet2_core/geometry/BoundingBox.h> // 引入 Lanelet2 几何库（边界框）
#include <lanelet2_core/geometry/Lanelet.h> // 引入 Lanelet2 几何库（车道）
#include <tf2/utils.h> // 引入 TF2 工具库

#include <limits> // 引入数值极限库
#include <vector> // 引入向量库

namespace
{
using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>; // 定义路径段类型

// 检查点是否在车道内
bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point)
{
  // 计算点与车道多边形的最小距离
  const auto distance = boost::geometry::distance(
    lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(point).basicPoint());
  constexpr double th_distance = std::numeric_limits<double>::epsilon(); // 距离阈值
  return distance < th_distance; // 如果距离小于阈值，则点在车道内
}

// 检查点是否在停车位内
bool is_in_parking_space(
  const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point)
{
  for (const auto & parking_space : parking_spaces) {
    lanelet::ConstPolygon3d parking_space_polygon;
    // 将停车位线串转换为多边形
    if (!lanelet::utils::lineStringWithWidthToPolygon(parking_space, &parking_space_polygon)) {
      continue;
    }

    // 计算点与停车位多边形的最小距离
    const double distance = boost::geometry::distance(
      lanelet::utils::to2D(parking_space_polygon).basicPolygon(),
      lanelet::utils::to2D(point).basicPoint());
    constexpr double th_distance = std::numeric_limits<double>::epsilon(); // 距离阈值
    if (distance < th_distance) {
      return true; // 如果距离小于阈值，则点在停车位内
    }
  }
  return false; // 点不在任何停车位内
}

// 检查点是否在停车场内
bool is_in_parking_lot(
  const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point)
{
  for (const auto & parking_lot : parking_lots) {
    // 计算点与停车场多边形的最小距离
    const double distance = boost::geometry::distance(
      lanelet::utils::to2D(parking_lot).basicPolygon(), lanelet::utils::to2D(point).basicPoint());
    constexpr double th_distance = std::numeric_limits<double>::epsilon(); // 距离阈值
    if (distance < th_distance) {
      return true; // 如果距离小于阈值，则点在停车场内
    }
  }
  return false; // 点不在任何停车场内
}

// 将目标点投影到地图上，并返回其高度
double project_goal_to_map(
  const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point)
{
  // 生成车道的精细中心线
  const lanelet::ConstLineString3d center_line =
    lanelet::utils::generateFineCenterline(lanelet_component);
  // 将目标点投影到中心线上
  lanelet::BasicPoint3d project = lanelet::geometry::project(center_line, goal_point.basicPoint());
  return project.z(); // 返回投影点的高度
}

// 获取最接近中心线的位姿
geometry_msgs::msg::Pose get_closest_centerline_pose(
  const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point,
  autoware::vehicle_info_utils::VehicleInfo vehicle_info)
{
  lanelet::Lanelet closest_lanelet;
  // 获取最接近的车道
  if (!lanelet::utils::query::getClosestLaneletWithConstrains(
        road_lanelets, point, &closest_lanelet, 0.0)) {
    // 如果点不在任何车道内，则返回原始点
    return point;
  }

  // 生成车道的精细中心线
  const auto refined_center_line = lanelet::utils::generateFineCenterline(closest_lanelet, 1.0);
  closest_lanelet.setCenterline(refined_center_line);

  // 计算车道的偏航角
  const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position);

  // 找到最接近的点
  const auto nearest_idx = autoware::motion_utils::findNearestIndex(
    convertCenterlineToPoints(closest_lanelet), point.position);
  const auto nearest_point = closest_lanelet.centerline()[nearest_idx];

  // 根据车辆的左右悬垂调整点的位置
  const auto shift_length = (vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0;
  const auto delta_x = -shift_length * std::sin(lane_yaw);
  const auto delta_y = shift_length * std::cos(lane_yaw);

  lanelet::BasicPoint3d refined_point(
    nearest_point.x() + delta_x, nearest_point.y() + delta_y, nearest_point.z());

  // 将调整后的点转换为位姿
  return convertBasicPoint3dToPose(refined_point, lane_yaw);
}

}  // anonymous namespace

namespace autoware::mission_planner::lanelet2
{

// 初始化默认路径规划器的通用部分
void DefaultPlanner::initialize_common(rclcpp::Node * node)
{
  is_graph_ready_ = false; // 标记图未准备好
  node_ = node; // 保存节点指针

  const auto durable_qos = rclcpp::QoS(1).transient_local();
  // 创建目标点足迹的调试标记发布器
  pub_goal_footprint_marker_ =
    node_->create_publisher<MarkerArray>("~/debug/goal_footprint", durable_qos);

  // 获取车辆信息
  vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo();
  // 声明参数
  param_.goal_angle_threshold_deg = node_->declare_parameter<double>("goal_angle_threshold_deg");
  param_.enable_correct_goal_pose = node_->declare_parameter<bool>("enable_correct_goal_pose");
  param_.consider_no_drivable_lanes = node_->declare_parameter<bool>("consider_no_drivable_lanes");
  param_.check_footprint_inside_lanes =
    node_->declare_parameter<bool>("check_footprint_inside_lanes");
}

// 初始化默认路径规划器（通过订阅地图）
void DefaultPlanner::initialize(rclcpp::Node * node)
{
  initialize_common(node);
  // 创建地图订阅器
  map_subscriber_ = node_->create_subscription<LaneletMapBin>(
    "~/input/vector_map", rclcpp::QoS{10}.transient_local(),
    std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1));
}

// 初始化默认路径规划器（通过传入地图消息）
void DefaultPlanner::initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg)
{
  initialize_common(node);
  map_callback(msg); // 直接处理地图消息
}

// 检查路径规划器是否准备好
bool DefaultPlanner::ready() const
{
  return is_graph_ready_; // 返回图是否准备好的标志
}

// 地图回调函数
void DefaultPlanner::map_callback(const LaneletMapBin::ConstSharedPtr msg)
{
  route_handler_.setMap(*msg); // 设置路径处理器中的地图
  is_graph_ready_ = true; // 标记图已准备好
}

// 可视化路径
PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) const
{
  lanelet::ConstLanelets route_lanelets; // 路径车道
  lanelet::ConstLanelets end_lanelets; // 结束车道
  lanelet::ConstLanelets goal_lanelets; // 目标车道

  // 遍历路径段，提取车道信息
  for (const auto & route_section : route.segments) {
    for (const auto & lane_id : route_section.primitives) {
      auto lanelet = route_handler_.getLaneletsFromId(lane_id.id);
      route_lanelets.push_back(lanelet);
      if (route_section.preferred_primitive.id == lane_id.id) {
        goal_lanelets.push_back(lanelet); // 目标车道
      } else {
        end_lanelets.push_back(lanelet); // 结束车道
      }
    }
  }

  // 定义颜色
  const std_msgs::msg::ColorRGBA cl_route =
    autoware::universe_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15);
  const std_msgs::msg::ColorRGBA cl_ll_borders =
    autoware::universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999);
  const std_msgs::msg::ColorRGBA cl_end =
    autoware::universe_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05);
  const std_msgs::msg::ColorRGBA cl_goal =
    autoware::universe_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05);

  // 创建路径标记数组
  visualization_msgs::msg::MarkerArray route_marker_array;
  insert_marker_array(
    &route_marker_array,
    lanelet::visualization::laneletsBoundaryAsMarkerArray(route_lanelets, cl_ll_borders, false));
  insert_marker_array(
    &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray(
                           "route_lanelets", route_lanelets, cl_route));
  insert_marker_array(
    &route_marker_array,
    lanelet::visualization::laneletsAsTriangleMarkerArray("end_lanelets", end_lanelets, cl_end));
  insert_marker_array(
    &route_marker_array,
    lanelet::visualization::laneletsAsTriangleMarkerArray("goal_lanelets", goal_lanelets, cl_goal));

  return route_marker_array; // 返回路径标记数组
}

// 可视化调试足迹
visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
  autoware::universe_utils::LinearRing2d goal_footprint)
{
  visualization_msgs::msg::MarkerArray msg;
  // 创建默认标记
  auto marker = autoware::universe_utils::createDefaultMarker(
    "map", rclcpp::Clock().now(), "goal_footprint", 0, visualization_msgs::msg::Marker::LINE_STRIP,
    autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0),
    autoware::universe_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0));
  marker.lifetime = rclcpp::Duration::from_seconds(2.5); // 设置标记的生命周期

  // 添加足迹的点
  marker.points.push_back(
    autoware::universe_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0));
  marker.points.push_back(
    autoware::universe_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0));
  marker.points.push_back(
    autoware::universe_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0));
  marker.points.push_back(
    autoware::universe_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0));
  marker.points.push_back(
    autoware::universe_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0));
  marker.points.push_back(
    autoware::universe_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0));
  marker.points.push_back(marker.points.front()); // 闭合足迹

  msg.markers.push_back(marker); // 添加标记到数组
  return msg; // 返回标记数组
}

// 获取从起始车道到指定距离内的所有后续车道
lanelet::ConstLanelets next_lanelets_up_to(
  const lanelet::ConstLanelet & start_lanelet, const double up_to_distance,
  const route_handler::RouteHandler & route_handler)
{
  lanelet::ConstLanelets lanelets;
  if (up_to_distance <= 0.0) {
    return lanelets;
  }
  // 获取当前车道的后续车道
  for (const auto & next_lane : route_handler.getNextLanelets(start_lanelet)) {
    lanelets.push_back(next_lane);
    // 递归获取后续车道的后续车道
    const auto next_lanelets = next_lanelets_up_to(
      next_lane, up_to_distance - lanelet::geometry::length2d(next_lane), route_handler);
    lanelets.insert(lanelets.end(), next_lanelets.begin(), next_lanelets.end());
  }
  return lanelets;
}

// 检查目标足迹是否在车道内
bool DefaultPlanner::check_goal_footprint_inside_lanes(
  const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets,
  const universe_utils::Polygon2d & goal_footprint) const
{
  universe_utils::MultiPolygon2d ego_lanes;
  universe_utils::Polygon2d poly;
  // 遍历路径车道，提取车道的多边形
  for (const auto & ll : path_lanelets) {
    const auto left_shoulder = route_handler_.getLeftShoulderLanelet(ll);
    if (left_shoulder) {
      boost::geometry::convert(left_shoulder->polygon2d().basicPolygon(), poly);
      ego_lanes.push_back(poly);
    }
    const auto right_shoulder = route_handler_.getRightShoulderLanelet(ll);
    if (right_shoulder) {
      boost::geometry::convert(right_shoulder->polygon2d().basicPolygon(), poly);
      ego_lanes.push_back(poly);
    }
    boost::geometry::convert(ll.polygon2d().basicPolygon(), poly);
    ego_lanes.push_back(poly);
  }
  // 获取当前车道的后续车道
  const auto next_lanelets =
    next_lanelets_up_to(current_lanelet, vehicle_info_.max_longitudinal_offset_m, route_handler_);
  for (const auto & ll : next_lanelets) {
    boost::geometry::convert(ll.polygon2d().basicPolygon(), poly);
    ego_lanes.push_back(poly);
  }

  // 检查目标足迹是否在车道内
  universe_utils::MultiPolygon2d difference;
  boost::geometry::difference(goal_footprint, ego_lanes, difference);
  return boost::geometry::is_empty(difference); // 如果差异为空，则目标足迹在车道内
}

// 检查目标点是否有效
bool DefaultPlanner::is_goal_valid(
  const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets)
{
  const auto logger = node_->get_logger();

  const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position);

  // 检查目标点是否在肩道内
  lanelet::Lanelet closest_shoulder_lanelet;
  const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(goal);
  if (lanelet::utils::query::getClosestLanelet(
        shoulder_lanelets, goal, &closest_shoulder_lanelet)) {
    const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position);
    const auto goal_yaw = tf2::getYaw(goal.orientation);
    const auto angle_diff = autoware::universe_utils::normalizeRadian(lane_yaw - goal_yaw);
    const double th_angle = autoware::universe_utils::deg2rad(param_.goal_angle_threshold_deg);
    if (std::abs(angle_diff) < th_angle) {
      return true; // 目标点在肩道内且角度符合要求
    }
  }
  lanelet::ConstLanelet closest_lanelet;
  const auto road_lanelets_at_goal = route_handler_.getRoadLaneletsAtPose(goal);
  if (!lanelet::utils::query::getClosestLanelet(road_lanelets_at_goal, goal, &closest_lanelet)) {
    // 如果目标点不在任何车道内，则查找最近的车道
    const lanelet::BasicPoint2d goal_point{goal.position.x, goal.position.y};
    auto closest_dist = std::numeric_limits<double>::max();
    const auto closest_road_lanelet_found =
      route_handler_.getLaneletMapPtr()->laneletLayer.nearestUntil(
        goal_point, [&](const auto & bbox, const auto & ll) {
          // 通过增加距离搜索最近的车道
          if (lanelet::geometry::distance2d(bbox, goal_point) > closest_dist)
            return true;  // 停止搜索
          const auto dist = lanelet::geometry::distance2d(goal_point, ll.polygon2d());
          if (route_handler_.isRoadLanelet(ll) && dist < closest_dist) {
            closest_dist = dist;
            closest_lanelet = ll;
          }
          return false;  // 继续搜索
        });
    if (!closest_road_lanelet_found) return false; // 如果未找到车道，则目标点无效
  }

  // 获取车辆的足迹
  const auto local_vehicle_footprint = vehicle_info_.createFootprint();
  autoware::universe_utils::LinearRing2d goal_footprint =
    transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(goal));
  pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint));
  const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint);

  // 检查目标足迹是否超出车道（如果目标点不在停车场内）
  if (
    param_.check_footprint_inside_lanes &&
    !check_goal_footprint_inside_lanes(closest_lanelet, path_lanelets, polygon_footprint) &&
    !is_in_parking_lot(
      lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()),
      lanelet::utils::conversion::toLaneletPoint(goal.position))) {
    RCLCPP_WARN(logger, "Goal's footprint exceeds lane!");
    return false; // 目标足迹超出车道
  }

  // 检查目标点是否在车道内
  if (is_in_lane(closest_lanelet, goal_lanelet_pt)) {
    const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position);
    const auto goal_yaw = tf2::getYaw(goal.orientation);
    const auto angle_diff = autoware::universe_utils::normalizeRadian(lane_yaw - goal_yaw);

    const double th_angle = autoware::universe_utils::deg2rad(param_.goal_angle_threshold_deg);
    if (std::abs(angle_diff) < th_angle) {
      return true; // 目标点在车道内且角度符合要求
    }
  }

  // 检查目标点是否在停车位内
  const auto parking_spaces =
    lanelet::utils::query::getAllParkingSpaces(route_handler_.getLaneletMapPtr());
  if (is_in_parking_space(parking_spaces, goal_lanelet_pt)) {
    return true; // 目标点在停车位内
  }

  // 检查目标点是否在停车场内
  const auto parking_lots =
    lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr());
  return is_in_parking_lot(parking_lots, goal_lanelet_pt); // 目标点在停车场内
}

// 规划路径
PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
{
  const auto logger = node_->get_logger();

  // 打印路径规划的点
  std::stringstream log_ss;
  for (const auto & point : points) {
    log_ss << "x: " << point.position.x << " "
           << "y: " << point.position.y << std::endl;
  }
  RCLCPP_DEBUG_STREAM(
    logger, "start planning route with check points: " << std::endl
                                                       << log_ss.str());

  LaneletRoute route_msg;
  RouteSections route_sections;

  lanelet::ConstLanelets all_route_lanelets;
  for (std::size_t i = 1; i < points.size(); i++) {
    const auto start_check_point = points.at(i - 1);
    const auto goal_check_point = points.at(i);
    lanelet::ConstLanelets path_lanelets;
    // 规划两个检查点之间的路径车道
    if (!route_handler_.planPathLaneletsBetweenCheckpoints(
          start_check_point, goal_check_point, &path_lanelets, param_.consider_no_drivable_lanes)) {
      RCLCPP_WARN(logger, "Failed to plan route.");
      return route_msg; // 如果规划失败，则返回空路径
    }

    // 将规划的车道添加到路径中
    for (const auto & lane : path_lanelets) {
      if (!all_route_lanelets.empty() && lane.id() == all_route_lanelets.back().id()) continue;
      all_route_lanelets.push_back(lane);
    }
  }
  route_handler_.setRouteLanelets(all_route_lanelets); // 设置路径车道
  route_sections = route_handler_.createMapSegments(all_route_lanelets); // 创建路径段

  auto goal_pose = points.back();
  if (param_.enable_correct_goal_pose) {
    // 如果启用了目标点位姿校正，则获取最接近中心线的位姿
    goal_pose = get_closest_centerline_pose(
      lanelet::utils::query::laneletLayer(route_handler_.getLaneletMapPtr()), goal_pose,
      vehicle_info_);
  }

  // 检查目标点是否有效
  if (!is_goal_valid(goal_pose, all_route_lanelets)) {
    RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose");
    return route_msg; // 如果目标点无效，则返回空路径
  }

  // 检查路径是否循环
  if (route_handler::RouteHandler::isRouteLooped(route_sections)) {
    RCLCPP_WARN(logger, "Loop detected within route!");
    return route_msg; // 如果路径循环，则返回空路径
  }

  // 校正目标点的高度
  const auto refined_goal = refine_goal_height(goal_pose, route_sections);
  RCLCPP_DEBUG(logger, "Goal Pose Z : %lf", refined_goal.position.z);

  // 设置路径消息
  route_msg.start_pose = points.front(); // 起点位姿
  route_msg.goal_pose = refined_goal; // 目标点位姿
  route_msg.segments = route_sections; // 路径段
  return route_msg; // 返回路径消息
}

// 校正目标点的高度
geometry_msgs::msg::Pose DefaultPlanner::refine_goal_height(
  const Pose & goal, const RouteSections & route_sections)
{
  const auto goal_lane_id = route_sections.back().preferred_primitive.id;
  const auto goal_lanelet = route_handler_.getLaneletsFromId(goal_lane_id);
  const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position);
  const auto goal_height = project_goal_to_map(goal_lanelet, goal_lanelet_pt);

  Pose refined_goal = goal;
  refined_goal.position.z = goal_height; // 设置校正后的高度
  return refined_goal;
}

// 更新路径
void DefaultPlanner::updateRoute(const PlannerPlugin::LaneletRoute & route)
{
  route_handler_.setRoute(route); // 设置路径处理器中的路径
}

// 清除路径
void DefaultPlanner::clearRoute()
{
  route_handler_.clearRoute(); // 清除路径处理器中的路径
}

}  // namespace autoware::mission_planner::lanelet2

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(
  autoware::mission_planner::lanelet2::DefaultPlanner, autoware::mission_planner::PlannerPlugin)

7. arrival_checker.cpp

是一个用于检查车辆是否到达目标点的工具类。它的核心功能是：

设置目标点：支持设置目标点位姿和 UUID（唯一标识符）。

检查到达状态：

    检查车辆是否接近目标点（距离和角度是否符合要求）。

    检查车辆是否已停止。

源码注释

// Copyright 2022 TIER IV, Inc.
//
// 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 "arrival_checker.hpp" // 引入到达检查器头文件

#include <autoware/universe_utils/geometry/geometry.hpp> // 引入几何工具库
#include <autoware/universe_utils/math/normalization.hpp> // 引入数学工具库（归一化）
#include <autoware/universe_utils/math/unit_conversion.hpp> // 引入数学工具库（单位转换）

#include <tf2/utils.h> // 引入 TF2 工具库

namespace autoware::mission_planner
{

// 构造函数，初始化到达检查器
ArrivalChecker::ArrivalChecker(rclcpp::Node * node) : vehicle_stop_checker_(node)
{
  // 从参数服务器读取角度阈值（单位为度），并转换为弧度
  const double angle_deg = node->declare_parameter<double>("arrival_check_angle_deg");
  angle_ = autoware::universe_utils::deg2rad(angle_deg);

  // 从参数服务器读取距离阈值
  distance_ = node->declare_parameter<double>("arrival_check_distance");

  // 从参数服务器读取持续时间阈值
  duration_ = node->declare_parameter<double>("arrival_check_duration");
}

// 清除目标点
void ArrivalChecker::set_goal()
{
  // 清除当前目标点
  goal_with_uuid_ = std::nullopt;
}

// 设置目标点
void ArrivalChecker::set_goal(const PoseWithUuidStamped & goal)
{
  // 设置新的目标点，并忽略与之前目标点 UUID 不匹配的修改
  goal_with_uuid_ = goal;
}

// 检查车辆是否到达目标点
bool ArrivalChecker::is_arrived(const PoseStamped & pose) const
{
  // 如果没有目标点，则返回未到达
  if (!goal_with_uuid_) {
    return false;
  }
  const auto goal = goal_with_uuid_.value();

  // 检查目标点和车辆位姿的坐标系是否一致
  if (goal.header.frame_id != pose.header.frame_id) {
    return false;
  }

  // 检查车辆与目标点的距离是否小于阈值
  if (distance_ < autoware::universe_utils::calcDistance2d(pose.pose, goal.pose)) {
    return false;
  }

  // 检查车辆与目标点的角度差是否小于阈值
  const double yaw_pose = tf2::getYaw(pose.pose.orientation); // 获取车辆的偏航角
  const double yaw_goal = tf2::getYaw(goal.pose.orientation); // 获取目标点的偏航角
  const double yaw_diff = autoware::universe_utils::normalizeRadian(yaw_pose - yaw_goal); // 计算角度差
  if (angle_ < std::fabs(yaw_diff)) {
    return false; // 如果角度差大于阈值，则返回未到达
  }

  // 检查车辆是否已停止
  return vehicle_stop_checker_.isVehicleStopped(duration_);
}

}  // namespace autoware::mission_planner

---

总结

主要把全局路径生成模块给讲完了，下一章继续往后讲，其中有很多实现是基于Lanelet2 高精地图的，这部分会在这个系列讲完之后单独出一版。

标签：const,goal,universe,route,路径,lanelet,源码,Autoware,auto
From： https://blog.csdn.net/weixin_48386130/article/details/145140674