lidar_utils 
类：
1.AngleFilter
描述：过滤器类，以检查点的方位角是否在由开始角和结束角定义的范围内。范围定义为沿逆时针方向从开始角到结束角。
公共类型

using VectorT = autoware∷common∷types∷PointXYZIF;

公共成员函数

1.1.

AngleFilter(float32_t start_angle,float32_t end_angle)

描述：构造函数。

/// @brief 构造函数
/// @param start_angle最小角度，单位为弧度
/// @param end_angle最大角度，单位为弧度
AngleFilter::AngleFilter(float32_t start_angle,float32_t end_angle)
{
  using autoware∷common∷geometry∷make_unit_vector2d;
  using autoware∷common∷geometry∷cross_2d;
  using autoware∷common∷geometry∷get_normal;
  using autoware∷common∷geometry∷plus_2d;
  using autoware∷common∷geometry∷times_2d;
  using autoware∷common∷geometry∷norm_2d;
  using autoware∷common∷geometry∷dot_2d;

  const auto start_vec = make_unit_vector2d<VectorT>(start_angle);
  const auto end_vec = make_unit_vector2d<VectorT>(end_angle);

  // 处理两个角度方向相反的情况(small_angle_dir = 0)
  if (std∷fabs(dot_2d(start_vec,end_vec) - (-1.0F)) < FEPS) {
    m_range_normal = get_normal(start_vec);
  } else {
    // 范围法线是在可接受范围的中间的单位向量。(规范化(开始+结束)
    m_range_normal = plus_2d(start_vec,end_vec);
    m_range_normal = times_2d( m_range_normal,(1.0F / norm_2d(m_range_normal)));

    // 如果小角度不是在CCW方向，那么我们就需要大角度，这样法线就倒了。
    const auto small_angle_dir = cross_2d(start_vec,end_vec);
    if ((small_angle_dir + FEPS) < 0.0F) {
      m_range_normal = times_2d(m_range_normal,-1.0F);
    }
  }

  // 阈值是可接受角度范围的一半的余弦值。
  // 几何解释:
  // 一个点的方位角越接近距离法线，它在距离法线上的投影长度就越大。(与夹角的余弦成正比)。
  // 因此投影长度是一个点到角度范围的角距离的指示器。角度范围的最小值和最大值定义了这个度量的下界。
  const auto thresh = dot_2d(start_vec,m_range_normal);
  m_threshold_negative = (thresh + FEPS) < 0.0F;
  // Square是预先计算的，因为检查将发生在Square空间中，以避免sqrt()调用。
  m_threshold2 = thresh * thresh;
}

1.2.

bool8_t operator()(const T & pt) const

描述：检查一个点的方位角是否在逆时针方向的范围[start,end]。该点被视为一个2D矢量，其在范围法线上的投影与一个阈值进行比较。

/// @brief 检查一个点的方位角是否在逆时针方向的范围[start,end]。该点被视为一个2D矢量，其在范围法线上的投影与一个阈值进行比较。
/// @tparam T Point type
/// @param pt point to check
/// @return 如果点包含在范围内则返回true。
template<typename T>
bool8_t operator()(const T & pt) const
{
  using common∷geometry∷dot_2d;
  bool8_t ret = false;

  // 向量大小的平方
  const auto pt_len2 = dot_2d(pt,pt);
  const auto proj_on_normal = dot_2d(pt,m_range_normal);
  const auto proj_on_normal2 = proj_on_normal * proj_on_normal;
  const auto is_proj_negative = (proj_on_normal + FEPS) < 0.0F;

  // 由于输入向量的投影是按其自身的长度进行缩放的，
  // 因此阈值也是按输入向量的长度进行缩放的，以使比较成为可能。

  // 为了避免使用平方根计算长度，表达式保持为平方形式，因此进行下列符号检查以确保平方形式的比较的正确性。
  if ((!m_threshold_negative) && (!is_proj_negative)) {
    ret = (proj_on_normal2) >= (pt_len2 * (m_threshold2 - FEPS));
  } else if (m_threshold_negative && (!is_proj_negative)) {
    ret = true;
  } else if ((!m_threshold_negative) && is_proj_negative) {
    ret = false;
  } else {
    ret = (proj_on_normal2) <= (pt_len2 * (m_threshold2 + FEPS));
  }
  return ret;
}

