【一看就会】Autoware.universe的控制部分源码梳理【三】

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

前言
一、Autonomous Emergency Braking
二、autoware_collision_detector
三、autoware_control_validator
四、autoware_external_cmd_selector
五、autoware_joy_controller
六、autoware_lane_departure_checker
- 主要功能&输入&输出
- 2.源码注释
总结

前言

control最核心的路径跟踪部分在之前的内容中讲完了（虽然讲的很烂，最后我会再出一章梳理一下）。

但是在control.launch.xml，还启动了别的控制模块，毕竟车辆不止有路径跟随一种控制。
启动的模块之前的文章中有写，在此再列一下：

轨迹跟踪（Trajectory Follower）：控制车辆沿着规划轨迹前进。
车道偏离检查（Lane Departure Checker）：检测车辆是否偏离车道。
换挡决策（Shift Decider）：控制车辆的换挡操作。
车辆命令网关（Vehicle Command Gate）：处理车辆控制命令的输入输出。
操作模式转换管理器（Operation Mode Transition Manager）：管理车辆的操作模式转换。
障碍物碰撞检测（Obstacle Collision Checker）：监测障碍物和碰撞检测。
自动紧急制动（Autonomous Emergency Braking）：检测碰撞风险并自动启动紧急制动。
预测路径检查（Predicted Path Checker）：检查车辆的预测轨迹。
控制评估（Control Evaluator）：评估和监测控制系统的性能。
碰撞检测器（Collision Detector）：检测可能的碰撞。

所有的这些都在control功能包里面。
control功能包下面还有18个包，具体的还是得看我之前写的https://blog.csdn.net/weixin_48386130/article/details/144890288?spm=1001.2014.3001.5501
我接下来就依次进行讲解。

对了，我为什么会开这个系列的博客，是因为Autoware.universe官方文档里面没写这部分。像是planning这部分官方有文档的大家就直接去看就行，control这部分等官方有了文档之后大家也优先看官方文档。

一、Autonomous Emergency Braking

AEB 是最后的安全保障机制，当其他避障规划都失败时，通过紧急制动来避免碰撞。

1.原理介绍

主要功能：
1.碰撞检测：使用点云数据和预测对象检测潜在碰撞
2.路径生成：基于 IMU 和 MPC 生成预测路径
3.RSS (Responsibility-Sensitive Safety) 距离计算
4.紧急制动决策

碰撞检测流程：
1.获取传感器数据（点云/预测对象）
2.生成预测路径
3.计算车辆轮廓
4.检测碰撞对象
5.计算 RSS 距离
6.决策是否紧急制动

主要参数配置：

# 关键参数
expand_width: 0.1                # 碰撞检测区域扩展宽度
path_footprint_extra_margin: 4.0 # 路径足迹额外边距
longitudinal_offset: 2.0         # 纵向偏移量
t_response: 1.0                  # 响应时间
a_ego_min: -3.0                 # 自车最小加速度
a_obj_min: -1.0                 # 障碍物最小加速度

碰撞检测逻辑：

bool AEB::hasCollision(const double current_v, const ObjectData & closest_object)
{
  // 计算 RSS 安全距离
  const double rss_dist = std::invoke([&]() {
    const double & obj_v = closest_object.velocity;
    const double & t = t_response_;
    const double pre_braking_covered_distance = std::abs(current_v) * t;
    const double braking_distance = (current_v * current_v) / (2 * std::fabs(a_ego_min_));
    const double ego_stopping_distance = pre_braking_covered_distance + braking_distance;
    // ...
    return ego_stopping_distance + obj_braking_distance + longitudinal_offset_;
  });

  // 如果实际距离小于安全距离，触发制动
  if (closest_object.distance_to_object > rss_dist) return false;
  // collision happens
  collision_data_keeper_.setCollisionData(collision_data);
  return true;
}

2.源码注释

一共两个cpp文件，源码注释如下：

node.cpp

// Copyright 2024 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 <autoware/autonomous_emergency_braking/utils.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>

#include <optional>

namespace autoware::motion::control::autonomous_emergency_braking::utils
{
using autoware::universe_utils::Polygon2d; // 2D多边形类型
using autoware_perception_msgs::msg::PredictedObject; // 预测对象消息类型
using autoware_perception_msgs::msg::PredictedObjects; // 预测对象集合消息类型
using geometry_msgs::msg::Point; // 点消息类型
using geometry_msgs::msg::Pose; // 位姿消息类型
using geometry_msgs::msg::TransformStamped; // 时间戳变换消息类型
using geometry_msgs::msg::Vector3; // 3D向量消息类型

// 将预测对象从一个坐标系转换到另一个坐标系
PredictedObject transformObjectFrame(
  const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped)
{
  PredictedObject output = input;
  const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear; // 线速度
  const auto & angular_twist = input.kinematics.initial_twist_with_covariance.twist.angular; // 角速度
  const auto & pose = input.kinematics.initial_pose_with_covariance.pose; // 初始位姿

  geometry_msgs::msg::Pose t_pose; // 变换后的位姿
  Vector3 t_linear_twist; // 变换后的线速度
  Vector3 t_angular_twist; // 变换后的角速度

  // 使用tf2库进行坐标变换
  tf2::doTransform(pose, t_pose, transform_stamped);
  tf2::doTransform(linear_twist, t_linear_twist, transform_stamped);
  tf2::doTransform(angular_twist, t_angular_twist, transform_stamped);

  // 更新输出对象的位姿和速度
  output.kinematics.initial_pose_with_covariance.pose = t_pose;
  output.kinematics.initial_twist_with_covariance.twist.linear = t_linear_twist;
  output.kinematics.initial_twist_with_covariance.twist.angular = t_angular_twist;
  return output;
}

// 将多边形对象转换为几何多边形
Polygon2d convertPolygonObjectToGeometryPolygon(
  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape)
{
  if (obj_shape.footprint.points.empty()) {
    return {}; // 如果多边形点集为空，返回空多边形
  }
  Polygon2d object_polygon;
  tf2::Transform tf_map2obj;
  fromMsg(current_pose, tf_map2obj); // 将位姿消息转换为tf2变换
  const auto obj_points = obj_shape.footprint.points; // 获取多边形点集
  object_polygon.outer().reserve(obj_points.size() + 1); // 预留空间
  for (const auto & obj_point : obj_points) {
    tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); // 将点转换为tf2向量
    tf2::Vector3 tf_obj = tf_map2obj * obj; // 变换到目标坐标系
    object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); // 添加到多边形
  }
  object_polygon.outer().push_back(object_polygon.outer().front()); // 闭合多边形
  boost::geometry::correct(object_polygon); // 修正多边形

  return object_polygon;
}

// 将圆柱体对象转换为几何多边形
Polygon2d convertCylindricalObjectToGeometryPolygon(
  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape)
{
  Polygon2d object_polygon;

  const double obj_x = current_pose.position.x; // 对象的x坐标
  const double obj_y = current_pose.position.y; // 对象的y坐标

  constexpr int n = 20; // 多边形的边数
  const double r = obj_shape.dimensions.x / 2; // 圆柱体的半径
  object_polygon.outer().reserve(n + 1); // 预留空间
  for (int i = 0; i < n; ++i) {
    object_polygon.outer().emplace_back(
      obj_x + r * std::cos(2.0 * M_PI / n * i), obj_y + r * std::sin(2.0 * M_PI / n * i)); // 计算多边形顶点
  }

  object_polygon.outer().push_back(object_polygon.outer().front()); // 闭合多边形
  boost::geometry::correct(object_polygon); // 修正多边形

  return object_polygon;
}

// 将边界框对象转换为几何多边形
Polygon2d convertBoundingBoxObjectToGeometryPolygon(
  const Pose & current_pose, const double & base_to_front, const double & base_to_rear,
  const double & base_to_width)
{
  const auto mapped_point = [](const double & length_scalar, const double & width_scalar) {
    tf2::Vector3 map;
    map.setX(length_scalar); // 设置x坐标
    map.setY(width_scalar); // 设置y坐标
    map.setZ(0.0); // 设置z坐标
    map.setW(1.0); // 设置齐次坐标
    return map;
  };

  // 设置边界框的四个顶点
  const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width);
  const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width);
  const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width);
  const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width);

  // 将顶点从地图坐标系转换到对象坐标系
  tf2::Transform tf_map2obj;
  tf2::fromMsg(current_pose, tf_map2obj);
  const tf2::Vector3 p1_obj = tf_map2obj * p1_map;
  const tf2::Vector3 p2_obj = tf_map2obj * p2_map;
  const tf2::Vector3 p3_obj = tf_map2obj * p3_map;
  const tf2::Vector3 p4_obj = tf_map2obj * p4_map;

  Polygon2d object_polygon;
  object_polygon.outer().reserve(5); // 预留空间
  object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); // 添加顶点
  object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y());
  object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y());
  object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y());

  object_polygon.outer().push_back(object_polygon.outer().front()); // 闭合多边形
  boost::geometry::correct(object_polygon); // 修正多边形

  return object_polygon;
}

// 将预测对象转换为多边形
Polygon2d convertObjToPolygon(const PredictedObject & obj)
{
  Polygon2d object_polygon{};
  if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) {
    // 如果是圆柱体，调用圆柱体转换函数
    object_polygon = utils::convertCylindricalObjectToGeometryPolygon(
      obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
  } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
    // 如果是边界框，调用边界框转换函数
    const double & length_m = obj.shape.dimensions.x / 2;
    const double & width_m = obj.shape.dimensions.y / 2;
    object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon(
      obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m);
  } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) {
    // 如果是多边形，调用多边形转换函数
    object_polygon = utils::convertPolygonObjectToGeometryPolygon(
      obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
  } else {
    throw std::runtime_error("Unsupported shape type"); // 不支持的类型抛出异常
  }
  return object_polygon;
}

// 获取坐标系之间的变换
std::optional<geometry_msgs::msg::TransformStamped> getTransform(
  const std::string & target_frame, const std::string & source_frame,
  const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger)
{
  geometry_msgs::msg::TransformStamped tf_current_pose;
  try {
    // 从tf_buffer中查找变换
    tf_current_pose = tf_buffer.lookupTransform(
      target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0));
  } catch (tf2::TransformException & ex) {
    // 如果查找失败，记录错误日志
    RCLCPP_ERROR_STREAM(
      logger, "[AEB] Failed to look up transform from " + source_frame + " to " + target_frame);
    return std::nullopt; // 返回空值
  }
  return std::make_optional(tf_current_pose); // 返回变换
}

// 从多边形填充可视化标记
void fillMarkerFromPolygon(
  const std::vector<Polygon2d> & polygons, visualization_msgs::msg::Marker & polygon_marker)
{
  for (const auto & poly : polygons) {
    for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) {
      const auto & boost_cp = poly.outer().at(dp_idx); // 当前点
      const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); // 下一个点
      const auto curr_point =
        autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); // 创建点
      const auto next_point =
        autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0);
      polygon_marker.points.push_back(curr_point); // 添加到标记
      polygon_marker.points.push_back(next_point);
    }
  }
}

// 从3D多边形填充可视化标记
void fillMarkerFromPolygon(
  const std::vector<Polygon3d> & polygons, visualization_msgs::msg::Marker & polygon_marker)
{
  for (const auto & poly : polygons) {
    for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) {
      const auto & boost_cp = poly.outer().at(dp_idx); // 当前点
      const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); // 下一个点
      const auto curr_point =
        autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z()); // 创建点
      const auto next_point =
        autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z());
      polygon_marker.points.push_back(curr_point); // 添加到标记
      polygon_marker.points.push_back(next_point);
    }
  }
}

}  // namespace autoware::motion::control::autonomous_emergency_braking::utils

主要功能说明：

    transformObjectFrame: 将预测对象从一个坐标系转换到另一个坐标系。

    convertPolygonObjectToGeometryPolygon: 将多边形对象转换为几何多边形。

    convertCylindricalObjectToGeometryPolygon: 将圆柱体对象转换为几何多边形。

    convertBoundingBoxObjectToGeometryPolygon: 将边界框对象转换为几何多边形。

    convertObjToPolygon: 根据对象类型调用相应的转换函数。

    getTransform: 获取两个坐标系之间的变换。

    fillMarkerFromPolygon: 从多边形生成可视化标记。

utils.cpp

// Copyright 2024 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 <autoware/autonomous_emergency_braking/utils.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>

#include <optional>

namespace autoware::motion::control::autonomous_emergency_braking::utils
{
using autoware::universe_utils::Polygon2d; // 2D多边形类型
using autoware_perception_msgs::msg::PredictedObject; // 预测对象消息类型
using autoware_perception_msgs::msg::PredictedObjects; // 预测对象集合消息类型
using geometry_msgs::msg::Point; // 点消息类型
using geometry_msgs::msg::Pose; // 位姿消息类型
using geometry_msgs::msg::TransformStamped; // 时间戳变换消息类型
using geometry_msgs::msg::Vector3; // 3D向量消息类型

// 将预测对象从一个坐标系转换到另一个坐标系
PredictedObject transformObjectFrame(
  const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped)
{
  PredictedObject output = input;
  const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear; // 线速度
  const auto & angular_twist = input.kinematics.initial_twist_with_covariance.twist.angular; // 角速度
  const auto & pose = input.kinematics.initial_pose_with_covariance.pose; // 初始位姿

  geometry_msgs::msg::Pose t_pose; // 变换后的位姿
  Vector3 t_linear_twist; // 变换后的线速度
  Vector3 t_angular_twist; // 变换后的角速度

  // 使用tf2库进行坐标变换
  tf2::doTransform(pose, t_pose, transform_stamped);
  tf2::doTransform(linear_twist, t_linear_twist, transform_stamped);
  tf2::doTransform(angular_twist, t_angular_twist, transform_stamped);

  // 更新输出对象的位姿和速度
  output.kinematics.initial_pose_with_covariance.pose = t_pose;
  output.kinematics.initial_twist_with_covariance.twist.linear = t_linear_twist;
  output.kinematics.initial_twist_with_covariance.twist.angular = t_angular_twist;
  return output;
}

// 将多边形对象转换为几何多边形
Polygon2d convertPolygonObjectToGeometryPolygon(
  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape)
{
  if (obj_shape.footprint.points.empty()) {
    return {}; // 如果多边形点集为空，返回空多边形
  }
  Polygon2d object_polygon;
  tf2::Transform tf_map2obj;
  fromMsg(current_pose, tf_map2obj); // 将位姿消息转换为tf2变换
  const auto obj_points = obj_shape.footprint.points; // 获取多边形点集
  object_polygon.outer().reserve(obj_points.size() + 1); // 预留空间
  for (const auto & obj_point : obj_points) {
    tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); // 将点转换为tf2向量
    tf2::Vector3 tf_obj = tf_map2obj * obj; // 变换到目标坐标系
    object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); // 添加到多边形
  }
  object_polygon.outer().push_back(object_polygon.outer().front()); // 闭合多边形
  boost::geometry::correct(object_polygon); // 修正多边形

  return object_polygon;
}

// 将圆柱体对象转换为几何多边形
Polygon2d convertCylindricalObjectToGeometryPolygon(
  const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape)
{
  Polygon2d object_polygon;

  const double obj_x = current_pose.position.x; // 对象的x坐标
  const double obj_y = current_pose.position.y; // 对象的y坐标

  constexpr int n = 20; // 多边形的边数
  const double r = obj_shape.dimensions.x / 2; // 圆柱体的半径
  object_polygon.outer().reserve(n + 1); // 预留空间
  for (int i = 0; i < n; ++i) {
    object_polygon.outer().emplace_back(
      obj_x + r * std::cos(2.0 * M_PI / n * i), obj_y + r * std::sin(2.0 * M_PI / n * i)); // 计算多边形顶点
  }

  object_polygon.outer().push_back(object_polygon.outer().front()); // 闭合多边形
  boost::geometry::correct(object_polygon); // 修正多边形

  return object_polygon;
}

