#// 包含所需的头文件 #include #include #include #include using namespace std; // 定义点云类型和点云指针类型 typedef pcl::PointXYZINormal PointType; typedef pcl::PointCloud PointCloudXYZI; // 定义枚举类型，表示不同型号的激光雷达和不同的特征类型等 enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3} enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; enum Surround{Prev, Next}; enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; // 宏定义，用于判断激光点坐标是否合法 #define IS_VALID(a) ((abs(a)>1e8) ? true : false) // 定义了一个结构体 orgtype（车辆感知系统中的障碍物类型），包含以下成员变量： // range：障碍物到车辆的距离 // dista：车辆到障碍物的距离 // angle：车辆与障碍物的夹角，包括左右两个角度 // intersect：障碍物与车辆轨迹的交点数目，默认为2 // edj：障碍物前后两个点的换道预测值（E_jump类型），默认为Nr_nor // ftype：障碍物类型，包括Normal、Pedestrian、Cyclist等，默认为Nor struct orgtype { double range; double dista; double angle[2]; double intersect; E_jump edj[2]; Feature ftype; // 构造函数，初始值为0或默认值 orgtype() { range = 0; edj[Prev] = Nr_nor; edj[Next] = Nr_nor; ftype = Nor; intersect = 2; } }; namespace velodyne_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; float time; uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace velodyne_ros POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, time, time) (uint16_t, ring, ring) ) //这段代码定义了一个名为velodyne_ros::Point的结构体，表示了Velodyne激光雷达采集到的点云数据，包括点云的三维坐标(x, y, z)、强度(intensity)、时间戳(time)、环号(ring)等信息。其中，PCL_ADD_POINT4D用于为结构体添加四元素坐标系，uint16_t表示无符号16位整数，EIGEN_ALIGN16和EIGEN_MAKE_ALIGNED_OPERATOR_NEW是为了保证内存对齐。最后，POINT_CLOUD_REGISTER_POINT_STRUCT宏用于在PCL(Point Cloud Library)中注册结构体，使其可以被PCL识别和使用。 namespace ouster_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; uint32_t t; uint16_t reflectivity; uint8_t ring; uint16_t ambient; uint32_t range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace ouster_ros // clang-format off POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, // 注册ouster_ros命名空间下的Point结构体 (float, x, x) // 定义x坐标域，类型为float，名称为x (float, y, y) // 定义y坐标域，类型为float，名称为y (float, z, z) // 定义z坐标域，类型为float，名称为z (float, intensity, intensity) // 定义强度域，类型为float，名称为intensity // 使用std::uint32_t避免与pcl命名空间下的uint32_t冲突 (std::uint32_t, t, t) // 定义时间戳域，类型为std::uint32_t，名称为t (std::uint16_t, reflectivity, reflectivity) // 定义反射率域，类型为std::uint16_t，名称为reflectivity (std::uint8_t, ring, ring) // 定义激光环号域，类型为std::uint8_t，名称为ring (std::uint16_t, ambient, ambient) // 定义环境光域，类型为std::uint16_t，名称为ambient (std::uint32_t, range, range) // 定义距离域，类型为std::uint32_t，名称为range ) class Preprocess { public: // EIGEN_MAKE_ALIGNED_OPERATOR_NEW Preprocess(); // 构造函数 ~Preprocess(); // 析构函数 void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 处理自定义消息类型的点云数据 void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 处理普通消息类型的点云数据 void set(bool feat_en, int lid_type, double bld, int pfilt_num); // 设置特征开启状态、激光雷达类型、盲区半径和点云过滤阈值 // sensor_msgs::PointCloud2::ConstPtr pointcloud; （未被使用，可以忽略） PointCloudXYZI pl_full, pl_corn, pl_surf; // 定义点云数据结构（全部点、角点、面点） PointCloudXYZI pl_buff[128]; // 定义点云缓存数组（最大支持128线激光雷达） vector typess[128]; // 定义激光雷达点云类型数组（最大支持128线激光雷达） int lidar_type, point_filter_num, N_SCANS; // 定义激光雷达类型、点云过滤阈值、扫描线数目 double blind; // 定义盲区半径 bool feature_enabled, given_offset_time; // 定义特征开启状态、偏移时间状态 ros::Publisher pub_full, pub_surf, pub_corn; // 定义发布器（发布全部点、角点、面点） // 声明私有成员函数 private: void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); // 处理Livox Avia类型雷达数据的回调函数 void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 处理Velodyne Ouster64型号激光雷达数据的回调函数 void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 处理Velodyne VLP-16型号激光雷达数据的回调函数 void give_feature(PointCloudXYZI &pl, vector &types); // 提取点云中的特征点 void pub_func(PointCloudXYZI &pl, const ros::Time &ct); // 发布处理后的点云的函数 int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); // 判断平面点的函数 bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); // 判断小平面点的函数 bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); // 判断边缘点的函数 // 声明私有成员变量 int group_size; // 平面点聚类时的最大聚类数量 double disA, disB, inf_bound; // 平面点判定时的距离阈值、角度阈值、无穷远距离 double limit_maxmid, limit_midmin, limit_maxmin; // 平面点判定时的角度阈值 double p2l_ratio; // 平面点聚类时的最小点数占比 double jump_up_limit, jump_down_limit; // 跳变点判定时的高度差阈值 double cos160; // 平面点判定时的角度cos值阈值 double edgea, edgeb; // 边缘点判定时的距离阈值与角度阈值 double smallp_intersect, smallp_ratio; // 小平面点判定时的交点距离阈值与占比阈值 double vx, vy, vz; // 相对运动的速度