静态公共属性

static constexpr float32_t PI = 3.14159265359F;
static constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon() * 1e2F;

2.DistanceFilter
描述：筛选类，以检查点是否位于由最小和最大半径定义的范围内。
公共成员函数

2.1.

DistanceFilter(float32_t min_radius,float32_t max_radius)

描述：构造函数

/// @brief 构造函数
/// @param min_radius 这个点的半径应该大于min_radius
/// @param max_radius这个点的半径应该小于max_radius
DistanceFilter∷DistanceFilter(float32_t min_radius,float32_t max_radius)
: m_min_r2(min_radius * min_radius),m_max_r2(max_radius * max_radius)
{
  if (m_max_r2 < m_min_r2) {
    throw std∷domain_error("DistanceFilter: max_radius cannot be less than min_radius");
  }
}

2.2.

bool8_t operator()(const T & pt) const

描述：检查点是否在滤镜允许的范围内。检查是用平方的形式，以避免平方根。

/// @brief 检查点是否在过滤器允许的范围内。检查以正方形形式进行，以避免“sqrt”
/// @tparam T点类型
/// @param pt要过滤的指针
/// @return 如果point在过滤器的范围内，则返回True。
template<typename T>
bool8_t operator()(const T & pt) const
{
  using common∷geometry∷dot_3d;
  auto pt_radius = dot_3d(pt,pt);
  return comp∷abs_gte(pt_radius,m_min_r2,FEPS) && comp∷abs_lte(pt_radius,m_max_r2,FEPS);
}

静态公共属性

static constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();

3.IntensityIteratorWrapper
公共成员函数

3.1.

explicit IntensityIteratorWrapper(const PointCloud2 & msg);

描述：构造函数。

IntensityIteratorWrapper∷IntensityIteratorWrapper( const PointCloud2 & msg)
: m_intensity_it_uint8(msg,"intensity"),m_intensity_it_float32(msg,"intensity")
{
  auto && intensity_field_it =
    std∷find_if( std∷cbegin(msg.fields),std∷cend(msg.fields),
    [](const sensor_msgs∷msg∷PointField & field) {return field.name == "intensity";});
  m_intensity_datatype = (*intensity_field_it).datatype;
  switch (m_intensity_datatype) {
    case sensor_msgs::msg::PointField::UINT8:
    case sensor_msgs::msg::PointField::FLOAT32:
      break;
    default:
      throw std::runtime_error("Intensity type not supported: " + m_intensity_datatype);
  }
}

3.2.

bool8_t IntensityIteratorWrapper∷eof()

描述：判断类型是否正确。

bool8_t IntensityIteratorWrapper∷eof()
{
  switch (m_intensity_datatype) {
    // 由于某种原因，相等操作符(==)不能用于PointCloud2ConstIterator
    case sensor_msgs∷msg∷PointField∷UINT8:
      return !(m_intensity_it_uint8 != m_intensity_it_uint8.end());
    case sensor_msgs∷msg∷PointField∷FLOAT32:
      return !(m_intensity_it_float32 != m_intensity_it_float32.end());
    default:
      throw std∷runtime_error("Intensity type not supported: " + m_intensity_datatype);
  }
}

3.3.

void IntensityIteratorWrapper∷next()

描述：下一个数据

void IntensityIteratorWrapper∷next()
{
  switch (m_intensity_datatype) {
    case sensor_msgs∷msg∷PointField∷UINT8:
      ++m_intensity_it_uint8;
      break;
    case sensor_msgs∷msg∷PointField∷FLOAT32:
      ++m_intensity_it_float32;
      break;
    default:
      throw std∷runtime_error("Intensity type not supported: " + m_intensity_datatype);
  }
}

3.4.

void get_current_value(PointFieldValueT & point_field_value)

描述：获得当前值。

template<typename PointFieldValueT>
void get_current_value(PointFieldValueT & point_field_value)
{
  switch (m_intensity_datatype) {
    case sensor_msgs∷msg∷PointField∷UINT8:
      point_field_value = *m_intensity_it_uint8;
      break;
    case sensor_msgs∷msg∷PointField∷FLOAT32:
      point_field_value = *m_intensity_it_float32;
      break;
    default:
      throw std∷runtime_error("Intensity type not supported: " + m_intensity_datatype);
  }
}