// 将边界框对象转换为几何多边形
Polygon2d convertBoundingBoxObjectToGeometryPolygon(
  const Pose & current_pose, const double & base_to_front, const double & base_to_rear,
  const double & base_to_width)
{
  const auto mapped_point = [](const double & length_scalar, const double & width_scalar) {
    tf2::Vector3 map;
    map.setX(length_scalar); // 设置x坐标
    map.setY(width_scalar); // 设置y坐标
    map.setZ(0.0); // 设置z坐标
    map.setW(1.0); // 设置齐次坐标
    return map;
  };

  // 设置边界框的四个顶点
  const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width);
  const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width);
  const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width);
  const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width);

  // 将顶点从地图坐标系转换到对象坐标系
  tf2::Transform tf_map2obj;
  tf2::fromMsg(current_pose, tf_map2obj);
  const tf2::Vector3 p1_obj = tf_map2obj * p1_map;
  const tf2::Vector3 p2_obj = tf_map2obj * p2_map;
  const tf2::Vector3 p3_obj = tf_map2obj * p3_map;
  const tf2::Vector3 p4_obj = tf_map2obj * p4_map;

  Polygon2d object_polygon;
  object_polygon.outer().reserve(5); // 预留空间
  object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); // 添加顶点
  object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y());
  object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y());
  object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y());

  object_polygon.outer().push_back(object_polygon.outer().front()); // 闭合多边形
  boost::geometry::correct(object_polygon); // 修正多边形

  return object_polygon;
}

// 将预测对象转换为多边形
Polygon2d convertObjToPolygon(const PredictedObject & obj)
{
  Polygon2d object_polygon{};
  if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) {
    // 如果是圆柱体，调用圆柱体转换函数
    object_polygon = utils::convertCylindricalObjectToGeometryPolygon(
      obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
  } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
    // 如果是边界框，调用边界框转换函数
    const double & length_m = obj.shape.dimensions.x / 2;
    const double & width_m = obj.shape.dimensions.y / 2;
    object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon(
      obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m);
  } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) {
    // 如果是多边形，调用多边形转换函数
    object_polygon = utils::convertPolygonObjectToGeometryPolygon(
      obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
  } else {
    throw std::runtime_error("Unsupported shape type"); // 不支持的类型抛出异常
  }
  return object_polygon;
}

// 获取坐标系之间的变换
std::optional<geometry_msgs::msg::TransformStamped> getTransform(
  const std::string & target_frame, const std::string & source_frame,
  const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger)
{
  geometry_msgs::msg::TransformStamped tf_current_pose;
  try {
    // 从tf_buffer中查找变换
    tf_current_pose = tf_buffer.lookupTransform(
      target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0));
  } catch (tf2::TransformException & ex) {
    // 如果查找失败，记录错误日志
    RCLCPP_ERROR_STREAM(
      logger, "[AEB] Failed to look up transform from " + source_frame + " to " + target_frame);
    return std::nullopt; // 返回空值
  }
  return std::make_optional(tf_current_pose); // 返回变换
}

// 从2D多边形填充可视化标记
void fillMarkerFromPolygon(
  const std::vector<Polygon2d> & polygons, visualization_msgs::msg::Marker & polygon_marker)
{
  for (const auto & poly : polygons) {
    for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) {
      const auto & boost_cp = poly.outer().at(dp_idx); // 当前点
      const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); // 下一个点
      const auto curr_point =
        autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); // 创建点
      const auto next_point =
        autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0);
      polygon_marker.points.push_back(curr_point); // 添加到标记
      polygon_marker.points.push_back(next_point);
    }
  }
}

// 从3D多边形填充可视化标记
void fillMarkerFromPolygon(
  const std::vector<Polygon3d> & polygons, visualization_msgs::msg::Marker & polygon_marker)
{
  for (const auto & poly : polygons) {
    for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) {
      const auto & boost_cp = poly.outer().at(dp_idx); // 当前点
      const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); // 下一个点
      const auto curr_point =
        autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z()); // 创建点
      const auto next_point =
        autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z());
      polygon_marker.points.push_back(curr_point); // 添加到标记
      polygon_marker.points.push_back(next_point);
    }
  }
}

}  // namespace autoware::motion::control::autonomous_emergency_braking::utils

主要功能说明：

    transformObjectFrame: 将预测对象从一个坐标系转换到另一个坐标系。

    convertPolygonObjectToGeometryPolygon: 将多边形对象转换为几何多边形。

    convertCylindricalObjectToGeometryPolygon: 将圆柱体对象转换为几何多边形。

    convertBoundingBoxObjectToGeometryPolygon: 将边界框对象转换为几何多边形。

    convertObjToPolygon: 根据对象类型调用相应的转换函数。

    getTransform: 获取两个坐标系之间的变换。

    fillMarkerFromPolygon: 从多边形生成可视化标记。

二、autoware_collision_detector

这个是检测碰撞的。主要功能是检测车辆周围的障碍物并发出警告。
不会控制车辆作出动作，是一个监控系统，同时使用点云和动态物体检测，然后发出诊断信息，就是一个风险预警。
到底有那些模块会用这部分检测信息，我还不知道，等之后清楚了再来补充。
主要功能：

class CollisionDetectorNode {
  // 主要参数
  struct NodeParam {
    bool use_pointcloud;           // 是否使用点云数据
    bool use_dynamic_object;       // 是否使用动态物体检测
    double collision_distance;     // 碰撞检测距离
    double nearby_filter_radius;   // 近距离过滤半径
    double keep_ignoring_time;     // 忽略时间
  };

  // 主要检测函数
  void checkCollision() {
    // 1. 获取最近的障碍物
    const auto nearest_obstacle = getNearestObstacle();
    
    // 2. 判断是否有碰撞风险
    const auto is_obstacle_found = 
      !nearest_obstacle ? false : nearest_obstacle.get().first < node_param_.collision_distance;

    // 3. 如果是自动驾驶模式且检测到障碍物，发出警告
    if (operation_mode_ptr_->mode == OperationModeState::AUTONOMOUS && 
        (is_obstacle_found || is_obstacle_found_recently)) {
      status.level = DiagnosticStatus::ERROR;
      status.message = "collision detected";
    }
  }

就一个cpp文件，node.cpp源码注释：

// Copyright 2024 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 "autoware/collision_detector/node.hpp"

#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/ros/uuid_helper.hpp>
#include <tf2_eigen/tf2_eigen.hpp>

#include <boost/assert.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/format.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace autoware::collision_detector
{
namespace bg = boost::geometry;
using Point2d = bg::model::d2::point_xy<double>; // 2D点类型
using Polygon2d = bg::model::polygon<Point2d>; // 2D多边形类型
using autoware::universe_utils::createPoint; // 创建点的工具函数
using autoware::universe_utils::pose2transform; // 将位姿转换为变换的工具函数

namespace
{

// 创建一个32位精度的点
geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const double z)
{
  geometry_msgs::msg::Point32 p;
  p.x = x;
  p.y = y;
  p.z = z;
  return p;
}

// 根据位姿和足迹创建对象的多边形
Polygon2d createObjPolygon(
  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint)
{
  geometry_msgs::msg::Polygon transformed_polygon{};
  geometry_msgs::msg::TransformStamped geometry_tf{};
  geometry_tf.transform = pose2transform(pose); // 将位姿转换为变换
  tf2::doTransform(footprint, transformed_polygon, geometry_tf); // 对足迹进行坐标变换

  Polygon2d object_polygon;
  for (const auto & p : transformed_polygon.points) {
    object_polygon.outer().push_back(Point2d(p.x, p.y)); // 将点添加到多边形
  }

  bg::correct(object_polygon); // 修正多边形

  return object_polygon;
}

// 根据位姿和尺寸创建对象的多边形
Polygon2d createObjPolygon(
  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size)
{
  const double length_m = size.x / 2.0; // 长度的一半
  const double width_m = size.y / 2.0; // 宽度的一半

  geometry_msgs::msg::Polygon polygon{};

  // 添加边界框的四个顶点
  polygon.points.push_back(createPoint32(length_m, -width_m, 0.0));
  polygon.points.push_back(createPoint32(length_m, width_m, 0.0));
  polygon.points.push_back(createPoint32(-length_m, width_m, 0.0));
  polygon.points.push_back(createPoint32(-length_m, -width_m, 0.0));

  return createObjPolygon(pose, polygon); // 调用前面的函数创建多边形
}

// 为圆柱体对象创建多边形
Polygon2d createObjPolygonForCylinder(const geometry_msgs::msg::Pose & pose, const double diameter)
{
  geometry_msgs::msg::Polygon polygon{};

  const double radius = diameter * 0.5; // 半径
  // 添加六边形的顶点
  for (int i = 0; i < 6; ++i) {
    const double angle = 2.0 * M_PI * static_cast<double>(i) / 6.0; // 计算角度
    const double x = radius * std::cos(angle); // x坐标
    const double y = radius * std::sin(angle); // y坐标
    polygon.points.push_back(createPoint32(x, y, 0.0)); // 添加点
  }

  return createObjPolygon(pose, polygon); // 调用前面的函数创建多边形
}

// 创建自车多边形
Polygon2d createSelfPolygon(const VehicleInfo & vehicle_info)
{
  const double & front_m = vehicle_info.max_longitudinal_offset_m; // 前部偏移
  const double & width_left_m = vehicle_info.max_lateral_offset_m; // 左侧偏移
  const double & width_right_m = vehicle_info.min_lateral_offset_m; // 右侧偏移
  const double & rear_m = vehicle_info.min_longitudinal_offset_m; // 后部偏移

  Polygon2d ego_polygon;

  // 添加自车多边形的四个顶点
  ego_polygon.outer().push_back(Point2d(front_m, width_left_m));
  ego_polygon.outer().push_back(Point2d(front_m, width_right_m));
  ego_polygon.outer().push_back(Point2d(rear_m, width_right_m));
  ego_polygon.outer().push_back(Point2d(rear_m, width_left_m));

  bg::correct(ego_polygon); // 修正多边形

  return ego_polygon;
}
}  // namespace

// 构造函数
CollisionDetectorNode::CollisionDetectorNode(const rclcpp::NodeOptions & node_options)
: Node("collision_detector_node", node_options), updater_(this)
{
  // 参数初始化
  {
    auto & p = node_param_;
    p.use_pointcloud = this->declare_parameter<bool>("use_pointcloud"); // 是否使用点云
    p.use_dynamic_object = this->declare_parameter<bool>("use_dynamic_object"); // 是否使用动态对象
    p.collision_distance = this->declare_parameter<double>("collision_distance"); // 碰撞距离
    p.nearby_filter_radius = this->declare_parameter<double>("nearby_filter_radius"); // 附近过滤半径
    p.keep_ignoring_time = this->declare_parameter<double>("keep_ignoring_time"); // 忽略时间
    p.nearby_object_type_filters.filter_car =
      this->declare_parameter<bool>("nearby_object_type_filters.filter_car"); // 过滤汽车
    p.nearby_object_type_filters.filter_truck =
      this->declare_parameter<bool>("nearby_object_type_filters.filter_truck"); // 过滤卡车
    p.nearby_object_type_filters.filter_bus =
      this->declare_parameter<bool>("nearby_object_type_filters.filter_bus"); // 过滤公交车
    p.nearby_object_type_filters.filter_trailer =
      this->declare_parameter<bool>("nearby_object_type_filters.filter_trailer"); // 过滤拖车
    p.nearby_object_type_filters.filter_unknown =
      this->declare_parameter<bool>("nearby_object_type_filters.filter_unknown"); // 过滤未知对象
    p.nearby_object_type_filters.filter_bicycle =
      this->declare_parameter<bool>("nearby_object_type_filters.filter_bicycle"); // 过滤自行车
    p.nearby_object_type_filters.filter_motorcycle =
      this->declare_parameter<bool>("nearby_object_type_filters.filter_motorcycle"); // 过滤摩托车
    p.nearby_object_type_filters.filter_pedestrian =
      this->declare_parameter<bool>("nearby_object_type_filters.filter_pedestrian"); // 过滤行人
  }

  vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); // 获取车辆信息
  last_obstacle_found_stamp_ = this->now(); // 初始化最后发现障碍物的时间戳

  // 诊断更新器
  updater_.setHardwareID("collision_detector");
  updater_.add("collision_detect", this, &CollisionDetectorNode::checkCollision); // 添加碰撞检测任务
  updater_.setPeriod(0.1); // 设置更新周期
}

// 过滤对象
PredictedObjects CollisionDetectorNode::filterObjects(const PredictedObjects & input_objects)
{
  PredictedObjects filtered_objects;
  filtered_objects.header = input_objects.header;
  filtered_objects.header.stamp = this->now();

  const rclcpp::Time current_object_time = input_objects.header.stamp;
  const rclcpp::Duration observed_objects_keep_time =
    rclcpp::Duration::from_seconds(0.5);  //  0.5秒
  const rclcpp::Duration ignored_objects_keep_time =
    rclcpp::Duration::from_seconds(10.0);  // 10秒

  // 移除过期的对象
  removeOldObjects(observed_objects_, current_object_time, observed_objects_keep_time);
  removeOldObjects(ignored_objects_, current_object_time, ignored_objects_keep_time);

  // 获取从对象坐标系到base_link的变换
  const auto transform_stamped =
    getTransform("base_link", input_objects.header.frame_id, input_objects.header.stamp, 0.5);

  if (!transform_stamped) {
    RCLCPP_ERROR(this->get_logger(), "Failed to get transform from object frame to base_link");
    return filtered_objects;
  }

  Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.get().transform).cast<float>();

  for (const auto & object : input_objects.objects) {
    // 将对象位置转换到base_link坐标系
    Eigen::Vector3f object_position(
      object.kinematics.initial_pose_with_covariance.pose.position.x,
      object.kinematics.initial_pose_with_covariance.pose.position.y,
      object.kinematics.initial_pose_with_covariance.pose.position.z);
    Eigen::Vector3f transformed_position = isometry * object_position;

    // 计算对象与base_link的距离
    const double object_distance = transformed_position.head<2>().norm();
    const bool is_within_range = (object_distance <= node_param_.nearby_filter_radius);

    // 判断对象是否应该被排除
    bool should_be_excluded = shouldBeExcluded(object.classification.front().label);

    const bool is_within_range_and_filtering_class = is_within_range && should_be_excluded;

    // 如果对象不在范围内或不需要过滤，直接添加到结果中
    if (!is_within_range_and_filtering_class) {
      filtered_objects.objects.push_back(object);

      // 更新观察到的对象
      auto observed_it = std::find_if(
        observed_objects_.begin(), observed_objects_.end(),
        [&object](const auto & observed_object) {
          return observed_object.object_id == object.object_id;
        });
      if (observed_it != observed_objects_.end()) {
        observed_it->timestamp = current_object_time;
      } else {
        observed_objects_.push_back({object.object_id, current_object_time});
      }

      continue;
    }

    // 检查对象是否在忽略列表中
    auto ignored_it = std::find_if(
      ignored_objects_.begin(), ignored_objects_.end(), [&object](const auto & ignored_object) {
        return ignored_object.object_id == object.object_id;
      });
    const bool was_ignored = (ignored_it != ignored_objects_.end());

    // 如果对象被忽略且仍在忽略时间内，继续过滤
    if (
      was_ignored && (current_object_time - ignored_it->timestamp) <
                       rclcpp::Duration::from_seconds(node_param_.keep_ignoring_time)) {
      // 检查对象是否在观察列表中
      auto observed_it = std::find_if(
        observed_objects_.begin(), observed_objects_.end(),
        [&object](const auto & observed_object) {
          return observed_object.object_id == object.object_id;
        });
      const bool was_observed = (observed_it != observed_objects_.end());
      if (was_observed) {
        observed_it->timestamp = current_object_time;
      } else {
        // 添加到观察列表和忽略列表
        observed_objects_.push_back({object.object_id, current_object_time});
      }
      continue;
    }

    // 检查对象是否在观察列表中
    auto observed_it = std::find_if(
      observed_objects_.begin(), observed_objects_.end(), [&object](const auto & observed_object) {
        return observed_object.object_id == object.object_id;
      });
    const bool was_observed = (observed_it != observed_objects_.end());

    if (was_observed) {
      observed_it->timestamp = current_object_time;
      // 直接添加到结果中
      filtered_objects.objects.push_back(object);
    } else {
      // 添加到观察列表和忽略列表
      observed_objects_.push_back({object.object_id, current_object_time});
      ignored_objects_.push_back({object.object_id, current_object_time});
      // 继续过滤
      continue;
    }
  }

  return filtered_objects;
}

