autoware.universe源码略读(1)--common

Autoware ad api specs（接口）

autoware.universe的接口规范，在autoware/src/universe/autoware.universe/system/default_ad_api/src/operation_mode.hpp默认接口中有使用到

Autoware auto common（常规定义）

定义了一些常用的数据类型、函数等，像在common/types.hpp类可以找到PointXYZ各种类型的定义，以及在helper_functions/float_comparisons中还可以找到一些数据比较的函数，这里比较有趣的是这个比例比较，计算两个的差，判断差值占最大值的比例是否满足要求

template <typename T>
bool rel_eq(const T & a, const T & b, const T & rel_eps)
{
  static_assert(
    std::is_floating_point<T>::value, "Float comparisons only support floating point types.");

  const auto delta = std::abs(a - b);
  const auto larger = std::max(std::abs(a), std::abs(b));
  const auto max_rel_delta = (larger * rel_eps);	// 大值×比例阈值
  return delta <= max_rel_delta;
}

剩下的还有个马氏距离的计算，源码还贴心地给了原理的注释

/// \brief Calculate square of mahalanobis distance
/// \tparam T Type of elements in the matrix
/// \tparam kNumOfStates Number of states
/// \param sample Single column matrix containing sample whose distance needs to be computed
/// \param mean Single column matrix containing mean of samples received so far
/// \param covariance_factor Covariance matrix
/// \return Square of mahalanobis distance
template <typename T, std::int32_t kNumOfStates>
types::float32_t calculate_squared_mahalanobis_distance(
  const Eigen::Matrix<T, kNumOfStates, 1> & sample, const Eigen::Matrix<T, kNumOfStates, 1> & mean,
  const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor)
{
  using Vector = Eigen::Matrix<T, kNumOfStates, 1>;
  // This is equivalent to the squared Mahalanobis distance of the form: diff.T * C.inv() * diff
  // Instead of the covariance matrix C we have its lower-triangular factor L, such that C = L * L.T
  // squared_mahalanobis_distance = diff.T * C.inv() * diff
  // = diff.T * (L * L.T).inv() * diff
  // = diff.T * L.T.inv() * L.inv() * diff
  // = (L.inv() * diff).T * (L.inv() * diff)
  // this allows us to efficiently find the squared Mahalanobis distance using (L.inv() * diff),
  // which can be found as a solution to: L * x = diff.
  const Vector diff = sample - mean;
  const Vector x = covariance_factor.ldlt().solve(diff);
  return x.transpose() * x;
}

源码还贴心地用gtest写了对应的测试，就在对应的test文件夹下，生成的test文件在build里也可以找到，可以直接执行

当然也可以用colcon工具来查看测试结果

colcon test --packages-select autoware_auto_common
colcon test-result --all

autoware_auto_geometry（几何学）

这里把几个文件都粗略的过了一遍，应该主要用处是后续点云聚类之后生成被检测物体时使用。像主要的功能有根据点生成多边形凸包，然后找到能cover这个凸包的矩形以及对应的信息

bounding_box_common

感觉这个是（二维）物体的边界相关的一些函数

函数名	作用
compute_height	计算Z轴高度信息
size_2d	计算长度和宽度
finalize_box	根据角点来计算物体的一个水平yaw角，并且计算得到中心点

剩下的还有一些类似根据角点来生成对应形状(make_shape)以及生成检测到的物体(make_detected_object)等等函数，就不一一记录了

eigenbox_2d

这里面主要的作用就是生成这个2D的box

函数名	作用
covariance_2d	计算协方差
eig_2d	计算特征向量和特征值
compute_supports	用于计算支撑点。支撑点是包围某个点集的最远点，用于确定包围盒的边界
compute_bounding_box	用于生成边界

上述函数在eigenbox_2d函数中都有被使用到，总体生成二维物体的思路就是：
计算协方差–生成特征向量并找到能覆盖住的二维边界值–生成边界，确定形状

lift

这个文件实现了一个用于估计二维定向边界框的拟合算法，它可以在给定一组点的情况下，找到最佳拟合方向，并生成对应的矩形，这里的算法是参考了论文《Efficient L-shape fitting of laser scanner data for vehicle pose estimation》，文章这里就不精读了，感兴趣的可以看看大佬做的总结

【摸鱼读paper】L-Shape fitting：基于L-Shape的3DLidar障碍物位姿估计 - 等我一下就好啦的文章 - 知乎

rotating_calipers

这个感觉是通过不停地旋转，来寻找最好的拟合边界的过程，主要的过程都在rotating_calipers_impl里
下边给了两种接口，一种是直接传点的迭代器，另一种是直接传点的列表

common_2d

2D点的一些函数，像取出x、y，以及相加相减、旋转等等，里面有一些函数感觉自己也是可以拿出来用的

函数名	作用
intersection_2d	计算两条直线的交点
closest_segment_point_2d	找到直线上距离目标点最近的点
check_point_position_to_line_2d	判断点和直线的位置关系

common_3d

3D点的几个简单函数，包括3D点的内积、距离

convex_hull

这个文件主要是实现了凸包算法，关于什么是凸包，可以参考凸包-维基百科，通俗点理解感觉就是把所有的点能包围住的一个形状，这里的凸包算法可以看一下这篇文章2D凸包算法（三）：Andrew’s Monotone Chain

hull_pockets