4.PointCloudIts
描述：点云迭代器包装器，允许重用迭代器。由于实现了PointCloud2IteratorBase<⋯>∷set_field方法，迭代器的重新初始化代价非常高。
构造函数

PointCloudIts();

描述：构造函数

/// @brief 创建新的迭代器包装器，为4个迭代器保留空间，即x，y，z和强度
PointCloudIts∷PointCloudIts() {m_its.reserve(4);}

公共成员函数

4.1.

void reset(sensor_msgs∷msg∷PointCloud2 & cloud,uint32_t idx = 0);

描述：重置。

/// @brief 将给定云的迭代器重置为给定索引
/// @param[in] cloud将迭代器重置为的点云
/// @param[in] idx迭代器向前移动的索引/偏移量
void PointCloudIts::reset(sensor_msgs::msg::PointCloud2 & cloud,uint32_t idx)
{
  // 销毁旧的迭代器
  m_its.clear();
  // 创建新的迭代器
  m_its.emplace_back(cloud,"x");
  m_its.emplace_back(cloud,"y");
  m_its.emplace_back(cloud,"z");
  m_its.emplace_back(cloud,"intensity");
  // 到给定索引的高级迭代器
  x_it() += idx;
  y_it() += idx;
  z_it() += idx;
  intensity_it() += idx;
}

4.2.

sensor_msgs∷PointCloud2Iterator<float32_t> & x_it()

描述：返回x字段的迭代器。

/// \brief 返回x字段的迭代器
inline sensor_msgs∷PointCloud2Iterator<float32_t> & x_it()
{
  return m_its[0];
}

4.3.

sensor_msgs∷PointCloud2Iterator<float32_t> & y_it()

描述：返回y字段的迭代器。

/// @brief 返回y字段的迭代器
inline sensor_msgs∷PointCloud2Iterator<float32_t> & y_it()
{
  return m_its[1];
}

sensor_msgs∷PointCloud2Iterator<float32_t> & z_it()
/// \brief 返回z字段的迭代器
inline sensor_msgs∷PointCloud2Iterator<float32_t> & z_it()
{
  return m_its[2];
}
sensor_msgs::PointCloud2Iterator<float32_t> & intensity_it()
描述：
/// \brief 返回“intensity”字段的迭代器
inline sensor_msgs∷PointCloud2Iterator<float32_t> & intensity_it()
{
  return m_its[3];
}
    StaticTransformer
描述：变换以对给定的点应用常数变换。
    构造函数
explicit StaticTransformer(const geometry_msgs::msg::Transform & tf);
描述：构造函数。预计算旋转和平移矩阵从转换msg。
/// \brief 的构造函数。预计算旋转和平移矩阵从转换msg。
/// \param tf转换msg以应用于点。
StaticTransformer∷StaticTransformer(const geometry_msgs∷msg∷Transform & tf)
{
  Eigen∷Quaternionf rotation(tf.rotation.w,tf.rotation.x,tf.rotation.y,tf.rotation.z);
  if (!comp∷abs_eq(rotation.norm(),1.0f,EPSf)) {
    throw std∷domain_error("StaticTransformer: quaternion is not normalized");
  }
  m_tf.setIdentity();
  m_tf.rotate(rotation);
  m_tf.translate(Eigen::Vector3f(tf.translation.x,tf.translation.y,tf.translation.z));
}
    公共成员函数
void transform(const T & ref,T & out) const
描述：将转换应用到定义了适当点适配器的点。
/// \brief 将转换应用到定义了适当点适配器的点。
/// \tparam T Point type
/// \param ref 输入点
/// \param out 输出点参考
template<typename T>
void transform(const T & ref,T & out) const // NOLINT(假阳性:这不是std∷transform)
{
  using common∷geometry∷point_adapter∷x_;
  using common∷geometry∷point_adapter∷y_;
  using common∷geometry∷point_adapter∷z_;
  using common∷geometry∷point_adapter∷xr_;
  using common∷geometry∷point_adapter∷yr_;
  using common∷geometry∷point_adapter∷zr_;
  Eigen∷Matrix<float32_t,3U,1U> ref_mat({x_(ref),y_(ref),z_(ref)});
  Eigen∷Vector3f out_mat = m_tf * ref_mat;
  xr_(out) = out_mat(0U,0U);
  yr_(out) = out_mat(1U,0U);
  zr_(out) = out_mat(2U,0U);
}