// 移除过期的对象
void CollisionDetectorNode::removeOldObjects(
  std::vector<TimestampedObject> & container, const rclcpp::Time & current_time,
  const rclcpp::Duration & duration_sec)
{
  container.erase(
    std::remove_if(
      container.begin(), container.end(),
      [&](const TimestampedObject & obj) { return (current_time - obj.timestamp) > duration_sec; }),
    container.end());
}

// 判断对象是否应该被排除
bool CollisionDetectorNode::shouldBeExcluded(
  const autoware_perception_msgs::msg::ObjectClassification::_label_type & classification) const
{
  switch (classification) {
    case autoware_perception_msgs::msg::ObjectClassification::CAR:
      return node_param_.nearby_object_type_filters.filter_car;
    case autoware_perception_msgs::msg::ObjectClassification::TRUCK:
      return node_param_.nearby_object_type_filters.filter_truck;
    case autoware_perception_msgs::msg::ObjectClassification::BUS:
      return node_param_.nearby_object_type_filters.filter_bus;
    case autoware_perception_msgs::msg::ObjectClassification::TRAILER:
      return node_param_.nearby_object_type_filters.filter_trailer;
    case autoware_perception_msgs::msg::ObjectClassification::UNKNOWN:
      return node_param_.nearby_object_type_filters.filter_unknown;
    case autoware_perception_msgs::msg::ObjectClassification::BICYCLE:
      return node_param_.nearby_object_type_filters.filter_bicycle;
    case autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE:
      return node_param_.nearby_object_type_filters.filter_motorcycle;
    case autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN:
      return node_param_.nearby_object_type_filters.filter_pedestrian;
    default:
      return false;
  }
}

// 检查碰撞
void CollisionDetectorNode::checkCollision(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
  odometry_ptr_ = sub_odometry_.takeData(); // 获取当前里程计数据

  if (!odometry_ptr_) {
    RCLCPP_INFO_THROTTLE(
      this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current odometry...");
    return;
  }

  pointcloud_ptr_ = sub_pointcloud_.takeData(); // 获取点云数据
  object_ptr_ = sub_dynamic_objects_.takeData(); // 获取动态对象数据
  operation_mode_ptr_ = sub_operation_mode_.takeData(); // 获取操作模式数据

  if (node_param_.use_pointcloud && !pointcloud_ptr_) {
    RCLCPP_WARN_THROTTLE(
      this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pointcloud info...");
    return;
  }

  if (node_param_.use_dynamic_object && !object_ptr_) {
    RCLCPP_WARN_THROTTLE(
      this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info...");
    return;
  }

  if (!operation_mode_ptr_) {
    RCLCPP_WARN_THROTTLE(
      this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for operation mode info...");
    return;
  }
  filtered_object_ptr_ = std::make_shared<PredictedObjects>(filterObjects(*object_ptr_)); // 过滤对象

  const auto nearest_obstacle = getNearestObstacle(); // 获取最近的障碍物

  const auto is_obstacle_found =
    !nearest_obstacle ? false : nearest_obstacle.get().first < node_param_.collision_distance;

  if (is_obstacle_found) {
    last_obstacle_found_stamp_ = this->now(); // 更新最后发现障碍物的时间戳
  }

  const auto is_obstacle_found_recently =
    (this->now() - last_obstacle_found_stamp_).seconds() < 1.0;

  diagnostic_msgs::msg::DiagnosticStatus status;
  if (
    operation_mode_ptr_->mode == OperationModeState::AUTONOMOUS &&
    (is_obstacle_found || is_obstacle_found_recently)) {
    status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
    status.message = "collision detected";
    if (is_obstacle_found) {
      stat.addf("Distance to nearest neighbor object", "%lf", nearest_obstacle.get().first);
    }
  } else {
    status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  }

  stat.summary(status.level, status.message);
}

// 获取最近的障碍物
boost::optional<Obstacle> CollisionDetectorNode::getNearestObstacle() const
{
  boost::optional<Obstacle> nearest_pointcloud{boost::none};
  boost::optional<Obstacle> nearest_object{boost::none};

  if (node_param_.use_pointcloud) {
    nearest_pointcloud = getNearestObstacleByPointCloud(); // 从点云中获取最近的障碍物
  }

  if (node_param_.use_dynamic_object) {
    nearest_object = getNearestObstacleByDynamicObject(); // 从动态对象中获取最近的障碍物
  }

  if (!nearest_pointcloud && !nearest_object) {
    return {};
  }

  if (!nearest_pointcloud) {
    return nearest_object;
  }

  if (!nearest_object) {
    return nearest_pointcloud;
  }

  return nearest_pointcloud.get().first < nearest_object.get().first ? nearest_pointcloud
                                                                     : nearest_object;
}

// 从点云中获取最近的障碍物
boost::optional<Obstacle> CollisionDetectorNode::getNearestObstacleByPointCloud() const
{
  const auto transform_stamped =
    getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5);

  geometry_msgs::msg::Point nearest_point;
  auto minimum_distance = std::numeric_limits<double>::max();

  if (!transform_stamped) {
    return {};
  }

  Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.get().transform).cast<float>();
  pcl::PointCloud<pcl::PointXYZ> transformed_pointcloud;
  pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud);
  pcl::transformPointCloud(transformed_pointcloud, transformed_pointcloud, isometry);

  const auto ego_polygon = createSelfPolygon(vehicle_info_);

  for (const auto & p : transformed_pointcloud) {
    Point2d boost_point(p.x, p.y);

    const auto distance_to_object = bg::distance(ego_polygon, boost_point);

    if (distance_to_object < minimum_distance) {
      nearest_point = createPoint(p.x, p.y, p.z);
      minimum_distance = distance_to_object;
    }
  }

  return std::make_pair(minimum_distance, nearest_point);
}

// 从动态对象中获取最近的障碍物
boost::optional<Obstacle> CollisionDetectorNode::getNearestObstacleByDynamicObject() const
{
  const auto transform_stamped = getTransform(
    filtered_object_ptr_->header.frame_id, "base_link", filtered_object_ptr_->header.stamp, 0.5);

  geometry_msgs::msg::Point nearest_point;
  auto minimum_distance = std::numeric_limits<double>::max();

  if (!transform_stamped) {
    return {};
  }

  tf2::Transform tf_src2target;
  tf2::fromMsg(transform_stamped.get().transform, tf_src2target);

  const auto ego_polygon = createSelfPolygon(vehicle_info_);

  for (const auto & object : filtered_object_ptr_->objects) {
    const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;

    tf2::Transform tf_src2object;
    tf2::fromMsg(object_pose, tf_src2object);

    geometry_msgs::msg::Pose transformed_object_pose;
    tf2::toMsg(tf_src2target.inverse() * tf_src2object, transformed_object_pose);

    const auto object_polygon = [&]() {
      switch (object.shape.type) {
        case Shape::POLYGON:
          return createObjPolygon(transformed_object_pose, object.shape.footprint);
        case Shape::CYLINDER:
          return createObjPolygonForCylinder(transformed_object_pose, object.shape.dimensions.x);
        case Shape::BOUNDING_BOX:
          return createObjPolygon(transformed_object_pose, object.shape.dimensions);
        default:
          // 不支持的类型
          RCLCPP_WARN(this->get_logger(), "Unsupported shape type: %d", object.shape.type);
          return createObjPolygon(transformed_object_pose, object.shape.dimensions);
      }
    }();

    const auto distance_to_object = bg::distance(ego_polygon, object_polygon);

    if (distance_to_object < minimum_distance) {
      nearest_point = object_pose.position;
      minimum_distance = distance_to_object;
    }
  }

  return std::make_pair(minimum_distance, nearest_point);
}

// 获取坐标系之间的变换
boost::optional<geometry_msgs::msg::TransformStamped> CollisionDetectorNode::getTransform(
  const std::string & source, const std::string & target, const rclcpp::Time & stamp,
  double duration_sec) const
{
  geometry_msgs::msg::TransformStamped transform_stamped;

  try {
    transform_stamped =
      tf_buffer_.lookupTransform(source, target, stamp, tf2::durationFromSec(duration_sec));
  } catch (const tf2::TransformException & ex) {
    return {};
  }

  return transform_stamped;
}

}  // namespace autoware::collision_detector

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::collision_detector::CollisionDetectorNode)

三、autoware_control_validator

这个是控制验证模块，主要用于验证控制输出的合法性。
也是一个监控模块，不会去干预车辆的控制。

主要作用：
1.实时监控控制输出的合法性
2.检测异常行为（如超速、倒车）
3.发布诊断信息
4.可视化警告信息

主要监控三部分：
1.横向偏差
2.速度偏差
3.累积错误计数

class ControlValidator {
  // 验证三个主要方面
  void validate() {
    // 1. 横向偏差验证
    calc_lateral_deviation_status();
    
    // 2. 速度偏差验证
    calc_velocity_deviation_status();
    
    // 3. 累计错误计数
    validation_status_.invalid_count = 
      is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1;
  }
};

四、autoware_external_cmd_selector

这部分是处理外部指令的，用于在本地控制(local)和远程控制(remote)之间进行切换。
都是人工干预命令，和自动驾驶不是一条线。
本地控制就是安全员在车内的控制。
远程控制就是远程操控中心的控制。

主要作用：
1.命令源选择：在本地控制和远程控制之间切换
2.命令转发：将选中源的命令转发给执行系统
3.安全保障：
心跳检测确保控制源活跃
模式切换服务确保可控的切换过程
4.诊断监控：提供系统状态诊断信息

主要功能：

class ExternalCmdSelector {
  // 两种控制模式
  enum CommandSourceMode {
    LOCAL,   // 本地控制
    REMOTE   // 远程控制
  };
  
  // 处理不同来源的命令
  void onLocalControlCmd();   // 处理本地控制命令
  void onRemoteControlCmd();  // 处理远程控制命令
}

输入命令类型：

// 本地和远程都接收这些类型的命令
sub_local_control_cmd_      // 控制命令(速度、转向等)
sub_local_shift_cmd_        // 档位命令
sub_local_turn_signal_cmd_  // 转向灯命令
sub_local_heartbeat_        // 心跳信号

命令选择逻辑：

void onLocalControlCmd(const ExternalControlCommand::ConstSharedPtr msg) {
  // 只有在本地模式下才转发本地命令
  if (current_selector_mode_.data != CommandSourceMode::LOCAL) {
    return;
  }
  pub_control_cmd_->publish(*msg);
}

配置参数：

ros__parameters:
  update_rate: 10.0                 # 更新频率
  initial_selector_mode: "local"    # 初始模式：local或remote

这个和自动驾驶关系也不大，需要的时候再来看就行。
就一个cpp文件，源码注释：

// Copyright 2020 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 "autoware/external_cmd_selector/external_cmd_selector_node.hpp"

#include <tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp>

#include <chrono>
#include <memory>
#include <string>
#include <utility>

namespace autoware::external_cmd_selector
{
// 构造函数
ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_options)
: Node("external_cmd_selector", node_options)
{
  using std::placeholders::_1;
  using std::placeholders::_2;

  // 参数
  double update_rate = declare_parameter<double>("update_rate"); // 更新频率
  std::string initial_selector_mode = declare_parameter<std::string>("initial_selector_mode"); // 初始选择模式

  // 发布器
  pub_current_selector_mode_ =
    create_publisher<CommandSourceMode>("~/output/current_selector_mode", 1); // 当前选择模式
  pub_control_cmd_ = create_publisher<ExternalControlCommand>("~/output/control_cmd", 1); // 控制命令
  pub_shift_cmd_ = create_publisher<InternalGearShift>("~/output/gear_cmd", 1); // 换挡命令
  pub_turn_signal_cmd_ = create_publisher<InternalTurnSignal>("~/output/turn_indicators_cmd", 1); // 转向灯命令
  pub_hazard_signal_cmd_ = create_publisher<InternalHazardSignal>("~/output/hazard_lights_cmd", 1); // 危险灯命令
  pub_heartbeat_ = create_publisher<InternalHeartbeat>("~/output/heartbeat", 1); // 心跳信号

  // 回调组
  callback_group_subscribers_ =
    this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // 订阅者回调组
  callback_group_services_ =
    this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // 服务回调组

  auto subscriber_option = rclcpp::SubscriptionOptions();
  subscriber_option.callback_group = callback_group_subscribers_;

  // 订阅者
  sub_local_control_cmd_ = create_subscription<ExternalControlCommand>(
    "~/input/local/control_cmd", 1, std::bind(&ExternalCmdSelector::onLocalControlCmd, this, _1),
    subscriber_option); // 本地控制命令
  sub_local_shift_cmd_ = create_subscription<ExternalGearShift>(
    "~/input/local/shift_cmd", 1, std::bind(&ExternalCmdSelector::onLocalShiftCmd, this, _1),
    subscriber_option); // 本地换挡命令
  sub_local_turn_signal_cmd_ = create_subscription<ExternalTurnSignal>(
    "~/input/local/turn_signal_cmd", 1,
    std::bind(&ExternalCmdSelector::onLocalTurnSignalCmd, this, _1), subscriber_option); // 本地转向灯命令
  sub_local_heartbeat_ = create_subscription<ExternalHeartbeat>(
    "~/input/local/heartbeat", 1, std::bind(&ExternalCmdSelector::onLocalHeartbeat, this, _1),
    subscriber_option); // 本地心跳信号

  sub_remote_control_cmd_ = create_subscription<ExternalControlCommand>(
    "~/input/remote/control_cmd", 1, std::bind(&ExternalCmdSelector::onRemoteControlCmd, this, _1),
    subscriber_option); // 远程控制命令
  sub_remote_shift_cmd_ = create_subscription<ExternalGearShift>(
    "~/input/remote/shift_cmd", 1, std::bind(&ExternalCmdSelector::onRemoteShiftCmd, this, _1),
    subscriber_option); // 远程换挡命令
  sub_remote_turn_signal_cmd_ = create_subscription<ExternalTurnSignal>(
    "~/input/remote/turn_signal_cmd", 1,
    std::bind(&ExternalCmdSelector::onRemoteTurnSignalCmd, this, _1), subscriber_option); // 远程转向灯命令
  sub_remote_heartbeat_ = create_subscription<ExternalHeartbeat>(
    "~/input/remote/heartbeat", 1, std::bind(&ExternalCmdSelector::onRemoteHeartbeat, this, _1),
    subscriber_option); // 远程心跳信号

  // 服务
  srv_select_external_command_ = create_service<CommandSourceSelect>(
    "~/service/select_external_command",
    std::bind(&ExternalCmdSelector::onSelectExternalCommandService, this, _1, _2),
    rmw_qos_profile_services_default, callback_group_services_); // 选择外部命令服务

  // 初始化模式
  auto convert_selector_mode = [](const std::string & mode_text) {
    if (mode_text == "local") {
      return CommandSourceMode::LOCAL; // 本地模式
    }
    if (mode_text == "remote") {
      return CommandSourceMode::REMOTE; // 远程模式
    }
    throw std::invalid_argument("unknown selector mode"); // 未知模式
  };
  current_selector_mode_.data = convert_selector_mode(initial_selector_mode); // 设置初始模式

  // 诊断更新器
  updater_.setHardwareID("external_cmd_selector");
  updater_.add("heartbeat", [](auto & stat) {
    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Alive"); // 心跳状态
  });

  // 定时器
  const auto period_ns = rclcpp::Rate(update_rate).period();
  timer_ = rclcpp::create_timer(
    this, get_clock(), period_ns, std::bind(&ExternalCmdSelector::onTimer, this),
    callback_group_subscribers_); // 定时器回调
}