这个好像是找到多边形和凸包之间的形状的部分，根据对应的gtest文件test_hull_pockets.hpp来看是这样的，但是我感觉里面有一个注释应该是不太对劲。。。

这里我没懂为什么是top left有一个，而且这个图画得好像也不太对劲，把对应得gtest修改一下，再重新编译试了下

ASSERT_FLOAT_EQ(x_(pockets[0][0]), 4.0);
  ASSERT_FLOAT_EQ(y_(pockets[0][0]), 5.0);
  ASSERT_FLOAT_EQ(x_(pockets[0][1]), 4.0);
  ASSERT_FLOAT_EQ(y_(pockets[0][1]), 2.0);
  ASSERT_FLOAT_EQ(x_(pockets[0][2]), 2.0); // 自己把后两个顶点也加上了
  ASSERT_FLOAT_EQ(y_(pockets[0][2]), 2.0);
  ASSERT_FLOAT_EQ(x_(pockets[0][3]), 2.0);
  ASSERT_FLOAT_EQ(y_(pockets[0][3]), 5.0);

这里全部的gtest都通过了，也就是说明自己加的这两个点也没有问题。所以这里其实就是正上方的一个正方形

intersection（多边形交叉）

这个在官方文档有单独一页的介绍，具体来讲就是得到了两个多边形的交叉区域，并且能够标注出来

1. 首先第一个函数，get_sorted_face_list，我之前以为在convec_hull后得到的就是凸包点了，结果这里又看了下，那里得到的指向凸包点下一个点的迭代器（也就是内点，应该是这么理解吧？），所以这个函数的作用就是把所有的凸包点给拿出来放在一个vector里了
2. 下一个函数append_contained_points把输入的internal多边形里在external多边形里的点拿了出来
3. append_intersection_points是计算两个多边形的所有交点了，这里的逻辑是依次遍历两个两个多边形的两个点形成的边，然后让edge1和edge1用intersection_2d函数来计算交点，再判断交点是不是在两个线段的范围内，在的话就放入result
4. intersect判断两个多边形是否相交的函数（但没找到哪里有使用这个函数），这里用到了分离轴定理，简单说就是依次选择两个多边形边对应的法向量，如果两个多边形在某一个法向量上的投影存在重叠，那就说明两个多边形相交了
5. convex_polygon_intersection2d把两个多边形的所有交点，以及重叠区域的点存放在result里，并且根据这些点也生成了一个凸包
6. convex_intersection_over_union_2d看起来是把两个多边形连着相交区域一起合二为一了

interval

感觉就是(x,y)这种东西，然后写了一些类似两个区间是否有共同范围之类的函数

lookup_table

一维查找表的相关函数，最后的loopup_1d和LookupTable1D是封装后的函数

函数名	作用
interpolate	一维插值
lookup_impl_1d	实现了一维查找的核心逻辑。接受一个查询值，并在给定的定义域上执行查找操作，返回对应的插值结果
check_table_lookup_invariants	检查了定义域和值域是否为空，以及定义域是否按升序排列

spatial_hash

空间hash表在官网也有相对详细的解释，原话是The spatial hash is a data structure designed for efficient fixed-radius near-neighbor queries in low dimensions，所以简单理解就是用来处理点云最近邻问题的。这里主要分为两部分，第一部分是配置部分，也就是spatial_hash_config.hpp，首先配置了x,y,z三个方向的最大最小值，查询半径（这里也就是指进行查找的时候一个单元格的大小，因为看后边的bin_rane函数可以看出来，确定所以的时候是用输入的最大范围radius除这个半径的），以及最大步长之类的信息。然后在spatial_hash.hpp里，主要实现了2D和3D两类，其中涉及到的函数封装比较多，比如最底层的bin_impl是把点索引变成一位索引的函数，但是封装了基层才在insert函数中调用。然后比较重要的可能就是near_impl函数了，这个主要就是先根据输入的参考点，生成对应的bin_range，然后遍历所有的体素，进行距离的比较。

  /// \brief Finds all points within a fixed radius of a reference point
  /// \param[in] x The x component of the reference point
  /// \param[in] y The y component of the reference point
  /// \param[in] z The z component of the reference point, respected only if the spatial hash is not
  ///              2D.
  /// \param[in] radius The radius within which to find all near points
  /// \return A const reference to a vector containing iterators pointing to
  ///         all points within the radius, and the actual distance to the reference point
  const OutputVector & near_impl(
    const float32_t x, const float32_t y, const float32_t z, const float32_t radius)
  {
    // reset output
    m_neighbors.clear();
    // Compute bin, bin range
    const Index3 ref_idx = m_config.index3(x, y, z);  // 参考点索引
    const float32_t radius2 = radius * radius;
    const details::BinRange idx_range = m_config.bin_range(ref_idx, radius);  // 半径范围内的bin
    Index3 idx = idx_range.first;
    // For bins in radius
    do {  // guaranteed to have at least the bin ref_idx is in
      // update book-keeping
      ++m_bins_hit;
      // Iterating in a square/cube pattern is easier than constructing sphere pattern
      if (m_config.is_candidate_bin(ref_idx, idx, radius2)) { // 当前的bin在不在候选bin中
        // For point in bin
        const Index jdx = m_config.index(idx);
        const auto range = m_hash.equal_range(jdx);
        for (auto it = range.first; it != range.second; ++it) {
          const auto & pt = it->second;
          const float32_t dist2 = m_config.distance_squared(x, y, z, pt);
          if (dist2 <= radius2) {   // 距离比较，符合要求的就加入到结果列表中
            // Only compute true distance if necessary
            m_neighbors.emplace_back(it, sqrtf(dist2));
          }
        }
      }
    } while (m_config.next_bin(idx_range, idx));  // 这里更新遍历的索引
    // update book-keeping
    m_neighbors_found += m_neighbors.size();
    return m_neighbors;
  }