结构体
    _create_custom_pcl_datatype
template<typename T>
struct _create_custom_pcl_datatype;

template<>
struct _create_custom_pcl_datatype<int8_t>
{
  static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷INT8;
};

template<>
struct _create_custom_pcl_datatype<uint8_t>
{
  static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷UINT8;
};

template<>
struct _create_custom_pcl_datatype<int16_t>
{
  static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷INT16;
};

template<>
struct _create_custom_pcl_datatype<uint16_t>
{
  static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷UINT16;
};

template<>
struct _create_custom_pcl_datatype<int32_t>
{
  static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷INT32;
};

template<>
struct _create_custom_pcl_datatype<uint32_t>
{
  static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷UINT32;
};

template<>
struct _create_custom_pcl_datatype<float32_t>
{
  static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷FLOAT32;
};

template<>
struct _create_custom_pcl_datatype<float64_t>
{
  static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷FLOAT64;
};
    SafeCloudIndices
描述：
struct SafeCloudIndices
{
  std∷size_t point_step;
  std∷size_t data_length;
};
函数
#include<lidar_utils.hpp>
    float32_t fast_atan2(float32_t y,float32_t x)
描述：近似值来自：http://www-labs.iro.umontreal.ca/~mignotte/IFT2425/Documents/EfficientApproximationArctgFunction.pdf 
///
/// Approximation was taken from:
/// http://www-labs.iro.umontreal.ca/~mignotte/IFT2425/Documents/EfficientApproximationArctgFunction.pdf
///
/// |Error = fast_atan2(y,x) - atan2f(y,x)| < 0.00468 rad
///
/// Octants:
///         pi/2
///       ` 3 | 2 /
///        `  |  /
///       4 ` | / 1
///   pi -----+----- 0
///       5 / | ` 8
///        /  |  `
///       / 6 | 7 `
///         3pi/2
///
///
float32_t fast_atan2(float32_t y,float32_t x)
{
  constexpr float32_t scaling_constant = 0.28086f;
  if (x == 0.0f) {
    // 特殊情况atan2(0.0,0.0) = 0.0
    if (y == 0.0f) {
      return 0.0f;
    }
    // x等于0，所以当(y > 0)时，值是Pi/2；或者当(y< 0)时，值是-Pi/2
    return ∷std∷copysign(autoware∷common∷types∷PI_2,y);
  }
  // 计算y和x的商
  float32_t div = y / x;

  // 如果|y|小于|x|(|div|<1)，那么我们要么是在1、4、5或8，要么是在2、3、6或7。
  if (fabsf(div) < 1.0f) {
    // 当在1、4、5或8时
    float32_t atan = div / (1.0f + scaling_constant * div * div);
    // 如果是4或5，我们需要分别加上pi或-pi
    if (x < 0.0f) {
      return ∷std∷copysign(autoware∷common∷types∷PI,y) + atan;
    }
    return atan;
  }

  // 当在2、3、6或7时
  return ∷std∷copysign(autoware∷common∷types∷PI_2,y) - div / (div * div + scaling_constant);
}
    LIDAR_UTILS_PUBLIC std∷size_t index_after_last_safe_byte_index(const sensor_msgs∷msg∷PointCloud2 & msg) noexcept
描述：计算点云访问的最小安全长度。
/// \brief 计算点云访问的最小安全长度
/// \param[in] msg要验证的点云消息
/// \ return Byte索引的最后一个点的结束ok访问
std∷size_t index_after_last_safe_byte_index(const sensor_msgs∷msg∷PointCloud2 & msg) noexcept
{
  // 从各种真实来源计算预期的数据量
  // lint -e9123 NOLINT类型转换到更大的类型绝对没有什么错……
  const auto expected_total_data1 = static_cast<std∷size_t>(msg.point_step * (msg.width * msg.height));
  // lint -e9123 NOLINT类型转换到更大的类型绝对没有什么错……
  const auto expected_total_data2 = static_cast<std::size_t>(msg.row_step * msg.height);
  const auto actual_total_data = msg.data.size();
  // 取其中最小的
  const auto min_data = std∷min(std∷min(expected_total_data1,expected_total_data2),actual_total_data);
  // 删除任何不能与point_step正确对齐的数据
  const auto last_index = min_data - (min_data % msg.point_step);
  return last_index;
}
    LIDAR_UTILS_PUBLIC SafeCloudIndices sanitize_point_cloud(const sensor_msgs∷msg∷PointCloud2 & msg);