// 处理本地控制命令
void ExternalCmdSelector::onLocalControlCmd(const ExternalControlCommand::ConstSharedPtr msg)
{
  if (current_selector_mode_.data != CommandSourceMode::LOCAL) {
    return; // 如果当前模式不是本地模式，则忽略
  }
  pub_control_cmd_->publish(*msg); // 发布控制命令
}

// 处理本地换挡命令
void ExternalCmdSelector::onLocalShiftCmd(const ExternalGearShift::ConstSharedPtr msg)
{
  if (current_selector_mode_.data != CommandSourceMode::LOCAL) {
    return; // 如果当前模式不是本地模式，则忽略
  }
  pub_shift_cmd_->publish(convert(*msg)); // 发布换挡命令
}

// 处理本地转向灯命令
void ExternalCmdSelector::onLocalTurnSignalCmd(const ExternalTurnSignal::ConstSharedPtr msg)
{
  if (current_selector_mode_.data != CommandSourceMode::LOCAL) {
    return; // 如果当前模式不是本地模式，则忽略
  }
  auto light_signal = tier4_auto_msgs_converter::convert(*msg); // 转换信号
  pub_turn_signal_cmd_->publish(light_signal.turn_signal); // 发布转向灯命令
  pub_hazard_signal_cmd_->publish(light_signal.hazard_signal); // 发布危险灯命令
}

// 处理本地心跳信号
void ExternalCmdSelector::onLocalHeartbeat(const ExternalHeartbeat::ConstSharedPtr msg)
{
  if (current_selector_mode_.data != CommandSourceMode::LOCAL) {
    return; // 如果当前模式不是本地模式，则忽略
  }
  pub_heartbeat_->publish(convert(*msg)); // 发布心跳信号
}

// 处理远程控制命令
void ExternalCmdSelector::onRemoteControlCmd(const ExternalControlCommand::ConstSharedPtr msg)
{
  if (current_selector_mode_.data != CommandSourceMode::REMOTE) {
    return; // 如果当前模式不是远程模式，则忽略
  }
  pub_control_cmd_->publish(*msg); // 发布控制命令
}

// 处理远程换挡命令
void ExternalCmdSelector::onRemoteShiftCmd(const ExternalGearShift::ConstSharedPtr msg)
{
  if (current_selector_mode_.data != CommandSourceMode::REMOTE) {
    return; // 如果当前模式不是远程模式，则忽略
  }
  pub_shift_cmd_->publish(convert(*msg)); // 发布换挡命令
}

// 处理远程转向灯命令
void ExternalCmdSelector::onRemoteTurnSignalCmd(const ExternalTurnSignal::ConstSharedPtr msg)
{
  if (current_selector_mode_.data != CommandSourceMode::REMOTE) {
    return; // 如果当前模式不是远程模式，则忽略
  }
  auto light_signal = tier4_auto_msgs_converter::convert(*msg); // 转换信号
  pub_turn_signal_cmd_->publish(light_signal.turn_signal); // 发布转向灯命令
  pub_hazard_signal_cmd_->publish(light_signal.hazard_signal); // 发布危险灯命令
}

// 处理远程心跳信号
void ExternalCmdSelector::onRemoteHeartbeat(const ExternalHeartbeat::ConstSharedPtr msg)
{
  if (current_selector_mode_.data != CommandSourceMode::REMOTE) {
    return; // 如果当前模式不是远程模式，则忽略
  }
  pub_heartbeat_->publish(convert(*msg)); // 发布心跳信号
}

// 处理选择外部命令服务
bool ExternalCmdSelector::onSelectExternalCommandService(
  const CommandSourceSelect::Request::SharedPtr req,
  const CommandSourceSelect::Response::SharedPtr res)
{
  current_selector_mode_.data = req->mode.data; // 更新当前选择模式
  res->success = true; // 设置响应成功
  res->message = "Success."; // 设置响应消息
  return true;
}

// 定时器回调
void ExternalCmdSelector::onTimer()
{
  pub_current_selector_mode_->publish(current_selector_mode_); // 发布当前选择模式
  updater_.force_update(); // 强制更新诊断
}

// 转换换挡命令
ExternalCmdSelector::InternalGearShift ExternalCmdSelector::convert(
  const ExternalGearShift & command)
{
  return tier4_auto_msgs_converter::convert(command); // 使用转换工具转换
}

// 转换心跳信号
ExternalCmdSelector::InternalHeartbeat ExternalCmdSelector::convert(
  const ExternalHeartbeat & command)
{
  return command; // 直接返回
}
}  // namespace autoware::external_cmd_selector

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::external_cmd_selector::ExternalCmdSelector)

主要功能说明：

    ExternalCmdSelector: 外部命令选择器节点，用于选择本地或远程控制命令。

    onLocalControlCmd: 处理本地控制命令。

    onLocalShiftCmd: 处理本地换挡命令。

    onLocalTurnSignalCmd: 处理本地转向灯命令。

    onLocalHeartbeat: 处理本地心跳信号。

    onRemoteControlCmd: 处理远程控制命令。

    onRemoteShiftCmd: 处理远程换挡命令。

    onRemoteTurnSignalCmd: 处理远程转向灯命令。

    onRemoteHeartbeat: 处理远程心跳信号。

    onSelectExternalCommandService: 处理选择外部命令服务。

    onTimer: 定时器回调，发布当前选择模式并更新诊断。

    convert: 转换换挡命令和心跳信号。

五、autoware_joy_controller

这个是手柄控制模块，支持多种游戏手柄来控制车辆。
负责在自动驾驶和手动控制之间切换，并提供手动控制功能。
和上面的那个选择控制源模块配合使用，两个功能包具体的工作流程如下：

joy_controller 将手柄输入转换为控制命令
命令发送到 external_cmd_selector
external_cmd_selector 根据当前模式选择是否转发这些命令

支持的手柄类型：

joy_type: 
  - DS4    # PlayStation 4手柄
  - G29    # Logitech G29方向盘
  - P65    # P65手柄
  - XBOX   # Xbox手柄

主要控制功能：

void AutowareJoyControllerNode::onTimer() {
  // 基本控制
  publishControlCommand();      // 发布控制命令(转向、加速、制动)
  publishExternalControlCommand(); // 发布外部控制命令
  
  // 特殊功能
  publishShift();              // 换挡
  publishTurnSignal();         // 转向灯
  publishGateMode();           // 控制模式切换
  publishAutowareEngage();     // Autoware使能控制
  publishVehicleEngage();      // 车辆使能控制
}

关键参数配置：

ros__parameters:
  update_rate: 10.0           # 更新频率
  accel_ratio: 3.0           # 加速比例
  brake_ratio: 5.0           # 制动比例
  steer_ratio: 0.5           # 转向比例
  max_forward_velocity: 20.0  # 最大前进速度
  max_backward_velocity: 3.0  # 最大后退速度

控制模式：

// 两种控制模式
GateMode::AUTO      // 自动驾驶模式
GateMode::EXTERNAL  // 手动控制模式

// 可以通过手柄切换模式
void publishGateMode() {
  if (prev_gate_mode_ == GateMode::AUTO) {
    gate_mode.data = GateMode::EXTERNAL;
  }
}

就是一个遥控模块，需要的时候再来看。
就一个cpp文件，源码注释如下：

// Copyright 2020 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
//
//     <url id="ctv28g1gem4fc97r3g0" type="url" status="parsed" title="Apache License, Version 2.0" wc="10467">http://www.apache.org/licenses/LICENSE-2.0</url> 
//
// 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 "autoware/joy_controller/joy_controller.hpp"
#include "autoware/joy_controller/joy_converter/ds4_joy_converter.hpp"
#include "autoware/joy_controller/joy_converter/g29_joy_converter.hpp"
#include "autoware/joy_controller/joy_converter/p65_joy_converter.hpp"
#include "autoware/joy_controller/joy_converter/xbox_joy_converter.hpp"

#include <tier4_api_utils/tier4_api_utils.hpp>

#include <algorithm>
#include <memory>
#include <string>

// 命名空间声明
namespace
{
    // 使用autoware::joy_controller命名空间中的类型
    using autoware::joy_controller::GateModeType;
    using autoware::joy_controller::GearShiftType;
    using autoware::joy_controller::TurnSignalType;
    using GearShift = tier4_external_api_msgs::msg::GearShift;
    using TurnSignal = tier4_external_api_msgs::msg::TurnSignal;
    using GateMode = tier4_control_msgs::msg::GateMode;

    // 获取上一个档位
    GearShiftType getUpperShift(const GearShiftType & shift)
    {
        // 根据当前档位返回上一个档位
        if (shift == GearShift::NONE) {
            return GearShift::PARKING;
        }
        if (shift == GearShift::PARKING) {
            return GearShift::REVERSE;
        }
        if (shift == GearShift::REVERSE) {
            return GearShift::NEUTRAL;
        }
        if (shift == GearShift::NEUTRAL) {
            return GearShift::DRIVE;
        }
        if (shift == GearShift::DRIVE) {
            return GearShift::LOW;
        }
        if (shift == GearShift::LOW) {
            return GearShift::LOW;
        }

        return GearShift::NONE;
    }

    // 获取下一个档位
    GearShiftType getLowerShift(const GearShiftType & shift)
    {
        // 根据当前档位返回下一个档位
        if (shift == GearShift::NONE) {
            return GearShift::PARKING;
        }
        if (shift == GearShift::PARKING) {
            return GearShift::PARKING;
        }
        if (shift == GearShift::REVERSE) {
            return GearShift::PARKING;
        }
        if (shift == GearShift::NEUTRAL) {
            return GearShift::REVERSE;
        }
        if (shift == GearShift::DRIVE) {
            return GearShift::NEUTRAL;
        }
        if (shift == GearShift::LOW) {
            return GearShift::DRIVE;
        }

        return GearShift::NONE;
    }

    // 获取档位名称
    const char * getShiftName(const GearShiftType & shift)
    {
        // 根据档位类型返回对应的名称
        if (shift == GearShift::NONE) {
            return "NONE";
        }
        if (shift == GearShift::PARKING) {
            return "PARKING";
        }
        if (shift == GearShift::REVERSE) {
            return "REVERSE";
        }
        if (shift == GearShift::NEUTRAL) {
            return "NEUTRAL";
        }
        if (shift == GearShift::DRIVE) {
            return "DRIVE";
        }
        if (shift == GearShift::LOW) {
            return "LOW";
        }

        return "NOT_SUPPORTED";
    }

    // 获取转向灯名称
    const char * getTurnSignalName(const TurnSignalType & turn_signal)
    {
        // 根据转向灯类型返回对应的名称
        if (turn_signal == TurnSignal::NONE) {
            return "NONE";
        }
        if (turn_signal == TurnSignal::LEFT) {
            return "LEFT";
        }
        if (turn_signal == TurnSignal::RIGHT) {
            return "RIGHT";
        }
        if (turn_signal == TurnSignal::HAZARD) {
            return "HAZARD";
        }

        return "NOT_SUPPORTED";
    }

    // 获取门模式名称
    const char * getGateModeName(const GateModeType & gate_mode)
    {
        // 根据门模式类型返回对应的名称
        using tier4_control_msgs::msg::GateMode;

        if (gate_mode == GateMode::AUTO) {
            return "AUTO";
        }
        if (gate_mode == GateMode::EXTERNAL) {
            return "EXTERNAL";
        }

        return "NOT_SUPPORTED";
    }

    // 计算映射值
    double calcMapping(const double input, const double sensitivity)
    {
        // 根据输入值和灵敏度计算映射值
        const double exponent = 1.0 / (std::max(0.001, std::min(1.0, sensitivity)));
        return std::pow(input, exponent);
    }

}  // namespace

// autoware::joy_controller命名空间
namespace autoware::joy_controller
{
    // 乔伊控制器节点类
    void AutowareJoyControllerNode::onJoy()
    {
        // 获取乔伊消息
        const auto msg = sub_joy_.takeData();
        if (!msg) {
            return;
        }

        // 更新最后接收到乔伊消息的时间
        last_joy_received_time_ = msg->header.stamp;
        // 根据乔伊类型创建对应的乔伊转换器
        if (joy_type_ == "G29") {
            joy_ = std::make_shared<const G29JoyConverter>(*msg);
        } else if (joy_type_ == "DS4") {
            joy_ = std::make_shared<const DS4JoyConverter>(*msg);
        } else if (joy_type_ == "XBOX") {
            joy_ = std::make_shared<const XBOXJoyConverter>(*msg);
        } else {
            joy_ = std::make_shared<const P65JoyConverter>(*msg);
        }

        // 处理档位相关操作
        if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) {
            publishShift();
        }

        // 处理转向灯相关操作
        if (joy_->turn_signal_left() || joy_->turn_signal_right() || joy_->clear_turn_signal()) {
            publishTurnSignal();
        }

        // 处理门模式相关操作
        if (joy_->gate_mode()) {
            publishGateMode();
        }

        // 处理自动驾驶相关操作
        if (joy_->autoware_engage() || joy_->autoware_disengage()) {
            publishAutowareEngage();
        }

        // 处理车辆相关操作
        if (joy_->vehicle_engage() || joy_->vehicle_disengage()) {
            publishVehicleEngage();
        }

        // 处理紧急停止相关操作
        if (joy_->emergency_stop()) {
            sendEmergencyRequest(true);
        }