当然这个函数也被封装了一层，最终的调用是near函数，也是针对PointT和x,y,z两种形式进行了适配。

visuibility_control

这个应该是用来做代码可见性控制的，在后边很多包里都用到了
GEOMETRY_PUBLIC就是对其他模块也可见
GEOMETRY_LOCAL应该就是仅对当前模块可见，搜了下好像只有insert_impl一个函数是仅当前模块可见的

autoware_auto_perception_rviz_plugin（感知结果rviz可视化）

这个主要是用来做可视化的，把感知检测到的物体在rviz界面可视化出来，主要分成了三种

1. 检测到的物体(detected_objects)
   

2. 跟踪到的物体(tracked_objects)–速度、方向信息等
   

3. 预测的物体(predicted_objects)–预测的轨迹线
   

这里在物体基类成员变量的类型都是rviz_common::properties::BoolProperty，ROS官方文档，主要的输入参数可以概括成

m_display_3d_property = new rviz_common::properties::BoolProperty(
      "Display 3D",   				   // The name of the property in the property tree
      true,          				   // Default value
      "Enable or disable 3D display",  // Description of the property
      this,           				   // The parent property
      SLOT(update3DDisplay()),         // Slot to call when the property changes
      this);          				   // The receiver object for the slot

然后基类里还是先了把接收到的msg分别转换为对应的形状的函数，包括预测轨迹等，然后主要的物体包括

  kDefaultObjectPropertyValues = {
    {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN,
     {"UNKNOWN", {255, 255, 255}}},
    {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}},
    {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}},
    {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN,
     {"PEDESTRIAN", {255, 192, 203}}},
    {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}},
    {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE,
     {"MOTORCYCLE", {119, 11, 32}}},
    {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER,
     {"TRAILER", {30, 144, 255}}},
    {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}};

剩下的大部分都是具体的可视化部分的代码，这里就不做仔细阅读了，后边如果需要自己改的话可以找一下对应部分删删补补应该就好

autoware_auto_tf2（坐标转换）

开发文档里说这个部分是为了"The autoware_auto_tf2 package aims to provide developers with tools to transform applicable autoware_auto_msgs types. In addition to this, this package also provides transform tools for messages types in geometry_msgs missing in tf2_geometry_msgs."，所以这里主要就是为了一些消息类型做运动学和动力学变换，关于KDL我也不了解，这里找了下用到这个变换的部分

// common/autoware_auto_gepmetry
std::vector<geometry_msgs::msg::Point32> GEOMETRY_PUBLIC get_transformed_corners(
  const autoware_auto_perception_msgs::msg::Shape & shape_msg,
  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation)
{
  std::vector<geometry_msgs::msg::Point32> retval(shape_msg.footprint.points.size());
  geometry_msgs::msg::TransformStamped tf;
  tf.transform.rotation = orientation;
  tf.transform.translation.x = centroid.x;
  tf.transform.translation.y = centroid.y;
  tf.transform.translation.z = centroid.z;

  for (size_t i = 0U; i < shape_msg.footprint.points.size(); ++i) {
    tf2::doTransform(shape_msg.footprint.points[i], retval[i], tf);
  }
  return retval;
}

这里我感觉就是从局部坐标系转换到全局坐标系下了，主要有点的变换、四元数（表示旋转）的变换。关于KDL的具体内容需要的话再看

autoware_point_types（点类型）

这里就是定义了一些点的类型，在之前的文件中也已经频繁使用了，PointXYZI应该就是最基本的类型，包含了三轴坐标和一个intensity信息，然后PointXYZIRADRT感觉上是那种激光点云的点，里面还有ring（在哪个线束？），azimuth（方位角）以及distance（距离）和时间戳这种信息

autoware_testing（冒烟测试）

冒烟测试的相关内容，参考官方文档吧

bag_time_manager_rviz_plugin（ROSBAG时间）

这个也没什么好说的，主要是在rviz界面也能控制rosbag包的时间特性了，比如暂停以及倍速播放之类的

component_interface_specs（组件接口）

这里主要是几个组件的接口规范定义，包括了控制（control）、定位（localization）、规划（planning）以及系统（system）四个部分，在后续对应的部分都有这些接口的使用

component_interface_utils（rclcpp工具）

这个包感觉主要是为了rclcpp而写的接口，里面包括服务端和客户端创建、发布和订阅消息等的接口，像topic_publisher就是消息发布的一个类，后边也有使用到，但是没看出来和rclcpp自己的有什么区别。。主要的函数似乎是在rclcpp.hpp里，里面有初始化各种服务类型的函数，似乎是后边很多地方用到了这里的NodeAdaptor这个类

cuda_utils（CUDA工具）

我以前也没接触过CUDA的东西，这里的这几个函数就只能大概猜了一下

1. cuda_check_error：检查 CUDA 函数调用是否出错，并在出错时抛出异常
2. cuda_unique_ptr：这个感觉就是针对CUDA进行了智能指针的设计，更好地管理涉及到CUDA的内存？
3. stream_unique_ptr：CUDA流相关，包括销毁流以及生成流