描述：计算给定点云的安全步幅和最大长度。
/// \brief 计算给定点云的安全步幅和最大长度
/// \return 每个点读取的数据大小ok和最后读取的索引ok，用于
/// 'for (auto idx = 0U;idx<ret.data_length;idx += msg.point_step) {memcpy(&pt,msg.data[idx],ret.point_step);}'
SafeCloudIndices sanitize_point_cloud(const sensor_msgs::msg::PointCloud2 & msg)
{
  /// XYZI or XYZ,or throw
  auto num_floats = 3U;
  if (has_intensity_and_throw_if_no_xyz(msg)) {
    num_floats = 4U;
  }
  return SafeCloudIndices{num_floats * sizeof(float32_t),index_after_last_safe_byte_index(msg)};//返回了SafeCloudIndices结构体
}
    LIDAR_UTILS_PUBLIC void init_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷string & frame_id,const std∷size_t size = static_cast<std∷size_t>(MAX_SCAN_POINTS));
描述：初始化点云的头部信息，包括x, y, z和强度。
/// \brief 初始化点云的头部信息，包括x,y,z和强度
/// \param[out] msg一个要初始化的点云消息
/// \param[in] frame_id点云的帧名(假设固定)
/// \param[in] size为底层数据数组预分配的点数
void init_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷string & frame_id,const std::size_t size)
{
  init_pcl_msg(
    msg,frame_id,size,4U,
    "x",1U,sensor_msgs∷msg∷PointField∷FLOAT32,
    "y",1U,sensor_msgs∷msg∷PointField∷FLOAT32,
    "z",1U,sensor_msgs∷msg∷PointField∷FLOAT32,
    "intensity",1U,sensor_msgs∷msg∷PointField∷FLOAT32);
}
    LIDAR_UTILS_PUBLIC void init_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷string & frame_id,const std∷size_t size,const uint32_t num_fields,Fields const & ...fields )
描述：初始化点云的报头信息，给定frame id，大小，帧数和字段的参数包。
/// 初始化点云的报头信息，给定帧id，大小，帧数和字段的参数包。
/// \tparam Fields包含字段类型的模板参数包。
/// \param msg点云消息。
/// \param frame_id点云的帧ID。
/// \param size初始化的点云的大小。
/// \param num_fields字段数。
/// \param fields定义字段的参数集合。每个字段必须按照严格的顺序包含如下参数:
/// ' field_name,count,data_type '。应该为每个字段提供这些参数
template<typename ...Fields>
LIDAR_UTILS_PUBLIC void init_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷string & frame_id,const std∷size_t size,const uint32_t num_fields,Fields const & ...fields )
{
  msg.height = 1U;
  msg.is_bigendian = false;
  msg.is_dense = false;
  msg.header.frame_id = frame_id;
  // 设置字段
  sensor_msgs∷PointCloud2Modifier modifier(msg);
  modifier.setPointCloud2Fields(num_fields,fields ...);
  // 分配内存，以便使用迭代器
  modifier.resize(size);
}

    LIDAR_UTILS_PUBLIC bool8_t add_point_to_cloud_raw( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZIF & pt,uint32_t point_cloud_idx);