        // 处理清除紧急停止相关操作
        if (joy_->clear_emergency_stop()) {
            sendEmergencyRequest(false);
        }
    }

    // 处理里程计消息
    void AutowareJoyControllerNode::onOdometry()
    {
        // 如果是原始控制模式，则直接返回
        if (raw_control_) {
            return;
        }

        // 获取里程计消息
        const auto msg = sub_odom_.takeData();
        if (!msg) {
            return;
        }

        // 创建并更新里程计消息
        auto twist = std::make_shared<geometry_msgs::msg::TwistStamped>();
        twist->header = msg->header;
        twist->twist = msg->twist.twist;

        twist_ = twist;
    }

    // 检查数据是否准备好
    bool AutowareJoyControllerNode::isDataReady()
    {
        // 检查乔伊消息是否准备好
        {
            if (!joy_) {
                RCLCPP_WARN_THROTTLE(
                    get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(),
                    "waiting for joy msg...");
                return false;
            }

            // 检查乔伊消息是否超时
            constexpr auto timeout = 2.0;
            const auto time_diff = this->now() - last_joy_received_time_;
            if (time_diff.seconds() > timeout) {
                RCLCPP_WARN_THROTTLE(
                    get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(),
                    "joy msg is timeout");
                return false;
            }
        }

        // 检查里程计消息是否准备好
        if (!raw_control_) {
            if (!twist_) {
                RCLCPP_WARN_THROTTLE(
                    get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(),
                    "waiting for twist msg...");
                return false;
            }

            // 检查里程计消息是否超时
            constexpr auto timeout = 0.5;
            const auto time_diff = this->now() - twist_->header.stamp;
            if (time_diff.seconds() > timeout) {
                RCLCPP_WARN_THROTTLE(
                    get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(),
                    "twist msg is timeout");
                return false;
            }
        }

        return true;
    }

    // 定时器回调函数
    void AutowareJoyControllerNode::onTimer()
    {
        // 处理里程计消息
        onOdometry();
        // 处理乔伊消息
        onJoy();

        // 如果数据没有准备好，则直接返回
        if (!isDataReady()) {
            return;
        }

        // 发布控制命令
        publishControlCommand();
        // 发布外部控制命令
        publishExternalControlCommand();
        // 发布心跳消息
        publishHeartbeat();
    }

    // 发布控制命令
    void AutowareJoyControllerNode::publishControlCommand()
    {
        // 创建控制命令消息
        autoware_control_msgs::msg::Control cmd;
        cmd.stamp = this->now();
        {
            // 计算转向角度和转向角速度
            cmd.lateral.steering_tire_angle = steer_ratio_ * joy_->steer();
            cmd.lateral.steering_tire_rotation_rate = steering_angle_velocity_;

            // 处理加速操作
            if (joy_->accel()) {
                cmd.longitudinal.acceleration = accel_ratio_ * joy_->accel();
                cmd.longitudinal.velocity =
                    twist_->twist.linear.x + velocity_gain_ * cmd.longitudinal.acceleration;
                cmd.longitudinal.velocity =
                    std::min(cmd.longitudinal.velocity, static_cast<float>(max_forward_velocity_));
            }

            // 处理制动操作
            if (joy_->brake()) {
                cmd.longitudinal.velocity = 0.0;
                cmd.longitudinal.acceleration = -brake_ratio_ * joy_->brake();
            }

            // 处理反向加速操作
            if (joy_->accel() && joy_->brake()) {
                cmd.longitudinal.acceleration = backward_accel_ratio_ * joy_->accel();
                cmd.longitudinal.velocity =
                    twist_->twist.linear.x - velocity_gain_ * cmd.longitudinal.acceleration;
                cmd.longitudinal.velocity =
                    std::max(cmd.longitudinal.velocity, static_cast<float>(-max_backward_velocity_));
            }
        }

        // 发布控制命令消息
        pub_control_command_->publish(cmd);
        prev_control_command_ = cmd;
    }

    // 发布外部控制命令
    void AutowareJoyControllerNode::publishExternalControlCommand()
    {
        // 创建外部控制命令消息
        tier4_external_api_msgs::msg::ControlCommandStamped cmd_stamped;
        cmd_stamped.stamp = this->now();
        {
            auto & cmd = cmd_stamped.control;

            // 计算转向角度和转向角速度
            cmd.steering_angle = steer_ratio_ * joy_->steer();
            cmd.steering_angle_velocity = steering_angle_velocity_;
            // 计算油门和制动值
            cmd.throttle =
                accel_ratio_ * calcMapping(static_cast<double>(joy_->accel()), accel_sensitivity_);
            cmd.brake = brake_ratio_ * calcMapping(static_cast<double>(joy_->brake()), brake_sensitivity_);
        }

        // 发布外部控制命令消息
        pub_external_control_command_->publish(cmd_stamped);
        prev_external_control_command_ = cmd_stamped.control;
    }

    // 发布档位消息
    void AutowareJoyControllerNode::publishShift()
    {
        // 创建档位消息
        tier4_external_api_msgs::msg::GearShiftStamped gear_shift;
        gear_shift.stamp = this->now();

        // 处理档位相关操作
        if (joy_->shift_up()) {
            gear_shift.gear_shift.data = getUpperShift(prev_shift_);
        }

        if (joy_->shift_down()) {
            gear_shift.gear_shift.data = getLowerShift(prev_shift_);
        }

        if (joy_->shift_drive()) {
            gear_shift.gear_shift.data = GearShift::DRIVE;
        }

        if (joy_->shift_reverse()) {
            gear_shift.gear_shift.data = GearShift::REVERSE;
        }

        // 打印档位信息
        RCLCPP_INFO(get_logger(), "GearShift::%s", getShiftName(gear_shift.gear_shift.data));

        // 发布档位消息
        pub_shift_->publish(gear_shift);
        prev_shift_ = gear_shift.gear_shift.data;
    }

    // 发布转向灯消息
    void AutowareJoyControllerNode::publishTurnSignal()
    {
        // 创建转向灯消息
        tier4_external_api_msgs::msg::TurnSignalStamped turn_signal;
        turn_signal.stamp = this->now();

        // 处理转向灯相关操作
        if (joy_->turn_signal_left() && joy_->turn_signal_right()) {
            turn_signal.turn_signal.data = TurnSignal::HAZARD;
        } else if (joy_->turn_signal_left()) {
            turn_signal.turn_signal.data = TurnSignal::LEFT;
        } else if (joy_->turn_signal_right()) {
            turn_signal.turn_signal.data = TurnSignal::RIGHT;
        }

        if (joy_->clear_turn_signal()) {
            turn_signal.turn_signal.data = TurnSignal::NONE;
        }

        // 打印转向灯信息
        RCLCPP_INFO(get_logger(), "TurnSignal::%s", getTurnSignalName(turn_signal.turn_signal.data));

        // 发布转向灯消息
        pub_turn_signal_->publish(turn_signal);
    }

    // 发布门模式消息
    void AutowareJoyControllerNode::publishGateMode()
    {
        // 创建门模式消息
        tier4_control_msgs::msg::GateMode gate_mode;

        // 处理门模式相关操作
        if (prev_gate_mode_ == GateMode::AUTO) {
            gate_mode.data = GateMode::EXTERNAL;
        }

        if (prev_gate_mode_ == GateMode::EXTERNAL) {
            gate_mode.data = GateMode::AUTO;
        }

        // 打印门模式信息
        RCLCPP_INFO(get_logger(), "GateMode::%s", getGateModeName(gate_mode.data));

        // 发布门模式消息
        pub_gate_mode_->publish(gate_mode);
        prev_gate_mode_ = gate_mode.data;
    }

    // 发布心跳消息
    void AutowareJoyControllerNode::publishHeartbeat()
    {
        // 创建心跳消息
        tier4_external_api_msgs::msg::Heartbeat heartbeat;
        heartbeat.stamp = this->now();
        // 发布心跳消息
        pub_heartbeat_->publish(heartbeat);
    }

    // 发送紧急停止请求
    void AutowareJoyControllerNode::sendEmergencyRequest(bool emergency)
    {
        // 打印紧急停止信息
        RCLCPP_INFO(get_logger(), "%s emergency stop", emergency ? "Set" : "Clear");

        // 创建紧急停止请求
        auto request = std::make_shared<tier4_external_api_msgs::srv::SetEmergency::Request>();
        request->emergency = emergency;

        // 发送紧急停止请求
        client_emergency_stop_->async_send_request(
            request,
            [this](rclcpp::Client<tier4_external_api_msgs::srv::SetEmergency>::SharedFuture result) {
                auto response = result.get();
                if (tier4_api_utils::is_success(response->status)) {
                    RCLCPP_INFO(get_logger(), "service succeeded");
                } else {
                    RCLCPP_WARN(get_logger(), "service failed: %s", response->status.message.c_str());
                }
            });
    }

    // 发布自动驾驶启动/停止请求
    void AutowareJoyControllerNode::publishAutowareEngage()
    {
        // 创建自动驾驶启动/停止请求
        auto req = std::make_shared<tier4_external_api_msgs::srv::Engage::Request>();
        if (joy_->autoware_engage()) {
            req->engage = true;
            RCLCPP_INFO(get_logger(), "Autoware Engage");
        }

        if (joy_->autoware_disengage()) {
            req->engage = false;
            RCLCPP_INFO(get_logger(), "Autoware Disengage");
        }

        // 检查服务是否可用
        if (!client_autoware_engage_->service_is_ready()) {
            RCLCPP_WARN(get_logger(), "%s is unavailable", client_autoware_engage_->get_service_name());
            return;
        }

        // 发送自动驾驶启动/停止请求
        client_autoware_engage_->async_send_request(
            req, [this](rclcpp::Client<tier4_external_api_msgs::srv::Engage>::SharedFuture result) {
                RCLCPP_INFO(
                    get_logger(), "%s: %d, %s", client_autoware_engage_->get_service_name(),
                    result.get()->status.code, result.get()->status.message.c_str());
            });
    }

    // 发布车辆启动/停止请求
    void AutowareJoyControllerNode::publishVehicleEngage()
    {
        // 创建车辆启动/停止请求
        autoware_vehicle_msgs::msg::Engage engage;

        if (joy_->vehicle_engage()) {
            engage.engage = true;
            RCLCPP_INFO(get_logger(), "Vehicle Engage");
        }

        if (joy_->vehicle_disengage()) {
            engage.engage = false;
            RCLCPP_INFO(get_logger(), "Vehicle Disengage");
        }

        // 发布车辆启动/停止请求
        pub_vehicle_engage_->publish(engage);
    }

    // 初始化定时器
    void AutowareJoyControllerNode::initTimer(double period_s)
    {
        // 将周期转换为纳秒
        const auto period_ns =
            std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
        // 创建定时器
        timer_ = rclcpp::create_timer(
            this, get_clock(), period_ns, std::bind(&AutowareJoyControllerNode::onTimer, this));
    }

    // 构造函数
    AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options)
        : Node("autoware_joy_controller", node_options)
    {
        // 获取参数
        joy_type_ = declare_parameter<std::string>("joy_type");
        update_rate_ = declare_parameter<double>("update_rate");
        accel_ratio_ = declare_parameter<double>("accel_ratio");
        brake_ratio_ = declare_parameter<double>("brake_ratio");
        steer_ratio_ = declare_parameter<double>("steer_ratio");
        steering_angle_velocity_ = declare_parameter<double>("steering_angle_velocity");
        accel_sensitivity_ = declare_parameter<double>("accel_sensitivity");
        brake_sensitivity_ = declare_parameter<double>("brake_sensitivity");
        raw_control_ = declare_parameter<bool>("control_command.raw_control");
        velocity_gain_ = declare_parameter<double>("control_command.velocity_gain");
        max_forward_velocity_ = declare_parameter<double>("control_command.max_forward_velocity");
        max_backward_velocity_ = declare_parameter<double>("control_command.max_backward_velocity");
        backward_accel_ratio_ = declare_parameter<double>("control_command.backward_accel_ratio");

        // 打印乔伊类型信息
        RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str());

        // 创建回调组
        callback_group_services_ =
            this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

        // 创建订阅者
        if (raw_control_) {
            twist_ = std::make_shared<geometry_msgs::msg::TwistStamped>();
        }

        // 创建发布者
        pub_control_command_ =
            this->create_publisher<autoware_control_msgs::msg::Control>("output/control_command", 1);
        pub_external_control_command_ =
            this->create_publisher<tier4_external_api_msgs::msg::ControlCommandStamped>(
                "output/external_control_command", 1);
        pub_shift_ =
            this->create_publisher<tier4_external_api_msgs::msg::GearShiftStamped>("output/shift", 1);
        pub_turn_signal_ = this->create_publisher<tier4_external_api_msgs::msg::TurnSignalStamped>(
            "output/turn_signal", 1);
        pub_gate_mode_ = this->create_publisher<tier4_control_msgs::msg::GateMode>("output/gate_mode", 1);
        pub_heartbeat_ =
            this->create_publisher<tier4_external_api_msgs::msg::Heartbeat>("output/heartbeat", 1);
        pub_vehicle_engage_ =
            this->create_publisher<autoware_vehicle_msgs::msg::Engage>("output/vehicle_engage", 1);

        // 创建服务客户端
        client_emergency_stop_ = this->create_client<tier4_external_api_msgs::srv::SetEmergency>(
            "service/emergency_stop", rmw_qos_profile_services_default, callback_group_services_);
        while (!client_emergency_stop_->wait_for_service(std::chrono::seconds(1))) {
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(get_logger(), "Interrupted while waiting for service.");
                rclcpp::shutdown();
                return;
            }
            RCLCPP_INFO(get_logger(), "Waiting for emergency_stop service connection...");
        }

        client_autoware_engage_ =
            this->create_client<tier4_external_api_msgs::srv::Engage>("service/autoware_engage");

        // 初始化定时器
        initTimer(1.0 / update_rate_);
    }
}  // namespace autoware::joy_controller

// 注册节点宏
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::joy_controller::AutowareJoyControllerNode)

六、autoware_lane_departure_checker

是否偏离车道检测，检测完之后发布检测信息，再由别的模块接收。
这个模块不会进行车辆控制。

主要功能&输入&输出

主要功能：

主要作用：
1. 实时监控车辆位置
2.预警可能的车道偏离
3.检测实际的车道偏离
4.检测边界线越线


主要检测类型：
ros__parameters:
  # 三种检测模式
  will_out_of_lane_checker: false    # 是否即将偏离车道
  out_of_lane_checker: false         # 是否已经偏离车道
  boundary_departure_checker: true    # 是否越过边界线

关键检测参数：

# 检测阈值
footprint_margin_scale: 1.0          # 车辆轮廓边界裕度
max_lateral_deviation: 2.0           # 最大横向偏差
max_longitudinal_deviation: 2.0      # 最大纵向偏差
max_yaw_deviation_deg: 60.0         # 最大偏航角偏差

主要检测功能：
void LaneDepartureChecker::update() {
  // 1. 轨迹偏差检测
  output.trajectory_deviation = calcTrajectoryDeviation(
    reference_trajectory, current_pose);
    
  // 2. 车道偏离检测
  output.is_out_of_lane = isOutOfLane(
    candidate_lanelets, vehicle_footprint);
    
  // 3. 边界越线检测
  output.will_cross_boundary = willCrossBoundary(
    vehicle_footprints, uncrossable_boundaries);
}


安全功能：
// 计算制动距离
double calcBrakingDistance(
  const double velocity,
  const double max_deceleration,
  const double delay_time)
{
  return (velocity * velocity) / (2.0 * max_deceleration) + 
         delay_time * velocity;
}

输入：

定位信息：
// 车辆当前位置和状态
"~/input/odometry": "/localization/kinematic_state"

地图信息：
// Lanelet2地图数据
"~/input/lanelet_map_bin": "/map/vector_map"

规划轨迹：
// 参考轨迹和预测轨迹
"~/input/reference_trajectory": "/planning/scenario_planning/trajectory"
"~/input/predicted_trajectory": "/control/trajectory_follower/predicted_trajectory"

路径信息：
// 规划路径
"~/input/route": "/planning/mission_planning/route"

配置参数：
# 检测参数
footprint_margin_scale: 1.0     # 车辆轮廓边界裕度
max_lateral_deviation: 2.0      # 最大横向偏差
max_longitudinal_deviation: 2.0 # 最大纵向偏差
boundary_types_to_detect: [curbstone]  # 需要检测的边界类型

输出：

诊断状态 (Diagnostic Status)：
void checkLaneDeparture(DiagnosticStatusWrapper & stat) {
  // 车道偏离状态
  if (output_.is_out_of_lane) {
    stat.summary(DiagnosticStatus::ERROR, "vehicle is out of lane!");
  }
  // 轨迹偏差状态
  if (output_.trajectory_deviation.lateral_deviation > param_.max_lateral_deviation) {
    stat.summary(DiagnosticStatus::ERROR, "lateral deviation is too large!");
  }
}

检测结果 (Output)：
struct Output {
  bool will_leave_lane;           // 是否即将偏离车道
  bool is_out_of_lane;           // 是否已偏离车道
  bool will_cross_boundary;      // 是否将越过边界
  
  PoseDeviation trajectory_deviation;  // 轨迹偏差信息
  std::vector<LinearRing2d> vehicle_footprints;  // 车辆轮廓
}


可视化信息：
// 发布可视化标记
visualization_msgs::msg::MarkerArray createDebugMarkerArray() {
  // 显示车道线
  marker_array.markers.push_back(createLaneletMarker());
  
  // 显示车辆轮廓
  marker_array.markers.push_back(createVehicleFootprintMarker());
  
  // 显示偏差信息
  marker_array.markers.push_back(createDeviationMarker());
}


处理时间信息：
// 发布处理时间
pub_processing_time_ = create_publisher<Float64Stamped>(
  "~/debug/processing_time_ms", 1);

2.源码注释

三个cpp文件。

lane_departure_checker_node.cpp

// Copyright 2020 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
//
//     <url id="" type="url" status="" title="" wc="">http://www.apache.org/licenses/LICENSE-2.0</url> 
//
// 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 "autoware/lane_departure_checker/lane_departure_checker_node.hpp"

#include <autoware/universe_utils/math/unit_conversion.hpp>
#include <autoware/universe_utils/ros/marker_helper.hpp>
#include <autoware/universe_utils/system/stop_watch.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_lanelet2_extension/visualization/visualization.hpp>

#include <autoware_planning_msgs/msg/lanelet_segment.hpp>

#include <map>
#include <memory>
#include <string>
#include <utility>
#include <vector>

// 使用命名空间中的函数
using autoware::universe_utils::rad2deg;