fake_test_node（GTEST测试）

这个主要是为了在gtest需要使用到ROS进行发布订阅消息时而定义的两个类，官方对两个类的描述分别是

这里我的理解TEST_P就是带有参数版本的TEST_F（也不知道这么理解对不对），具体的使用看了下test_fake_test_node，可以看到里面的两个测试的部分

TEST_F(FakeNodeFixture, Test) { run_test(15, this); }

INSTANTIATE_TEST_SUITE_P(	// 这里传参
  FakeNodeFixtureTests, FakeNodeFixtureParametrized,
  // cppcheck-suppress syntaxError  // cppcheck doesn't like the trailing comma.
  ::testing::Values(-5, 0, 42));

/// @test Test that we can use a parametrized test.
TEST_P(FakeNodeFixtureParametrized, Test) { run_test(GetParam(), this); }

再结合实际运行gtest的结果

所以上边testing::Values(-5, 0, 42))这里就是输入的参数了，然后还有需要注意的一点是，这里其实是先定义了一个FakeNodeCore类，里面实现了set_up和tear_down两个必须的部分（参考文章：linux geogle test教程及TEST_F，TEST_P宏用法），然后FakeTestNode和FakeTestNodeParametrized都是继承这个类实现的。这里感觉也能体现出一些ROS2和ROS1的不同之处，ROS2单独再整理太费时了，就这样边看边学吧

global_params.launch.py（全局参数）

这个相当于是ROS2下的一个launch文件，因为ROS2其实是更推荐使用python来编写launch文件的（对ROS 2Launch文件使用Python、XML和YAML）。结合了一下实际调用launch的时候，是需要输入vehicle_model这个参数的，而use_sim_time这个参数应该表示的是是否使用仿真时间，在generate_launch_description里有给出false的默认值

goal_distance_calculator（目标距离计算）

这个其实是把别的包里的函数封装好后的接口

struct Input
{
  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose;   // 当前位姿
  autoware_auto_planning_msgs::msg::Route::ConstSharedPtr route;  // 路径
};

struct Output
{
  PoseDeviation goal_deviation;       // 目标偏差(lateral/m, longitudinal/m, heading(deg/rad))
};

然后这里又是ROS2的一个新特性了，我本来以为这个长得比较像ros::nodelet，然后具体启动这个节点的话也需要显示地调用一下，结果搜了一下好像ROS2中节点直接在launch文件里调就行，就像这个节点，具体的调用在goal_distance_calculator/launch这里

<node pkg="goal_distance_calculator" exec="goal_distance_calculator_node" name="goal_distance_calculator" output="screen">
    <param from="$(var config_file)"/>
    <param name="oneshot" value="$(var oneshot)"/>
  </node>

grid_map_utils（网格地图）

官方文档说的是”This packages contains a re-implementation of the grid_map::PolygonIterator used to iterate over all cells of a grid map contained inside some polygon.“，其中grid_map我搜了下应该是一个网格地图相关的库，grid_map::PolygonIterator这个函数应该就是其中的一个多边形迭代器，用于遍历网格地图中由多边形定义的区域，然后Autoware这里是基于扫描线算法重新实现了下这个函数
类的两个参数还是网格地图grid_map::GridMap & grid_map与多边形grid_map::Polygon & polygon两个

函数名	作用
calculateSortedEdges	对边进行排序，严格按照大x指向小x的顺序
calculateIntersectionsPerLine	计算扫描线和多边形边的交点
check_table_lookup_invariants	获得能够覆盖给定边的行范围

这里的grid_map涉及到了一些position，自己想没太想明白，看了grid_map库grid_map_core中move解析之后豁然开朗，简单理解就是索引是从grid自己左上开始编号，然后position是中心相对grid map的，比如在test_polygon_iterator设置了大小为(8,5)，那么索引最多对应就是(7,4)了

interpolation（插值）

interpolation_utils

主要功能是检查待插值的数据是否有效

1. validateKeys判断查询的点(query_keys)是否有效
2. validateKeysAndValues索引和值是否有效

linear_interpolation

简单的线性插值，没什么好说的

spherical_linear_interpolation

球面线性插值，主要是对四元数进行的处理的。这里用的是tf2::slerp函数，关于这个的具体原理可以参考这篇文章四元数的球面线性插值（slerp）

spline_interpolation

这个文件对应的是样条插值
其中spline_interpolation对应的是样条插值的实现，官方文档有对于样条插值的简单介绍，
calcSplineCoefficients的作用是计算样条系数的，我们可以看到最终的问题其实是一个三对角矩阵问题

所以根据TDMA算法（对应代码中的solveTridiagonalMatrixAlgorithm）函数，就可以计算出样条系数
之后的getSplineInterpolatedValues就是计算对应点的插值结果了，getSplineInterpolatedDiffValues计算的是对应的点的导数值
spline是计算插值的接口，包含了计算样条系数的过程，返回计算得到的插值点
此外，还提供了一个Akima插值的方法，对应splineByAkima函数，这个方法似乎会让曲线变得更光滑自然一些

spline_interpolation_points_2d