描述：这个版本优先考虑速度和易于并行化，它假设：—PointXYZIF是一个POD对象，相当于存储在云中的一个点，这意味着相同的字段和相同的字节顺序。
/// \brief 通过memcpy在云中添加一个点而不是使用迭代器
/// 这个版本优先考虑速度和易于并行化
/// 它假设：- PointXYZIF是一个POD对象，相当于存储在云中的一个点，表示相同的字段和相同的字节序。
///                  -调用方负责在两个调用之间递增point_cloud_idx。这与add_point_to_cloud不同，add_point_cloud_idx由它自己递增。
bool8_t add_point_to_cloud_raw( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZIF & pt,uint32_t point_cloud_idx)
{
  bool8_t ret = false;

  // 由于编译器填充内存对齐边界，实际大小为20。
  // 这个检查是为了确保当我们执行16字节的插入时，不会跨越结构的边界。
  static_assert(
    sizeof(autoware::common∷types∷PointXYZIF) >= ((4U * sizeof(float32_t)) + sizeof(uint16_t)),
    "PointXYZF is not expected size: ");
  static_assert(
    std∷is_trivially_copyable<autoware∷common∷types∷PointXYZIF>∷value,
    "PointXYZF is not trivial,add_point_to_cloud instead of add_point_to_cloud_raw");

  const uint8_t * casted_point_ptr = reinterpret_cast<const uint8_t *>(&pt);
  uint8_t * const cloud_insertion_slot = &cloud.data[cloud.point_step * point_cloud_idx];
  std::size_t copy_size = std∷min(static_cast<std::size_t>(cloud.point_step),sizeof(pt));

  // 确保我们没有内存溢出
  uint8_t * const one_past_last_modified_address = &(cloud_insertion_slot[copy_size]);
  uint8_t * const vector_one_past_last_address = &(*cloud.data.end());

  if (one_past_last_modified_address <= vector_one_past_last_address) {
    // 添加点数据
    std::copy(casted_point_ptr,casted_point_ptr + copy_size,cloud_insertion_slot);
    ret = true;
  }
  return ret;
}
    bool8_t add_point_to_cloud
    LIDAR_UTILS_PUBLIC bool8_t add_point_to_cloud( PointCloudIts & cloud_its,const autoware∷common∷types∷PointXYZIF & pt,uint32_t & point_cloud_idx);
bool8_t add_point_to_cloud( PointCloudIts & cloud_its,const autoware∷common∷types∷PointXYZIF & pt,uint32_t & point_cloud_idx)
{
  bool8_t ret = false;

  auto & x_it = cloud_its.x_it();
  auto & y_it = cloud_its.y_it();
  auto & z_it = cloud_its.z_it();
  auto & intensity_it = cloud_its.intensity_it();
  // 由于编译器填充内存对齐边界，实际大小为20。
  // 这个检查是为了确保当我们执行16字节的插入时，不会跨越结构的边界。
  static_assert(
    sizeof(autoware∷common∷types∷PointXYZIF) >= ((4U * sizeof(float32_t)) + sizeof(uint16_t)),
    "PointXYZF is not expected size: ");

  if (x_it != x_it.end() && y_it != y_it.end() && z_it != z_it.end() && intensity_it != intensity_it.end())
  {
    // 添加点数据
    *x_it = pt.x;
    *y_it = pt.y;
    *z_it = pt.z;
    *intensity_it = pt.intensity;

    // 增加索引以跟踪点云的大小
    x_it += 1;
    y_it += 1;
    z_it += 1;
    intensity_it += 1;
    ++point_cloud_idx;

    ret = true;
  }
  return ret;
}
    LIDAR_UTILS_PUBLIC bool8_t add_point_to_cloud( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZIF & pt,uint32_t & point_cloud_idx);

bool8_t add_point_to_cloud( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZIF & pt,uint32_t & point_cloud_idx)
{
  bool8_t ret = false;

  sensor_msgs∷PointCloud2Iterator<float32_t> x_it(cloud,"x");
  sensor_msgs∷PointCloud2Iterator<float32_t> y_it(cloud,"y");
  sensor_msgs∷PointCloud2Iterator<float32_t> z_it(cloud,"z");
  sensor_msgs∷PointCloud2Iterator<float32_t> intensity_it(cloud,"intensity");

  x_it += point_cloud_idx;
  y_it += point_cloud_idx;
  z_it += point_cloud_idx;
  intensity_it += point_cloud_idx;

  // 由于编译器填充内存对齐边界，实际大小为20。
  // 这个检查是为了确保当我们执行16字节的插入时，不会跨越结构的边界。
  static_assert(
    sizeof(autoware∷common∷types∷PointXYZIF) >= ((4U * sizeof(float32_t)) + sizeof(uint16_t)),
    "PointXYZF is not expected size: ");

  if (x_it != x_it.end() && y_it != y_it.end() && z_it != z_it.end() && intensity_it != intensity_it.end())
  {
    // 添加点数据
    *x_it = pt.x;
    *y_it = pt.y;
    *z_it = pt.z;
    *intensity_it = pt.intensity;

    // 增加索引以跟踪点云的大小
    ++point_cloud_idx;
    ret = true;
  }
  return ret;
}
    LIDAR_UTILS_PUBLIC bool8_t add_point_to_cloud( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZF & pt,uint32_t & point_cloud_idx);