// 命名空间声明
namespace
{
    // 使用autoware_planning_msgs命名空间中的类型
    using autoware_planning_msgs::msg::LaneletSegment;

    // 将三角形转换为点
    std::array<geometry_msgs::msg::Point, 3> triangle2points(
        const geometry_msgs::msg::Polygon & triangle)
    {
        std::array<geometry_msgs::msg::Point, 3> points;
        for (size_t i = 0; i < 3; ++i) {
            const auto & p = triangle.points.at(i);

            geometry_msgs::msg::Point point;
            point.x = static_cast<double>(p.x);
            point.y = static_cast<double>(p.y);
            point.z = static_cast<double>(p.z);
            points.at(i) = point;
        }
        return points;
    }

    // 获取路线车道
    std::map<lanelet::Id, lanelet::ConstLanelet> getRouteLanelets(
        const lanelet::LaneletMapPtr & lanelet_map,
        const lanelet::routing::RoutingGraphPtr & routing_graph,
        const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr,
        const double vehicle_length)
    {
        std::map<lanelet::Id, lanelet::ConstLanelet> route_lanelets;

        bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr, lanelet_map);
        if (!is_route_valid) {
            return route_lanelets;
        }

        // 添加前路线段的前车道以防止检测错误
        {
            const auto extension_length = 2 * vehicle_length;

            for (const auto & primitive : route_ptr->segments.front().primitives) {
                const auto lane_id = primitive.id;
                for (const auto & lanelet_sequence : lanelet::utils::query::getPrecedingLaneletSequences(
                         routing_graph, lanelet_map->laneletLayer.get(lane_id), extension_length)) {
                    for (const auto & preceding_lanelet : lanelet_sequence) {
                        route_lanelets[preceding_lanelet.id()] = preceding_lanelet;
                    }
                }
            }
        }

        for (const auto & route_section : route_ptr->segments) {
            for (const auto & primitive : route_section.primitives) {
                const auto lane_id = primitive.id;
                route_lanelets[lane_id] = lanelet_map->laneletLayer.get(lane_id);
            }
        }

        // 添加最后路线段的后车道以防止检测错误
        {
            const auto extension_length = 2 * vehicle_length;

            for (const auto & primitive : route_ptr->segments.back().primitives) {
                const auto lane_id = primitive.id;
                for (const auto & lanelet_sequence : lanelet::utils::query::getSucceedingLaneletSequences(
                         routing_graph, lanelet_map->laneletLayer.get(lane_id), extension_length)) {
                    for (const auto & succeeding_lanelet : lanelet_sequence) {
                        route_lanelets[succeeding_lanelet.id()] = succeeding_lanelet;
                    }
                }
            }
        }

        return route_lanelets;
    }

    // 更新参数
    template <typename T>
    void update_param(
        const std::vector<rclcpp::Parameter> & parameters, const std::string & name, T & value)
    {
        auto it = std::find_if(
            parameters.cbegin(), parameters.cend(),
            [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; });
        if (it != parameters.cend()) {
            value = it->template get_value<T>();
        }
    }

}  // namespace

// autoware::lane_departure_checker命名空间
namespace autoware::lane_departure_checker
{
    // 车道偏离检查器节点类
    LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & options)
        : Node("lane_departure_checker_node", options)
    {
        using std::placeholders::_1;

        // 启用功能
        node_param_.will_out_of_lane_checker = declare_parameter<bool>("will_out_of_lane_checker");
        node_param_.out_of_lane_checker = declare_parameter<bool>("out_of_lane_checker");
        node_param_.boundary_departure_checker = declare_parameter<bool>("boundary_departure_checker");

        // 节点参数
        node_param_.update_rate = declare_parameter<double>("update_rate");
        node_param_.visualize_lanelet = declare_parameter<bool>("visualize_lanelet");
        node_param_.include_right_lanes = declare_parameter<bool>("include_right_lanes");
        node_param_.include_left_lanes = declare_parameter<bool>("include_left_lanes");
        node_param_.include_opposite_lanes = declare_parameter<bool>("include_opposite_lanes");
        node_param_.include_conflicting_lanes = declare_parameter<bool>("include_conflicting_lanes");

        // 边界偏离检查器
        node_param_.boundary_types_to_detect =
            declare_parameter<std::vector<std::string>>("boundary_types_to_detect");

        // 车辆信息
        const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
        vehicle_length_m_ = vehicle_info.vehicle_length_m;

        // 核心参数
        param_.footprint_margin_scale = declare_parameter<double>("footprint_margin_scale");
        param_.footprint_extra_margin = declare_parameter<double>("footprint_extra_margin");
        param_.resample_interval = declare_parameter<double>("resample_interval");
        param_.max_deceleration = declare_parameter<double>("max_deceleration");
        param_.delay_time = declare_parameter<double>("delay_time");
        param_.max_lateral_deviation = declare_parameter<double>("max_lateral_deviation");
        param_.max_longitudinal_deviation = declare_parameter<double>("max_longitudinal_deviation");
        param_.max_yaw_deviation_deg = declare_parameter<double>("max_yaw_deviation_deg");
        param_.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
        param_.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
        param_.min_braking_distance = declare_parameter<double>("min_braking_distance");

        // 参数回调
        set_param_res_ =
            add_on_set_parameters_callback(std::bind(&LaneDepartureCheckerNode::onParameter, this, _1));

        // 核心
        lane_departure_checker_ = std::make_unique<LaneDepartureChecker>();
        lane_departure_checker_->setParam(param_, vehicle_info);

        // 发布者
        processing_time_publisher_ =
            this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
        // 无操作

        // 诊断更新器
        updater_.setHardwareID("lane_departure_checker");

        updater_.add("lane_departure", this, &LaneDepartureCheckerNode::checkLaneDeparture);

        updater_.add("trajectory_deviation", this, &LaneDepartureCheckerNode::checkTrajectoryDeviation);

        // 定时器
        const auto period_ns = rclcpp::Rate(node_param_.update_rate).period();
        timer_ = rclcpp::create_timer(
            this, get_clock(), period_ns, std::bind(&LaneDepartureCheckerNode::onTimer, this));
    }

    // 检查数据是否准备好
    bool LaneDepartureCheckerNode::isDataReady()
    {
        if (!current_odom_) {
            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_twist msg...");
            return false;
        }

        if (!lanelet_map_) {
            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for lanelet_map msg...");
            return false;
        }

        if (!route_) {
            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route msg...");
            return false;
        }

        if (!reference_trajectory_) {
            RCLCPP_INFO_THROTTLE(
                get_logger(), *get_clock(), 5000, "waiting for reference_trajectory msg...");
            return false;
        }

        if (!predicted_trajectory_) {
            RCLCPP_INFO_THROTTLE(
                get_logger(), *get_clock(), 5000, "waiting for predicted_trajectory msg...");
            return false;
        }

        if (!operation_mode_) {
            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for operation_mode msg...");
            return false;
        }

        if (!control_mode_) {
            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for control_mode msg...");
            return false;
        }

        return true;
    }

    // 检查数据是否超时
    bool LaneDepartureCheckerNode::isDataTimeout()
    {
        const auto now = this->now();

        constexpr double th_pose_timeout = 1.0;
        const auto pose_time_diff = rclcpp::Time(current_odom_->header.stamp) - now;
        if (pose_time_diff.seconds() > th_pose_timeout) {
            RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "pose is timeout...");
            return true;
        }

        return false;
    }

    // 检查数据是否有效
    bool LaneDepartureCheckerNode::isDataValid()
    {
        if (reference_trajectory_->points.empty()) {
            RCLCPP_ERROR_THROTTLE(
                get_logger(), *get_clock(), 5000, "reference_trajectory is empty. Not expected!");
            return false;
        }

        if (predicted_trajectory_->points.empty()) {
            RCLCPP_ERROR_THROTTLE(
                get_logger(), *get_clock(), 5000, "predicted_trajectory is empty. Not expected!");
            return false;
        }

        return true;
    }

    // 定时器回调函数
    void LaneDepartureCheckerNode::onTimer()
    {
        std::map<std::string, double> processing_time_map;
        autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
        stop_watch.tic("Total");

        current_odom_ = sub_odom_.takeData();
        route_ = sub_route_.takeData();
        reference_trajectory_ = sub_reference_trajectory_.takeData();
        predicted_trajectory_ = sub_predicted_trajectory_.takeData();
        operation_mode_ = sub_operation_mode_.takeData();
        control_mode_ = sub_control_mode_.takeData();

        const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData();
        if (lanelet_map_bin_msg) {
            lanelet_map_ = std::make_shared<lanelet::LaneletMap>();
            lanelet::utils::conversion::fromBinMsg(
                *lanelet_map_bin_msg, lanelet_map_, &traffic_rules_, &routing_graph_);

            // 获取所有路肩车道
            lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_);
            shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
        }

        if (!isDataReady()) {
            return;
        }

        if (isDataTimeout()) {
            return;
        }

        if (!isDataValid()) {
            return;
        }

        processing_time_map["Node: checkData"] = stop_watch.toc(true);

        // 为了等待地图和路线都准备好，这里不使用回调函数
        if (last_route_ != route_ && !route_->segments.empty()) {
            std::map<lanelet::Id, lanelet::ConstLanelet>::iterator itr;

            auto map_route_lanelets_ =
                getRouteLanelets(lanelet_map_, routing_graph_, route_, vehicle_length_m_);

            lanelet::ConstLanelets shared_lanelets_tmp;

            for (itr = map_route_lanelets_.begin(); itr != map_route_lanelets_.end(); ++itr) {
                const auto shared_lanelet = getAllSharedLineStringLanelets(
                    itr->second, node_param_.include_right_lanes, node_param_.include_left_lanes,
                    node_param_.include_opposite_lanes, node_param_.include_conflicting_lanes, true);
                shared_lanelets_tmp.insert(
                    shared_lanelets_tmp.end(), shared_lanelet.begin(), shared_lanelet.end());
            }
            for (const auto & lanelet : shared_lanelets_tmp) {
                map_route_lanelets_[lanelet.id()] = lanelet;
            }
            route_lanelets_.clear();
            for (itr = map_route_lanelets_.begin(); itr != map_route_lanelets_.end(); ++itr) {
                route_lanelets_.push_back(itr->second);
            }
            last_route_ = route_;
        }
        processing_time_map["Node: getRouteLanelets"] = stop_watch.toc(true);

        input_.current_odom = current_odom_;
        input_.lanelet_map = lanelet_map_;
        input_.route = route_;
        input_.route_lanelets = route_lanelets_;
        input_.shoulder_lanelets = shoulder_lanelets_;
        input_.reference_trajectory = reference_trajectory_;
        input_.predicted_trajectory = predicted_trajectory_;
        input_.boundary_types_to_detect = node_param_.boundary_types_to_detect;
        processing_time_map["Node: setInputData"] = stop_watch.toc(true);

        output_ = lane_departure_checker_->update(input_);
        processing_time_map["Node: update"] = stop_watch.toc(true);

        updater_.force_update();
        processing_time_map["Node: updateDiagnostics"] = stop_watch.toc(true);

        {
            const auto & deviation = output_.trajectory_deviation;
            debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
                "deviation/lateral", deviation.lateral);
            debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>("deviation/yaw", deviation.yaw);
            debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
                "deviation/yaw_deg", rad2deg(deviation.yaw));
        }
        processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true);

        debug_publisher_.publish<visualization_msgs::msg::MarkerArray>(
            std::string("marker_array"), createMarkerArray());
        processing_time_map["Node: publishDebugMarker"] = stop_watch.toc(true);

        // 合并处理时间映射
        for (const auto & m : output_.processing_time_map) {
            processing_time_map["Core: " + m.first] = m.second;
        }

        processing_time_map["Total"] = stop_watch.toc("Total");
        processing_diag_publisher_.publish(processing_time_map);
        tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
        processing_time_msg.stamp = get_clock()->now();
        processing_time_msg.data = processing_time_map["Total"];
        processing_time_publisher_->publish(processing_time_msg);
    }

    // 参数回调函数
    rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(
        const std::vector<rclcpp::Parameter> & parameters)
    {
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        result.reason = "success";

        try {
            // 启用功能
            update_param(parameters, "will_out_of_lane_checker", node_param_.will_out_of_lane_checker);
            update_param(parameters, "out_of_lane_checker", node_param_.out_of_lane_checker);
            update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker);

            // 节点
            update_param(parameters, "visualize_lanelet", node_param_.visualize_lanelet);
            update_param(parameters, "include_right_lanes", node_param_.include_right_lanes);
            update_param(parameters, "include_left_lanes", node_param_.include_left_lanes);
            update_param(parameters, "include_opposite_lanes", node_param_.include_opposite_lanes);
            update_param(parameters, "include_conflicting_lanes", node_param_.include_conflicting_lanes);
            update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker);
            update_param(parameters, "boundary_types_to_detect", node_param_.boundary_types_to_detect);

            // 核心
            update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale);
            update_param(parameters, "footprint_extra_margin", param_.footprint_extra_margin);
            update_param(parameters, "resample_interval", param_.resample_interval);
            update_param(parameters, "max_deceleration", param_.max_deceleration);
            update_param(parameters, "delay_time", param_.delay_time);
            update_param(parameters, "min_braking_distance", param_.min_braking_distance);

            if (lane_departure_checker_) {
                lane_departure_checker_->setParam(param_);
            }
        } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
            result.successful = false;
            result.reason = e.what();
        }

        return result;
    }

    // 检查车道偏离
    void LaneDepartureCheckerNode::checkLaneDeparture(
        diagnostic_updater::DiagnosticStatusWrapper & stat)
    {
        using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus;
        int8_t level = DiagStatus::OK;
        std::string msg = "OK";

        if (output_.will_leave_lane && node_param_.will_out_of_lane_checker) {
            level = DiagStatus::WARN;
            msg = "vehicle will leave lane";
        }

        if (output_.is_out_of_lane && node_param_.out_of_lane_checker) {
            level = DiagStatus::ERROR;
            msg = "vehicle is out of lane";
        }

        if (output_.will_cross_boundary && node_param_.boundary_departure_checker) {
            level = DiagStatus::ERROR;
            msg = "vehicle will cross boundary";
        }

        stat.summary(level, msg);
    }

    // 检查轨迹偏差
    void LaneDepartureCheckerNode::checkTrajectoryDeviation(
        diagnostic_updater::DiagnosticStatusWrapper & stat)
    {
        using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus;
        using ControlModeStatus = autoware_vehicle_msgs::msg::ControlModeReport;
        using OperationModeStatus = autoware_adapi_v1_msgs::msg::OperationModeState;

        int8_t level = DiagStatus::OK;

        if (std::abs(output_.trajectory_deviation.lateral) >= param_.max_lateral_deviation) {
            level = DiagStatus::ERROR;
        }

        if (std::abs(output_.trajectory_deviation.longitudinal) >= param_.max_longitudinal_deviation) {
            level = DiagStatus::ERROR;
        }

        if (std::abs(rad2deg(output_.trajectory_deviation.yaw)) >= param_.max_yaw_deviation_deg) {
            level = DiagStatus::ERROR;
        }

        std::string msg = "OK";
        if (
            level == DiagStatus::ERROR && operation_mode_->mode == OperationModeStatus::AUTONOMOUS &&
            control_mode_->mode == ControlModeStatus::AUTONOMOUS) {
            msg = "trajectory deviation is too large";
        } else {
            level = DiagStatus::OK;
        }

        stat.addf("max lateral deviation", "%.3f", param_.max_lateral_deviation);
        stat.addf("lateral deviation", "%.3f", output_.trajectory_deviation.lateral);

        stat.addf("max longitudinal deviation", "%.3f", param_.max_longitudinal_deviation);
        stat.addf("longitudinal deviation", "%.3f", output_.trajectory_deviation.longitudinal);

        stat.addf("max yaw deviation", "%.3f", param_.max_yaw_deviation_deg);
        stat.addf("yaw deviation", "%.3f", rad2deg(output_.trajectory_deviation.yaw));

        stat.summary(level, msg);
    }

    // 创建标记数组
    visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray() const
    {
        using autoware::universe_utils::createDefaultMarker;
        using autoware::universe_utils::createMarkerColor;
        using autoware::universe_utils::createMarkerScale;

        visualization_msgs::msg::MarkerArray marker_array;

        const auto base_link_z = current_odom_->pose.pose.position.z;

        if (node_param_.visualize_lanelet) {
            // 路线车道
            {
                auto marker = createDefaultMarker(
                    "map", this->now(), "route_lanelets", 0, visualization_msgs::msg::Marker::TRIANGLE_LIST,
                    createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(0.0, 0.5, 0.5, 0.5));

                for (const auto & lanelet : input_.route_lanelets) {
                    std::vector<geometry_msgs::msg::Polygon> triangles;
                    lanelet::visualization::lanelet2Triangle(lanelet, &triangles);

                    for (const auto & triangle : triangles) {
                        for (const auto & point : triangle2points(triangle)) {
                            marker.points.push_back(point);
                            marker.colors.push_back(marker.color);
                        }
                    }
                }

                marker_array.markers.push_back(marker);
            }

            // 候选车道
            {
                auto marker = createDefaultMarker(
                    "map", this->now(), "candidate_lanelets", 0, visualization_msgs::msg::Marker::TRIANGLE_LIST,
                    createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.5));

                for (const auto & lanelet : output_.candidate_lanelets) {
                    std::vector<geometry_msgs::msg::Polygon> triangles;
                    lanelet::visualization::lanelet2Triangle(lanelet, &triangles);

                    for (const auto & triangle : triangles) {
                        for (const auto & point : triangle2points(triangle)) {
                            marker.points.push_back(point);
                            marker.colors.push_back(marker.color);
                        }
                    }
                }

                marker_array.markers.push_back(marker);
            }
        }

        if (output_.resampled_trajectory.size() >= 2) {
            // 重采样轨迹线
            {
                auto marker = createDefaultMarker(
                    "map", this->now(), "resampled_trajectory_line", 0,
                    visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.05, 0, 0),
                    createMarkerColor(1.0, 1.0, 1.0, 0.999));

                for (const auto & p : output_.resampled_trajectory) {
                    marker.points.push_back(p.pose.position);
                    marker.colors.push_back(marker.color);
                }

                marker_array.markers.push_back(marker);
            }

            // 重采样轨迹点
            {
                auto marker = createDefaultMarker(
                    "map", this->now(), "resampled_trajectory_points", 0,
                    visualization_msgs::msg::Marker::SPHERE_LIST, createMarkerScale(0.1, 0.1, 0.1),
                    createMarkerColor(0.0, 1.0, 0.0, 0.999));

                for (const auto & p : output_.resampled_trajectory) {
                    marker.points.push_back(p.pose.position);
                    marker.colors.push_back(marker.color);
                }

                marker_array.markers.push_back(marker);
            }
        }

        // 车辆足迹
        {
            const auto color_ok = createMarkerColor(0.0, 1.0, 0.0, 0.5);
            const auto color_will_leave_lane = createMarkerColor(0.5, 0.5, 0.0, 0.5);
            const auto color_is_out_of_lane = createMarkerColor(1.0, 0.0, 0.0, 0.5);

            auto color = color_ok;
            if (output_.will_leave_lane) {
                color = color_will_leave_lane;
            }
            if (output_.is_out_of_lane) {
                color = color_is_out_of_lane;
            }

            auto marker = createDefaultMarker(
                "map", this->now(), "vehicle_footprints", 0, visualization_msgs::msg::Marker::LINE_LIST,
                createMarkerScale(0.05, 0, 0), color);

            for (const auto & vehicle_footprint : output_.vehicle_footprints) {
                for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) {
                    const auto p1 = vehicle_footprint.at(i);
                    const auto p2 = vehicle_footprint.at(i + 1);

                    marker.points.push_back(toMsg(p1.to_3d(base_link_z)));
                    marker.points.push_back(toMsg(p2.to_3d(base_link_z)));
                }
            }

            marker_array.markers.push_back(marker);
        }

        return marker_array;
    }

    // 获取所有共享线字符串车道
    lanelet::ConstLanelets LaneDepartureCheckerNode::getAllSharedLineStringLanelets(
        const lanelet::ConstLanelet & current_lane, const bool is_right, const bool is_left,
        const bool is_opposite, const bool is_conflicting, const bool & invert_opposite)
    {
        lanelet::ConstLanelets shared{current_lane};

        if (is_right) {
            const lanelet::ConstLanelets all_right_lanelets =
                getAllRightSharedLinestringLanelets(current_lane, is_opposite, invert_opposite);
            shared.insert(shared.end(), all_right_lanelets.begin(), all_right_lanelets.end());
        }

        if (is_left) {
            const lanelet::ConstLanelets all_left_lanelets =
                getAllLeftSharedLinestringLanelets(current_lane, is_opposite, invert_opposite);
            shared.insert(shared.end(), all_left_lanelets.begin(), all_left_lanelets.end());
        }

        if (is_conflicting) {
            const auto conflicting_lanelets =
                lanelet::utils::getConflictingLanelets(routing_graph_, current_lane);
            shared.insert(shared.end(), conflicting_lanelets.begin(), conflicting_lanelets.end());
        }
        return shared;
    }

    // 获取所有右侧共享线字符串车道
    lanelet::ConstLanelets LaneDepartureCheckerNode::getAllRightSharedLinestringLanelets(
        const lanelet::ConstLanelet & lane, const bool & include_opposite, const bool & invert_opposite)
    {
        lanelet::ConstLanelets linestring_shared;
        auto lanelet_at_right = getRightLanelet(lane);
        auto lanelet_at_right_opposite = getRightOppositeLanelets(lane);
        while (lanelet_at_right) {
            linestring_shared.push_back(lanelet_at_right.get());
            lanelet_at_right = getRightLanelet(lanelet_at_right.get());
            if (!lanelet_at_right) {
                break;
            }
            lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.get());
        }

        if (!lanelet_at_right_opposite.empty() && include_opposite) {
            if (invert_opposite) {
                linestring_shared.push_back(lanelet_at_right_opposite.front().invert());
            } else {
                linestring_shared.push_back(lanelet_at_right_opposite.front());
            }
            auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front());
            while (lanelet_at_left) {
                if (invert_opposite) {
                    linestring_shared.push_back(lanelet_at_left.get().invert());
                } else {
                    linestring_shared.push_back(lanelet_at_left.get());
                }
                lanelet_at_left = getLeftLanelet(lanelet_at_left.get());
            }
        }
        return linestring_shared;
    }

    // 获取所有左侧共享线字符串车道
    lanelet::ConstLanelets LaneDepartureCheckerNode::getAllLeftSharedLinestringLanelets(
        const lanelet::ConstLanelet & lane, const bool & include_opposite, const bool & invert_opposite)
    {
        lanelet::ConstLanelets linestring_shared;
        auto lanelet_at_left = getLeftLanelet(lane);
        auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane);
        while (lanelet_at_left) {
            linestring_shared.push_back(lanelet_at_left.get());
            lanelet_at_left = getLeftLanelet(lanelet_at_left.get());
            if (!lanelet_at_left) {
                break;
            }
            lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.get());
        }

        if (!lanelet_at_left_opposite.empty() && include_opposite) {
            if (invert_opposite) {
                linestring_shared.push_back(lanelet_at_left_opposite.front().invert());
            } else {
                linestring_shared.push_back(lanelet_at_left_opposite.front());
            }
            auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front());
            while (lanelet_at_right) {
                if (invert_opposite) {
                    linestring_shared.push_back(lanelet_at_right.get().invert());
                } else {
                    linestring_shared.push_back(lanelet_at_right.get());
                }
                lanelet_at_right = getRightLanelet(lanelet_at_right.get());
            }
        }
        return linestring_shared;
    }

    // 获取左侧车道
    boost::optional<lanelet::ConstLanelet> LaneDepartureCheckerNode::getLeftLanelet(
        const lanelet::ConstLanelet & lanelet)
    {
        // 可路由车道
        const auto & left_lane = routing_graph_->left(lanelet);
        if (left_lane) {
            return left_lane;
        }

        // 不可路由车道（例如，变道不可行）
        const auto & adjacent_left_lane = routing_graph_->adjacentLeft(lanelet);
        return adjacent_left_lane;
    }

    // 获取左侧对向车道
    lanelet::Lanelets LaneDepartureCheckerNode::getLeftOppositeLanelets(
        const lanelet::ConstLanelet & lanelet)
    {
        const auto opposite_candidate_lanelets =
            lanelet_map_->laneletLayer.findUsages(lanelet.leftBound().invert());

        lanelet::Lanelets opposite_lanelets;
        for (const auto & candidate_lanelet : opposite_candidate_lanelets) {
            if (candidate_lanelet.rightBound().id() == lanelet.leftBound().id()) {
                continue;
            }

            opposite_lanelets.push_back(candidate_lanelet);
        }

        return opposite_lanelets;
    }

    // 获取右侧车道
    boost::optional<lanelet::ConstLanelet> LaneDepartureCheckerNode::getRightLanelet(
        const lanelet::ConstLanelet & lanelet) const
    {
        // 可路由车道
        const auto & right_lane = routing_graph_->right(lanelet);
        if (right_lane) {
            return right_lane;
        }

        // 不可路由车道（例如，变道不可行）
        const auto & adjacent_right_lane = routing_graph_->adjacentRight(lanelet);
        return adjacent_right_lane;
    }

    // 获取右侧对向车道
    lanelet::Lanelets LaneDepartureCheckerNode::getRightOppositeLanelets(
        const lanelet::ConstLanelet & lanelet)
    {
        const auto opposite_candidate_lanelets =
            lanelet_map_->laneletLayer.findUsages(lanelet.rightBound().invert());

        lanelet::Lanelets opposite_lanelets;
        for (const auto & candidate_lanelet : opposite_candidate_lanelets) {
            if (candidate_lanelet.leftBound().id() == lanelet.rightBound().id()) {
                continue;
            }

            opposite_lanelets.push_back(candidate_lanelet);
        }

        return opposite_lanelets;
    }

}  // namespace autoware::lane_departure_checker