spline_interpolation_points_2d是2D点的样条插值，是分为了XY坐标插值以及yaw角插值
在SplineInterpolationPoints2d这个类中，把点坐标X、Y和yaw角的插值分开处理了，分别计算样条系数然后分别进行插值。在对点的插值处理中，因为不再是简单的一维数据，所以这里涉及到了欧氏距离，会涉及到每个点到第一个点的欧氏距离，后面的插值也是基于这个距离进行的（应该）。

zero_order_hold

零阶保持，零阶保持的基本思想是在每个区间内保持前一个基准值不变（所以这样插值插出来是一段一段的？），换个意思讲就是找到前一个的值，直接用前一个的值作为当前值了，PS.随便拿出一个GTEST测试看了下，确实是这个意思

  {  // straight: query_keys is random
    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
    const std::vector<double> base_values{0.0, 1.5, 3.0, 4.5, 6.0};
    const std::vector<double> query_keys{0.0, 0.7, 1.9, 4.0};
    const std::vector<double> ans{0.0, 0.0, 1.5, 6.0};

    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
    for (size_t i = 0; i < query_values.size(); ++i) {
      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
    }
  }

像这里，0.0和0.7都对应到0.0对应的value，1.9就对应到1.0对应的，4.0就是4.0自己的了

kalman_filter（卡尔曼滤波）

关于卡尔曼滤波，相关的文献非常之多，这里不再赘述，这里对应的参数里面都有写

  /**
   * @brief initialization of kalman filter
   * @param x initial state
   * @param A coefficient matrix of x for process model
   * @param B coefficient matrix of u for process model
   * @param C coefficient matrix of x for measurement model
   * @param Q covariance matrix for process model
   * @param R covariance matrix for measurement model
   * @param P initial covariance of estimated state
   */
    Eigen::MatrixXd x_;  //!< @brief current estimated state
  	Eigen::MatrixXd
    	A_;  //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k]
  	Eigen::MatrixXd
    	B_;  //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k]
  	Eigen::MatrixXd C_;  //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k]
  	Eigen::MatrixXd Q_;  //!< @brief covariance matrix for process model x[k+1] = A*x[k] + B*u[k]
  	Eigen::MatrixXd R_;  //!< @brief covariance matrix for measurement model y[k] = C * x[k]
  	Eigen::MatrixXd P_;  //!< @brief covariance of estimated state

predict函数就是预测步骤，然后update函数对应的就是更新环节，里面针对不同的输入状况都预留了对应的接口
至于这里的time_delay_kalman_filter，我没有完全想明白，但看起来应该先是把状态向量扩展了

  dim_x_ = x.rows();
  dim_x_ex_ = dim_x_ * max_delay_step;

  x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
  P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);

  for (int i = 0; i < max_delay_step_; ++i) {
    x_.block(i * dim_x_, 0, dim_x_, 1) = x;
    P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0;
  }

然后在predictWithDelay中，根据时间延迟模型，进行了预测。我的理解就是新的协方差矩阵，是包含当前状态和延迟状态之间的协方差信息的。因为我也没用过这种带有时间延迟的滤波，之后用到的时候再深入研究一下，带有时间延迟的模型如下：

motion_utils（车辆的运动状态）

这个主要是反应车辆运动状态的一个库，里面包含了几个小模块

1. marker_helper：用来生成marker的，里面好像是针对状态设置了一些虚拟的墙(VirtualWall)
2. resample：对位置Pose、路径Path以及轨迹Trajectory进行重采样的部分，里面主要用到了之前介绍到的插值部分的函数，然后重采样主要可以分成两种，一种是指定长度resampled_arclength，一种是指定采样间隔resample_interval
3. trajectory：运动轨迹的一些特性，其中主要的作用就是根据给定的点，以及轨迹线，来找到最近的线段以及点的索引（这里面代码有点多，后边用到的时候具体看一下对应的函数）
4. vehicle：车辆状态的一些反馈，比如车辆是否停止以及是否到达等

osqp_interface（OSQP库接口）

这个模块主要是为OSQP库写的接口，以及进行格式的转换。OSQP库我的理解就是对带有一组线性约束的最小二乘问题进行求解，官方的描述都叫做二次规划(QP)，就像官网给出的这种问题一样

这里先定义了CSC矩阵，即列压缩系数矩阵的转换。稀疏矩阵中大多数元素为零，而CSC格式只存储非零元素及其索引，从而节省内存和提高计算效率。这里的转换很简单，其实就是遍历矩阵元素，然后判断是否为0，不是0的话分别把值存进vals，行索引存进row_idxs，列索引存进col_idxs
在osqp_interface中，构造类的时候的几个矩阵就直接对应了上图中的P、q、l、A和u

  /// \brief Constructor with problem setup
  /// \param P: (n,n) matrix defining relations between parameters.
  /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound.
  /// \param q: (n) vector defining the linear cost of the problem.
  /// \param l: (m) vector defining the lower bound problem constraint.
  /// \param u: (m) vector defining the upper bound problem constraint.
  /// \param eps_abs: Absolute convergence tolerance.
  OSQPInterface(
    const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
    const std::vector<double> & l, const std::vector<double> & u, const c_float eps_abs);
  OSQPInterface(
    const CSC_Matrix & P, const CSC_Matrix & A, const std::vector<double> & q,
    const std::vector<double> & l, const std::vector<double> & u, const c_float eps_abs);

如果输入的是Eigen格式的矩阵，就会在initializeProblem函数中，先把矩阵都转换成CSC格式，在初始化的函数里会把这些数据都赋给m_data，然后会初始化OSQPWorkspace，具体的求解会先调用optimize函数，如果格式不对的话还是先格式转换，对了的话直接调用solve函数进行求解（当然里面的核心还是用OSQP自己的函数求解的）。