bool8_t add_point_to_cloud( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZF & pt,uint32_t & point_cloud_idx)
{
  bool8_t ret = false;

  sensor_msgs∷PointCloud2Iterator<float32_t> x_it(cloud,"x");
  sensor_msgs∷PointCloud2Iterator<float32_t> y_it(cloud,"y");
  sensor_msgs∷PointCloud2Iterator<float32_t> z_it(cloud,"z");

  x_it += point_cloud_idx;
  y_it += point_cloud_idx;
  z_it += point_cloud_idx;

  static_assert(
    sizeof(autoware∷common∷types∷PointXYZIF) >= ((3U * sizeof(float32_t)) + sizeof(uint16_t)),
    "PointXYZF is not expected size: ");

  if (x_it != x_it.end() && y_it != y_it.end() && z_it != z_it.end())
  {
    // 添加点数据
    *x_it = pt.x;
    *y_it = pt.y;
    *z_it = pt.z;

    // 增加索引以跟踪点云的大小
    ++point_cloud_idx;
    ret = true;
  }
  return ret;
}

    void reset_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷size_t size,uint32_t & point_cloud_idx )
void reset_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷size_t size,uint32_t & point_cloud_idx )
{
  sensor_msgs∷PointCloud2Modifier pc_modifier(msg);
  pc_modifier.clear();
  point_cloud_idx = 0;
  pc_modifier.resize(size);
}
    void reset_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷size_t new_size )
void reset_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷size_t new_size )
{
  sensor_msgs∷PointCloud2Modifier pc_modifier(msg);
  pc_modifier.resize(new_size);
}
    get_cluster
    LIDAR_UTILS_PUBLIC std∷pair<autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator,autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator get_cluster(autoware_auto_msgs∷msg∷PointClusters& clusters,const std::size_t cls_id);
描述：根据集群id从集群中获取集群。
/// \brief 根据集群id从集群中获取集群
/// \param[in] clusters集群对象
/// \param[in] cls_id目标集群的id
/// \return 指针对，它们是集群点中的目标集群的开始和结束
std∷pair<autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator,autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator get_cluster(autoware_auto_msgs∷msg∷PointClusters& clusters,const std∷size_t cls_id)
{
    if (cls_id >= clusters.cluster_boundary.size()) {
        return { clusters.points.end(),clusters.points.end() };
    }

    uint32_t cls_begin_offset;
    uint32_t cls_end_offset;  // 这个偏移量是目标集群的结束后元素
    if (cls_id == 0U) {
        cls_begin_offset = 0U;
        cls_end_offset = clusters.cluster_boundary[cls_id];
    }
    else {
        cls_begin_offset = clusters.cluster_boundary[cls_id - 1U];
        cls_end_offset = clusters.cluster_boundary[cls_id];
    }
    return { clusters.points.begin() + cls_begin_offset,clusters.points.begin() + cls_end_offset };
}

    LIDAR_UTILS_PUBLIC std∷pair<autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator,autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator> get_cluster(autoware_auto_msgs∷msg∷PointClusters& clusters,const std∷size_t cls_id);
描述：根据集群id从集群中获取集群。
std∷pair<autoware_auto_msgs∷msg∷PointClusters∷_points_type∷const_iterator,autoware_auto_msgs∷msg∷PointClusters∷_points_type∷const_iterator> get_cluster(const autoware_auto_msgs∷msg∷PointClusters& clusters,const std∷size_t cls_id)
{
    if (cls_id >= clusters.cluster_boundary.size()) {
        return { clusters.points.end(),clusters.points.end() };
    }

    uint32_t cls_begin_offset;
    uint32_t cls_end_offset;  // 这个偏移量是目标集群的结束后元素
    if (cls_id == 0U) {
        cls_begin_offset = 0U;
        cls_end_offset = clusters.cluster_boundary[cls_id];
    }
    else {
        cls_begin_offset = clusters.cluster_boundary[cls_id - 1U];
        cls_end_offset = clusters.cluster_boundary[cls_id];
    }
    return { clusters.points.begin() + cls_begin_offset,clusters.points.begin() + cls_end_offset };
}
    has_intensity_and_throw_if_no_xyz
