标签：const msgs point utils Autoware API msg sensor cloud
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;
标签：const,msgs,point,utils,Autoware,API,msg,sensor,cloud 来源： https://blog.csdn.net/Siweimutong/article/details/120677865
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。