• 二次规划问题的具体求解原理只是简单看了下，可能没时间和精力去深入学习了

path_distance_calculator（计算到某个路径的距离）

这里核心的计算功能其实是调用之前motion_utils包里的calcSignedArcLength函数计算距离的，当然这里还进行了path话题的订阅以及~/output/distance这个话题的发布

perception_utils（感知模块的常用函数）

这里的函数应该都是为了感知模块而服务的，具体的感知操作并不在这里~！

conversion

顾名思义，就是一个转换的功能，这里也很简单，把检测到的物体变为要跟踪的物体toTrackedObject(s)，以及把跟踪的物体转为要检测的物体toDetectedObject(s)

geometry

得到各种消息类型的物体/位置消息的pose

matching

这里很有意思，这个主要是计算多边形各种特征的。不过之前在autoware_auto_gemotry模块里其实已经写过计算凸包、多边形相交或者相加之类的函数了，但是这里还是用的boost库的一些函数。这里还涉及到一些概念：

1. IoU：这个算的是两个多边形重叠区域占总区域的一个比例，我的理解是如果两个完全重合，这个值就是1了，这个时候也就说明检测到的非常准（图片引自深度学习中的IoU概念理解）
   
2. Precision：准确率，与点云匹配中的概念类似，这里计算的是相交区域占源区域的比值
3. Recall：召回率，相交区域占目标区域的比值

object_classification

对检测到的物体进行分类，可以看到里面应该是有存是各种物体的可能性的，找到最大可能性的分类。最后得到的都是label，当然看起来这里主要是针对各种vehicle的。

predicted_path_utils

这里是生成预测路线的一些函数，第一个是calcInterpolatedPose，这个主要是根据时间，来对路径上的点进行插值；第二个是resamplePredictedPath预测的路径进行重采样，其实也是插值，只不过上边返回的是点(Pose)，这个返回的是路径(Path)。

transform

首先这里的函数定义涉及到了C++17的新特性，这里的maybe_unused是为了避免编译器发出未使用变量或函数的警告。

[[maybe_unused]] inline boost::optional<geometry_msgs::msg::Transform> getTransform(
  const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
  const std::string & target_frame_id, const rclcpp::Time & time)

这里是使用了ROS自带的坐标转换查询函数lookupTransform，来查询source_frame_id到target_frame_id的转换关系
而另一个函数transformObjects就是利用转换关系对Objects进行坐标转换的函数了，这里的坐标变换都是基于tf2::Transform进行的，包括物体原本的位姿也被转换成了这种类型，之后再直接应用相同类型的转换矩阵，从而实现坐标的转换

polar_grid（rviz极坐标网格可视化）

这里全是可视化的一些东西，这个极坐标网格感觉可能是为雷达点云准备的，或者就是我理解的那个向外扩散的圆弧状的东西的可视化，应该用处就是在以车辆自身为原点向外扩散的部分吧

rtc_manager_rviz_plugin（RTC可视化）

根据ChatGPT的解释：RTC通常指的是 Region of Interest (ROI) Traffic Control。这个概念主要涉及在特定的区域内对交通行为进行控制和管理。RTCManagerPanel是一个用于管理和控制这些区域交通控制（RTC）的面板。

这个看起来依旧是rviz可视化相关的东西，根据README文件，可以实现的作用是：

signal_processing（信号处理）

这个模块主要是包含了数据处理的一些方法，看起来是分为了低通滤波和ButterWorth

lowpass_filter

这里的gain_，代表的就是滤波系数，然后每次都是用上一次的值和当前值进行的一个滤波，计算的公式就是：

butterworth

这个比低通滤波复杂了很多，可以参考Autoware的官方手册以及维基百科的解释。具体的实现原理这里就不再仔细说了

函数名	作用
Buttord	根据通带和阻带频率以及波纹容限计算滤波器的最小阶数。还计算截止频率
computeContinuousTimeRoots	为每个根计算实部和虚部并存储在数组中
poly	遍历根数组，使用递推公式更新系数数组

tensorrt_common（TensorRT库）

这个应该是为了调用TensorRT库而编写的一些函数，看了下应该只是涉及到了模型加载的功能

TensorRT 是 NVIDIA 提供的一种高性能深度学习推理库。它专门用于加速神经网络的推理过程，并且针对 NVIDIA GPU 进行了优化。

1. TrtCommon构造函数，主要是用来设置相关的配置参数，完成一些初始化相关的工作
2. setup用来加载模型文件，根据模型是否加载成功来设置是否初始化成功
3. loadEngine用来加载engine格式的文件，使用runtime_的deserializeCudaEngine方法反序列化引擎字符串，创建 CUDA 引擎实例。
4. buildEngineFromOnnx用来加载onnx格式的文件，里面涉及到了构建神经网络的过程？不过最终的目的还是为了创建CUDA引擎实例，这里用的是builder->buildEngineWithConfig(*network, *config)这个方法

tier4_api_utils（旧接口）

这个按官方手册的话说是关于logger的一个旧接口，新的接口在component_interface_utils里，所以这里就不看了

tier4_autoware_utils（常用函数库）