描述：检查pointcloud msg的前三个字段是否为float32_t类型的x, y, z，否则抛出异常；检查pointcloud msg是否有强度字段作为第四个字段，否则返回false
    bool8_t has_intensity_and_throw_if_no_xyz(const PointCloud2∷SharedPtr & cloud)
bool8_t has_intensity_and_throw_if_no_xyz(const PointCloud2∷SharedPtr & cloud)
{
  return has_intensity_and_throw_if_no_xyz(*cloud);
}
    bool8_t has_intensity_and_throw_if_no_xyz(const PointCloud2 & cloud)
bool8_t has_intensity_and_throw_if_no_xyz(const PointCloud2 & cloud)
{
  bool8_t ret = true;
  // 验证“点”的步骤
  if (cloud.fields.size() < 3U) {
    throw std∷runtime_error("Invalid PointCloud msg");
  }

  const auto check_field = [](
    const sensor_msgs∷msg∷PointField & field,
    const char8_t * const name,
    const uint32_t offset,
    const decltype(sensor_msgs∷msg∷PointField∷datatype) datatype) -> bool8_t {
      bool8_t res = true;
      if ((name != field.name) || (offset != field.offset) || (datatype != field.datatype) || (1U != field.count))
      {
        res = false;
      }
      return res;
    };

  if (!check_field(cloud.fields[0U],"x",0U,sensor_msgs∷msg∷PointField∷FLOAT32)) {
    throw std∷runtime_error("PointCloud doesn't have correct x field");
  } else if (!check_field(cloud.fields[1U],"y",4U,sensor_msgs∷msg∷PointField∷FLOAT32)) {
    throw std∷runtime_error("PointCloud doesn't have correct y field");
  } else if (!check_field(cloud.fields[2U],"z",8U,sensor_msgs∷msg∷PointField∷FLOAT32)) {
    throw std∷runtime_error("PointCloud doesn't have correct z field");
  } else {
    // do nothing
  }
  if (cloud.fields.size() >= 4U) {
    if (!check_field(cloud.fields[3U],"intensity",12U,sensor_msgs∷msg∷PointField∷FLOAT32)) {
      if (!check_field(cloud.fields[3U],"intensity",16U,sensor_msgs∷msg∷PointField∷UINT8)) {
        ret = false;
      }
    }
  } else {
    ret = false;
  }
  return ret;
}
    LIDAR_UTILS_PUBLIC sensor_msgs∷msg∷PointCloud2∷SharedPtr create_custom_pcl(const std∷vector<std∷string> & field_names,const uint32_t cloud_size)
template<typename T>
LIDAR_UTILS_PUBLIC sensor_msgs∷msg∷PointCloud2∷SharedPtr create_custom_pcl(const std∷vector<std∷string> & field_names,const uint32_t cloud_size)
{
  using sensor_msgs∷msg∷PointCloud2;
  PointCloud2∷SharedPtr msg = std∷make_shared<PointCloud2>();
  const auto field_size = field_names.size();
  msg->height = 1U;
  msg->width = cloud_size;
  msg->fields.resize(field_size);
  for (uint32_t i = 0U; i < field_size; i++) {
    msg->fields[i].name = field_names[i];
  }
  msg->point_step = 0U;
  for (uint32_t idx = 0U; idx < field_size; ++idx) {
    msg->fields[idx].offset = static_cast<uint32_t>(idx * sizeof(T));
    msg->fields[idx].datatype = _create_custom_pcl_datatype<T>∷DATATYPE;
    msg->fields[idx].count = 1U;
    msg->point_step += static_cast<uint32_t>(sizeof(T));
  }
  const std∷size_t capacity = msg->point_step * cloud_size;
  msg->data.clear();
  msg->data.reserve(capacity);
  for (std∷size_t i = 0; i < capacity; ++i) {
    msg->data.emplace_back(0U);  // 初始化所有值为0
  }
  msg->row_step = msg->point_step * msg->width;
  msg->is_bigendian = false;
  msg->is_dense = false;
  msg->header.frame_id = "base_link";
  return msg;
}
变量
static const uint32_t MAX_SCAN_POINTS = 57872U;