// 注册节点宏
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lane_departure_checker::LaneDepartureCheckerNode)

lane_departure_checker.cpp

// Copyright 2020 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
//
//     <url id="ctv3lkcr4djh7of05mjg" type="url" status="parsed" title="Apache License, Version 2.0" wc="10467">http://www.apache.org/licenses/LICENSE-2.0</url> 
//
// 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 "autoware/lane_departure_checker/lane_departure_checker.hpp"

#include "autoware/lane_departure_checker/utils.hpp"

#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/universe_utils/math/normalization.hpp>
#include <autoware/universe_utils/math/unit_conversion.hpp>
#include <autoware/universe_utils/system/stop_watch.hpp>

#include <boost/geometry.hpp>

#include <lanelet2_core/geometry/Polygon.h>
#include <tf2/utils.h>

#include <algorithm>
#include <vector>

// 使用命名空间中的函数
using autoware::motion_utils::calcArcLength;
using autoware::universe_utils::LinearRing2d;
using autoware::universe_utils::LineString2d;
using autoware::universe_utils::MultiPoint2d;
using autoware::universe_utils::MultiPolygon2d;
using autoware::universe_utils::Point2d;

// 命名空间声明
namespace
{
    // 使用autoware_planning_msgs命名空间中的类型
    using autoware_planning_msgs::msg::Trajectory;
    using autoware_planning_msgs::msg::TrajectoryPoint;
    using TrajectoryPoints = std::vector<TrajectoryPoint>;
    using geometry_msgs::msg::Point;

    // 计算制动距离
    double calcBrakingDistance(
        const double abs_velocity, const double max_deceleration, const double delay_time)
    {
        return (abs_velocity * abs_velocity) / (2.0 * max_deceleration) + delay_time * abs_velocity;
    }

    // 检查点是否在任何车道内
    bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2d & point)
    {
        for (const auto & ll : candidate_lanelets) {
            if (boost::geometry::within(point, ll.polygon2d().basicPolygon())) {
                return true;
            }
        }

        return false;
    }

    // 从足迹创建凸包
    LinearRing2d createHullFromFootprints(const std::vector<LinearRing2d> & footprints)
    {
        MultiPoint2d combined;
        for (const auto & footprint : footprints) {
            for (const auto & p : footprint) {
                combined.push_back(p);
            }
        }

        LinearRing2d hull;
        boost::geometry::convex_hull(combined, hull);

        return hull;
    }

    // 获取候选车道
    lanelet::ConstLanelets getCandidateLanelets(
        const lanelet::ConstLanelets & route_lanelets,
        const std::vector<LinearRing2d> & vehicle_footprints)
    {
        lanelet::ConstLanelets candidate_lanelets;

        // 查找在足迹凸包内的车道
        const auto footprint_hull = createHullFromFootprints(vehicle_footprints);
        for (const auto & route_lanelet : route_lanelets) {
            const auto poly = route_lanelet.polygon2d().basicPolygon();
            if (!boost::geometry::disjoint(poly, footprint_hull)) {
                candidate_lanelets.push_back(route_lanelet);
            }
        }

        return candidate_lanelets;
    }

}  // namespace

// autoware::lane_departure_checker命名空间
namespace autoware::lane_departure_checker
{
    // 更新车道偏离检查器
    Output LaneDepartureChecker::update(const Input & input)
    {
        Output output{};

        autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;

        // 计算轨迹偏差
        output.trajectory_deviation = calcTrajectoryDeviation(
            *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold,
            param_.ego_nearest_yaw_threshold);
        output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true);

        // 计算制动距离
        {
            constexpr double min_velocity = 0.01;
            const auto & raw_abs_velocity = std::abs(input.current_odom->twist.twist.linear.x);
            const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity;

            const auto braking_distance = std::max(
                param_.min_braking_distance,
                calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time));