按官方文档的话说这里包含了很多其他地方会用到的函数，事实上之前的一些模块也确实用使用到tier4_autoware_utils命名空间的函数。官方文档表示直接或间接地引用tier4_autoware_utils.hpp头文件会在预处理过程中耗费过多的时间，所以后边如果需要二次开发的话应该是删掉这个文件，然后都这直接引用子文件

C++新特性

1. maybe_unused：防止一些函数未使用时，编译带来的警告
2. nodiscard：函数返回值应该被使用，未被使用时进行警告
3. deprecated：标记不推荐使用的实体，编译时生成对应的警告

geometry

看起来Autoware的几何相关的部分是基于boost::geometry实现的，所以在这里首先就定义了一些别名以方便其他地方的使用。
boost::geometry剩下的就是2D点和3D点相关的基本操作，比如相互转换（粗略转换）以及消息类型转换等等。
boost_polygon_utils是针对多边形的，主要功能是生成2D的多边形
geometry各种工具函数，大多不复杂（不过还是有很多函数明明有对应的模块，但还是用第三方库实现的，比如部分插值相关的内容）
path_with_lane_id_geometry对PathPointWithLaneId这个类型的路径的设置
pose_deviation计算位置之间的差值

math

数学常值（圆周率以及重力值），角度归一化，区间生成以及度和弧度转换、m/s与km/h转换

ros

debug_publisher用来发布各种调试信息
debug_traits没太看懂，似乎是把一些消息类型设置成debug消息了？
marker_helper用来生成可视化的标记(Marker)
msg_covariance各个变量之间的协方差对应的索引
msg_operation针对四元数进行的加减号的重载
processing_time_publisher主要是把处理时间转为消息发布
self_pose_listener和transform_listener分别是对自身位置和tf2里转换关系的监听
update_param用来更新参数,wait_for_param用于在 ROS2 中等待获取远程节点的参数值，并通过参数客户端进行参数获取。

stop_watch

用来计时的，本质上还是基于std::chrono实现的

tier4_calibration_rviz_plugin（可视化）

这个也是在rviz中添加了一个panel，看名字是在刹车的时候的地图校准，但是没太想明白具体是怎么回事，等看到更具体的使用的时候再说吧

• PS：这里和后面的可视化，应该都是涉及到Qt编程了的？因为以前没接触过，所以在vscode中也没配好，很多函数也看不到定义啥的，所以这里真的只是单纯看了下每个可视化的功能，没有研究具体是怎么编写的。不过总的来说其实每个可视化执行的也都是ROS的话题订阅发布、服务发布这些，rviz可视化只不过是在rviz的界面上增加了这些属性

tier4_control_rviz_plugin（可视化+1）

也是rviz可视化的一些东西，这个主要是车辆的人为控制的一些东西，加了之后就是分别设置速度以及控制转盘的旋钮来控制车辆的运动，当然具体的实现肯定也是通过话题的发布来实现车辆的运动的，在示例里面添加着试了一下，开起来还挺麻烦的

这里目前好像是只对Ackermann一种模型有效？gear_cmd应该是指对发动机的控制（直接相关的就是加速度），对应有三种，行驶(DRIVE)、反向(REVERSE)以及停泊(PARK)

tier4_datetime_rviz_plugin（还是可视化）

好像就是显示日期和时间的一个panel，没有什么特殊的用途，显示的一共是两个时间

• ROS time
• WALL time:这个指的应该是程序从开始执行到结束，也就是当前时刻所花费的时间

tier4_debug_rviz_plugin（继续可视化）

这个也没完全看懂，看起来是显示那种占比百分图的可视化部分

tier4_debug_tools（调试工具）

这里面也是一些可能会用到的工具，这里面
有一些用python写的，比如tf2pose，就是根据对tf_buffer的监听，把得到的transform转换成Pose
至于用C++写的这个lateral_error_publisher，示意图表示的比较清楚

这里还有个stop_reason，这个可能就是字面意思？导致车辆停止的原因？还不是很确定

tier4_localization_rviz_plugin（又来可视化，定位轨迹）

这个是历史定位轨迹的可视化

tier4_perception_rviz_plugin（感知可视化）

这里主要是把感知检测到的各种物体进行可视化，有车辆位置(car_pose)、交互式的物体(interactive_object)、行人(pedestrian_pose)，还有一个是对鼠标事件的捕捉，通过鼠标获取到点(get_point_from_mouse)。

tier4_planning_rviz_plugin（路径规划的可视化）

控制相关部分的可视化，这里再重新来梳理两个概念
Path：路径，从起点到终点，避开障碍物且遵循路线规划的路径
Trajectory：轨迹，包含了动力学信息，感觉是结合了动态信息的，还包括了时间属性

tier4_screen_capture_rviz_plugin（屏幕抓取）

看了下应该就是对屏幕进行的截屏处理，具体的抓取函数应该是这个

void AutowareScreenCapturePanel::onClickScreenCapture()
{
  const std::string time_text = "capture/" + ros_time_label_->text().toStdString();
  getDisplayContext()->getViewManager()->getRenderPanel()->getRenderWindow()->captureScreenShot(
    time_text + ".png");
}

tier4_simulated_clock_rviz_plugin（时间控制）

主要作用就是在界面控制时钟暂停、加速等等

tier4_state_rviz_plugin（状态展示）

这个对应的就是左侧显示车辆状态的那部分，这里我的分支是galatic分支的代码，最新的代码这个界面以及一些名字应该是发生了改变的，不过目的没什么区别
旧版界面：

新的界面：

tier4_traffic_light_rviz_plugin（红绿灯）

这部分是负责交通信号灯的，之前在官方demo教程中，是有对这个Panel的介绍的，包括如何添加，如何设置属性等等，所以这个部分就是这里的具体实现。红绿灯应该是从矢量地图中提取到的：

void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg)
{
  if (received_vector_map_) return;
  // NOTE: examples from map_loader/lanelet2_map_visualization_node.cpp
  // 解析lanelet地图
  lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);
  lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map);
  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map);
  // 获取所有交通灯
  std::vector<lanelet::TrafficLightConstPtr> tl_reg_elems =
    lanelet::utils::query::trafficLights(all_lanelets);
  std::string info = "Fetching traffic lights :";
  std::string delim = " ";
  // 获取所有交通灯的id并且更新log
  for (auto && tl_reg_elem_ptr : tl_reg_elems) {
    for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) {
      auto id = static_cast<int>(traffic_light.id());
      info += (std::exchange(delim, ", ") + std::to_string(id));
      traffic_light_ids_.insert(id);
    }
  }
  RCLCPP_INFO_STREAM(raw_node_->get_logger(), info);
  received_vector_map_ = true;

  // 更新UI组件，加到下拉列表里
  for (auto && traffic_light_id : traffic_light_ids_) {
    traffic_light_id_input_->addItem(QString::fromStdString(std::to_string(traffic_light_id)));
  }
}

tier4_vehicle_rviz_plugin（车辆行驶信息）

这个就是在网格地图上显示出来的车辆的速度、转向的那些信息。然后这个是覆盖在地图上的

time_utils（时间消息）

这个部分主要是时间信息和msg的一个连接，然后这里也有一个stopwatch的计时，不过只通过measure得到了耗费时间
然后通过to_message和from_message实现了时间与msg的一个连接

trtexec_vendor（TensorRT可执行文件）

这里主要是一个脚本，目的就是找到trtexec可执行文件，并把这个路径添加到环境变量中
这个文件看了一下好像是能够把ONNX文件之类的转换成engine引擎文件，具体的使用可以参考一下两篇文章

1. TensorRT教程3：使用trtexec工具转engine
2. 从零开始 TensorRT（4）命令行工具篇：trtexec 基本功能

tvm_utility（端到端深度学习编译器）

首先关于TVM，这个概念也是第一次接触：

Apache TVM 是开源的机器学习编译器框架，用于 CPU、GPU 和机器学习加速器。它的目标是让机器学习工程师在任何硬件后端优化和高效运行计算。本教程的目的是通过定义和演示关键概念，引导读者了解 TVM 的所有主要特性。新用户应该能够从头到尾地学习本教程，并能够操作 TVM 进行自动模型优化，同时对 TVM 体系结构及其工作原理有基本的了解。

关于深度学习、机器学习的东西，因为现在没什么基础，所以赞数跳过，后边稍微了解一下原理后再仔细来看
这里我的理解是主要的执行步骤在这个schedule里面：

  TVMArrayContainerVector schedule(const TVMArrayContainerVector & input)
  {
    // Set input(s)
    for (uint32_t index = 0; index < input.size(); ++index) {
      if (input[index].getArray() == nullptr) {
        throw std::runtime_error("input variable is null");
      }
      set_input(config_.network_inputs[index].first.c_str(), input[index].getArray());
    }

    // Execute the inference
    execute();

    // Get output(s)
    for (uint32_t index = 0; index < output_.size(); ++index) {
      if (output_[index].getArray() == nullptr) {
        throw std::runtime_error("output variable is null");
      }
      get_output(index, output_[index].getArray());
    }
    return output_;
  }

关于YOLOv2 Tiny Example Pipeline，这里本来想跑了一下这个示例来体验了一下这里究竟做了什么工作。不过我发现官方所说的这个只适合他最新的代码，galatic分支下的代码好像是不支持这个示例的？因为都没有launch文件，试图直接用最新的代码编译也没搞明白。。所以这里就暂时放弃了，单独跑了一下生成的gtest

expected_output[0] = 0.356000; 
expected_output[1] = 0.763036;
expected_output[2] = 0.402512; 
expected_output[3] = 0.300442;
expected_output[4] = 0.612652; 
expected_output[5] = 0.581861; 
expected_output[6] = 0.421924;
expected_output[7] = 0.456947;
expected_output[8] = 0.378341;
expected_output[9] = 0.806586; 
expected_output[10] = 0.575851;

把输出转为float打印出来了，没太想出来具体输出到底是什么。根据图片来看应该至少能检测到狗和车吧？输出了11个数字也没太看明白。后面又去看了下最新的代码里的output，和这里是一样的，也是11个数字。我怀疑可能是对检测到的物体的概率？比如9对应的可能就是狗，然后1可能对应的就是车

web_controller（网络连接）

这里都是用js写的了，这就看不懂了，不过总体的作用就是可以打开一个网页来改变看到Autoware的状态以及发送消息

总结

到此，autoware的第一个模块common应该就算是看完了，这个模块更多的是一些基础函数、基础类（多边形）以及可视化的部分，所以一些不太涉及原理的代码大多看了一遍，也没有仔细思考怎么实现的就带过了。很多函数可能也没有搞得特别清楚，想着还是后边如果有用到，再边用边看具体的函数，这样可能会更利于理解一些。