            // 重采样轨迹并裁剪
            output.resampled_trajectory = utils::cutTrajectory(
                utils::resampleTrajectory(*input.predicted_trajectory, param_.resample_interval),
                braking_distance);
            output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true);
        }

        // 创建车辆足迹
        output.vehicle_footprints = utils::createVehicleFootprints(
            input.current_odom->pose, output.resampled_trajectory, *vehicle_info_ptr_,
            param_.footprint_margin_scale);
        output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true);

        // 创建车辆通过区域
        output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints);
        output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true);

        // 获取候选车道
        const auto candidate_road_lanelets =
            getCandidateLanelets(input.route_lanelets, output.vehicle_footprints);
        const auto candidate_shoulder_lanelets =
            getCandidateLanelets(input.shoulder_lanelets, output.vehicle_footprints);
        output.candidate_lanelets = candidate_road_lanelets;
        output.candidate_lanelets.insert(
            output.candidate_lanelets.end(), candidate_shoulder_lanelets.begin(),
            candidate_shoulder_lanelets.end());

        output.processing_time_map["getCandidateLanelets"] = stop_watch.toc(true);

        // 检查车辆是否会离开车道
        output.will_leave_lane = willLeaveLane(output.candidate_lanelets, output.vehicle_footprints);
        output.processing_time_map["willLeaveLane"] = stop_watch.toc(true);

        // 检查车辆是否已经偏离车道
        output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front());
        output.processing_time_map["isOutOfLane"] = stop_watch.toc(true);

        // 检查车辆是否会穿过边界
        const double max_search_length_for_boundaries =
            calcMaxSearchLengthForBoundaries(*input.predicted_trajectory);
        const auto uncrossable_boundaries = extractUncrossableBoundaries(
            *input.lanelet_map, input.predicted_trajectory->points.front().pose.position,
            max_search_length_for_boundaries, input.boundary_types_to_detect);
        output.will_cross_boundary = willCrossBoundary(output.vehicle_footprints, uncrossable_boundaries);
        output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true);

        return output;
    }

    // 检查路径是否会离开车道
    bool LaneDepartureChecker::checkPathWillLeaveLane(
        const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const
    {
        universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

        // 创建车辆足迹
        std::vector<LinearRing2d> vehicle_footprints =
            utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin);
        // 获取候选车道
        lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints);
        // 检查车辆是否会离开车道
        return willLeaveLane(candidate_lanelets, vehicle_footprints);
    }

    // 计算轨迹偏差
    PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation(
        const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
        const double yaw_threshold)
    {
        // 查找最近点
        const auto nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
            trajectory.points, pose, dist_threshold, yaw_threshold);
        // 计算偏差
        return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose);
    }

    // 创建车辆通过区域
    std::vector<LinearRing2d> LaneDepartureChecker::createVehiclePassingAreas(
        const std::vector<LinearRing2d> & vehicle_footprints)
    {
        // 创建相邻车辆足迹的凸包
        std::vector<LinearRing2d> areas;
        for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) {
            const auto & footprint1 = vehicle_footprints.at(i);
            const auto & footprint2 = vehicle_footprints.at(i + 1);
            areas.push_back(createHullFromFootprints({footprint1, footprint2}));
        }

        return areas;
    }

    // 检查车辆是否会离开车道
    bool LaneDepartureChecker::willLeaveLane(
        const lanelet::ConstLanelets & candidate_lanelets,
        const std::vector<LinearRing2d> & vehicle_footprints) const
    {
        universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

        // 检查每个车辆足迹是否在候选车道内
        for (const auto & vehicle_footprint : vehicle_footprints) {
            if (isOutOfLane(candidate_lanelets, vehicle_footprint)) {
                return true;
            }
        }

        return false;
    }

    // 从路径获取车道
    std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLaneletsFromPath(
        const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
    {
        universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

        // 获取足迹凸包的基本多边形
        std::vector<LinearRing2d> vehicle_footprints =
            utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin);
        LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints);

        lanelet::BasicPolygon2d footprint_hull_basic_polygon = toBasicPolygon2D(footprint_hull);

        // 查找与足迹凸包相交的所有车道
        return lanelet::geometry::findWithin2d(
            lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
    }

    // 获取路径的融合车道多边形
    std::optional<autoware::universe_utils::Polygon2d>
    LaneDepartureChecker::getFusedLaneletPolygonForPath(
        const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
    {
        universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

        const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
        if (lanelets_distance_pair.empty()) return std::nullopt;

        // 将车道融合成一个基本多边形
        autoware::universe_utils::MultiPolygon2d lanelet_unions;
        autoware::universe_utils::MultiPolygon2d result;

        for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) {
            const auto & route_lanelet = lanelets_distance_pair.at(i).second;
            const auto & p = route_lanelet.polygon2d().basicPolygon();
            autoware::universe_utils::Polygon2d poly = toPolygon2D(p);
            boost::geometry::union_(lanelet_unions, poly, result);
            lanelet_unions = result;
            result.clear();
        }

        if (lanelet_unions.empty()) return std::nullopt;
        return lanelet_unions.front();
    }

    // 更新路径的融合车道多边形
    bool LaneDepartureChecker::updateFusedLaneletPolygonForPath(
        const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
        std::vector<lanelet::Id> & fused_lanelets_id,
        std::optional<autoware::universe_utils::Polygon2d> & fused_lanelets_polygon) const
    {
        universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

        const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
        if (lanelets_distance_pair.empty()) return false;

        autoware::universe_utils::MultiPolygon2d lanelet_unions;
        autoware::universe_utils::MultiPolygon2d result;

        if (fused_lanelets_polygon) lanelet_unions.push_back(fused_lanelets_polygon.value());

        for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) {
            const auto & route_lanelet = lanelets_distance_pair.at(i).second;
            bool id_exist = std::any_of(
                fused_lanelets_id.begin(), fused_lanelets_id.end(),
                [&](const auto & id) { return id == route_lanelet.id(); });

            if (id_exist) continue;

            const auto & p = route_lanelet.polygon2d().basicPolygon();
            autoware::universe_utils::Polygon2d poly = toPolygon2D(p);
            boost::geometry::union_(lanelet_unions, poly, result);
            lanelet_unions = result;
            result.clear();
            fused_lanelets_id.push_back(route_lanelet.id());
        }

        if (lanelet_unions.empty()) {
            fused_lanelets_polygon = std::nullopt;
            return false;
        }

        fused_lanelets_polygon = lanelet_unions.front();
        return true;
    }

    // 检查路径是否会离开车道
    bool LaneDepartureChecker::checkPathWillLeaveLane(
        const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
    {
        universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

        // 检查足迹是否完全包含在融合车道多边形内
        const std::vector<LinearRing2d> vehicle_footprints =
            utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin);
        const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
        if (!fused_lanelets_polygon) return true;
        return !std::all_of(
            vehicle_footprints.begin(), vehicle_footprints.end(),
            [&fused_lanelets_polygon](const auto & footprint) {
                return boost::geometry::within(footprint, fused_lanelets_polygon.value());
            });
    }

    // 检查路径是否会离开车道
    bool LaneDepartureChecker::checkPathWillLeaveLane(
        const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
        std::vector<lanelet::Id> & fused_lanelets_id,
        std::optional<autoware::universe_utils::Polygon2d> & fused_lanelets_polygon) const
    {
        universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

        const std::vector<LinearRing2d> vehicle_footprints =
            utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin);

        auto is_all_footprints_within = [&](const auto & polygon) {
            return std::all_of(
                vehicle_footprints.begin(), vehicle_footprints.end(),
                [&polygon](const auto & footprint) { return boost::geometry::within(footprint, polygon); });
        };

        // 如果车道多边形存在且所有足迹都在其内，则路径不会离开车道
        if (fused_lanelets_polygon && is_all_footprints_within(fused_lanelets_polygon.value())) {
            return false;
        }

        // 更新当前路径的车道多边形
        if (!updateFusedLaneletPolygonForPath(
                lanelet_map_ptr, path, fused_lanelets_id, fused_lanelets_polygon)) {
            // 如果更新失败，则假设路径会离开车道
            return true;
        }

        // 检查任何足迹是否在更新后的车道多边形外
        return !is_all_footprints_within(fused_lanelets_polygon.value());
    }

    // 裁剪车道外的点
    PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes(
        const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index,
        std::vector<lanelet::Id> & fused_lanelets_id,
        std::optional<autoware::universe_utils::Polygon2d> & fused_lanelets_polygon)
    {
        universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

        PathWithLaneId temp_path;

        // 更新当前路径的车道多边形
        if (
            path.points.empty() || !updateFusedLaneletPolygonForPath(
                                     lanelet_map_ptr, path, fused_lanelets_id, fused_lanelets_polygon)) {
            return temp_path;
        }

        const auto vehicle_footprints =
            utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin);

        {
            universe_utils::ScopedTimeTrack st2(
                "check if footprint is within fused_lanelets_polygon", *time_keeper_);

            size_t idx = 0;
            std::for_each(
                vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
                    if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
                        temp_path.points.push_back(path.points.at(idx));
                    }
                    ++idx;
                });
        }
        PathWithLaneId cropped_path = path;
        cropped_path.points = temp_path.points;
        return cropped_path;
    }

    // 检查车辆是否已经偏离车道
    bool LaneDepartureChecker::isOutOfLane(
        const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint)
    {
        for (const auto & point : vehicle_footprint) {
            if (!isInAnyLane(candidate_lanelets, point)) {
                return true;
            }
        }

        return false;
    }

    // 计算边界的最大搜索长度
    double LaneDepartureChecker::calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const
    {
        const double max_ego_lon_length = std::max(
            std::abs(vehicle_info_ptr_->max_longitudinal_offset_m),
            std::abs(vehicle_info_ptr_->min_longitudinal_offset_m));
        const double max_ego_lat_length = std::max(
            std::abs(vehicle_info_ptr_->max_lateral_offset_m),
            std::abs(vehicle_info_ptr_->min_lateral_offset_m));
        const double max_ego_search_length = std::hypot(max_ego_lon_length, max_ego_lat_length);
        return calcArcLength(trajectory.points) + max_ego_search_length;
    }

    // 提取不可穿越的边界
    SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries(
        const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point,
        const double max_search_length, const std::vector<std::string> & boundary_types_to_detect)
    {
        const auto has_types =
            [](const lanelet::ConstLineString3d & ls, const std::vector<std::string> & types) {
                constexpr auto no_type = "";
                const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type);
                return (type != no_type && std::find(types.begin(), types.end(), type) != types.end());
            };

        SegmentRtree uncrossable_segments_in_range;
        LineString2d line;
        const auto ego_p = Point2d{ego_point.x, ego_point.y};
        for (const auto & ls : lanelet_map.lineStringLayer) {
            if (has_types(ls, boundary_types_to_detect)) {
                line.clear();
                for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()});
                for (auto segment_idx = 0LU; segment_idx + 1 < line.size(); ++segment_idx) {
                    const Segment2d segment = {line[segment_idx], line[segment_idx + 1]};
                    if (boost::geometry::distance(segment, ego_p) < max_search_length) {
                        uncrossable_segments_in_range.insert(segment);
                    }
                }
            }
        }
        return uncrossable_segments_in_range;
    }

    // 检查车辆是否会穿过边界
    bool LaneDepartureChecker::willCrossBoundary(
        const std::vector<LinearRing2d> & vehicle_footprints,
        const SegmentRtree & uncrossable_segments) const
    {
        universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

        for (const auto & footprint : vehicle_footprints) {
            std::vector<Segment2d> intersection_result;
            uncrossable_segments.query(
                boost::geometry::index::intersects(footprint), std::back_inserter(intersection_result));
            if (!intersection_result.empty()) {
                return true;
            }
        }
        return false;
    }

    // 将线性环转换为基本多边形
    lanelet::BasicPolygon2d LaneDepartureChecker::toBasicPolygon2D(
        const LinearRing2d & footprint_hull) const
    {
        lanelet::BasicPolygon2d basic_polygon;
        for (const auto & point : footprint_hull) {
            Eigen::Vector2d p(point.x(), point.y());
            basic_polygon.push_back(p);
        }
        return basic_polygon;
    }

    // 将基本多边形转换为多边形
    autoware::universe_utils::Polygon2d LaneDepartureChecker::toPolygon2D(
        const lanelet::BasicPolygon2d & poly) const
    {
        autoware::universe_utils::Polygon2d polygon;
        auto & outer = polygon.outer();

        for (const auto & p : poly) {
            autoware::universe_utils::Point2d p2d(p.x(), p.y());
            outer.push_back(p2d);
        }
        boost::geometry::correct(polygon);
        return polygon;
    }

}  // namespace autoware::lane_departure_checker

utils.cpp

// Copyright 2024 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
//
//     <url id="ctv3o3n2cre5rs2pcht0" type="url" status="parsed" title="Apache License, Version 2.0" wc="10467">http://www.apache.org/licenses/LICENSE-2.0</url> 
//
// 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 "autoware/lane_departure_checker/utils.hpp"

#include <autoware/universe_utils/geometry/geometry.hpp>

#include <boost/geometry.hpp>

// 命名空间声明
namespace
{
    // 定义足迹边距结构体
    struct FootprintMargin
    {
        double lon;  // 纵向边距
        double lat;  // 横向边距
    };

    // 计算足迹边距
    FootprintMargin calcFootprintMargin(
        const geometry_msgs::msg::PoseWithCovariance & covariance, const double scale)
    {
        // 获取协方差矩阵
        const auto Cov_in_map = covariance.covariance;
        Eigen::Matrix2d Cov_xy_map;
        Cov_xy_map << Cov_in_map[0 * 6 + 0], Cov_in_map[0 * 6 + 1], Cov_in_map[1 * 6 + 0],
            Cov_in_map[1 * 6 + 1];

        // 获取车辆偏航角
        const double yaw_vehicle = tf2::getYaw(covariance.pose.orientation);

        // 将协方差矩阵从地图坐标系转换到车辆坐标系
        Eigen::Matrix2d R_map2vehicle;
        R_map2vehicle << std::cos(-yaw_vehicle), -std::sin(-yaw_vehicle), std::sin(-yaw_vehicle),
            std::cos(-yaw_vehicle);
        const Eigen::Matrix2d Cov_xy_vehicle = R_map2vehicle * Cov_xy_map * R_map2vehicle.transpose();

        // 计算纵向和横向边距
        return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale};
    }
}  // namespace

// autoware::lane_departure_checker::utils命名空间
namespace autoware::lane_departure_checker::utils
{
    // 裁剪轨迹
    TrajectoryPoints cutTrajectory(const TrajectoryPoints & trajectory, const double length)
    {
        if (trajectory.empty()) {
            return {};
        }

        TrajectoryPoints cut;

        double total_length = 0.0;
        auto last_point = autoware::universe_utils::fromMsg(trajectory.front().pose.position);
        auto end_it = std::next(trajectory.cbegin());
        for (; end_it != trajectory.cend(); ++end_it) {
            const auto remain_distance = length - total_length;

            // 超出长度
            if (remain_distance <= 0) {
                break;
            }

            const auto & new_pose = end_it->pose;
            const auto new_point = autoware::universe_utils::fromMsg(new_pose.position);
            const auto points_distance = boost::geometry::distance(last_point.to_2d(), new_point.to_2d());

            // 需要插值
            if (remain_distance <= points_distance) {
                const Eigen::Vector3d p_interpolated =
                    last_point + remain_distance * (new_point - last_point).normalized();

                TrajectoryPoint p;
                p.pose.position.x = p_interpolated.x();
                p.pose.position.y = p_interpolated.y();
                p.pose.position.z = p_interpolated.z();
                p.pose.orientation = new_pose.orientation;

                cut.push_back(p);
                break;
            }

            total_length += points_distance;
            last_point = new_point;
        }
        cut.insert(cut.begin(), trajectory.begin(), end_it);

        return cut;
    }

    // 重采样轨迹
    TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double interval)
    {
        if (trajectory.points.size() < 2) {
            return trajectory.points;
        }

        TrajectoryPoints resampled;

        resampled.push_back(trajectory.points.front());
        auto prev_point = autoware::universe_utils::fromMsg(trajectory.points.front().pose.position);
        for (size_t i = 1; i < trajectory.points.size() - 1; ++i) {
            const auto & traj_point = trajectory.points.at(i);

            const auto next_point = autoware::universe_utils::fromMsg(traj_point.pose.position);

            if (boost::geometry::distance(prev_point.to_2d(), next_point.to_2d()) >= interval) {
                resampled.push_back(traj_point);
            }

            prev_point = next_point;
        }
        resampled.push_back(trajectory.points.back());

        return resampled;
    }

    // 创建车辆足迹
    std::vector<LinearRing2d> createVehicleFootprints(
        const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory,
        const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
        const double footprint_margin_scale)
    {
        // 根据协方差计算纵向和横向边距
        const auto margin = calcFootprintMargin(covariance, footprint_margin_scale);

        // 在base_link坐标系中创建车辆足迹
        const auto local_vehicle_footprint = vehicle_info.createFootprint(margin.lat, margin.lon);

        // 在每个轨迹点上创建车辆足迹
        std::vector<LinearRing2d> vehicle_footprints;
        for (const auto & p : trajectory) {
            vehicle_footprints.push_back(
                transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose)));
        }

        return vehicle_footprints;
    }

    // 创建车辆足迹
    std::vector<LinearRing2d> createVehicleFootprints(
        const PathWithLaneId & path, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
        const double footprint_extra_margin)
    {
        // 在base_link坐标系中创建车辆足迹
        const auto local_vehicle_footprint = vehicle_info.createFootprint(footprint_extra_margin);

        // 在每个路径点上创建车辆足迹
        std::vector<LinearRing2d> vehicle_footprints;
        for (const auto & p : path.points) {
            vehicle_footprints.push_back(transformVector(
                local_vehicle_footprint, autoware::universe_utils::pose2transform(p.point.pose)));
        }

        return vehicle_footprints;
    }
}  // namespace autoware::lane_departure_checker::utils

总结

先到这里，下一篇继续，要不太长了。