From b25f621780d9d03b88d3b8ba0d31b15b9c1cae90 Mon Sep 17 00:00:00 2001 From: 6467a2983a260c3da56c82e1 <6467a2983a260c3da56c82e1@devide> Date: Sat, 20 May 2023 07:12:56 +0000 Subject: [PATCH] Auto commit --- ...MU_Processing\346\263\250\351\207\212.cpp" | 0 .../preprocess\346\263\250\351\207\212.cpp" | 0 "maq/preprocess\346\263\250\351\207\212.h" | 141 ++++++++++++++++++ "\345\244\264\346\226\207\344\273\266.cpp" | 137 ----------------- 4 files changed, 141 insertions(+), 137 deletions(-) rename imu.cpp => "maq/IMU_Processing\346\263\250\351\207\212.cpp" (100%) rename imu222.cpp => "maq/preprocess\346\263\250\351\207\212.cpp" (100%) create mode 100644 "maq/preprocess\346\263\250\351\207\212.h" delete mode 100644 "\345\244\264\346\226\207\344\273\266.cpp" diff --git a/imu.cpp "b/maq/IMU_Processing\346\263\250\351\207\212.cpp" similarity index 100% rename from imu.cpp rename to "maq/IMU_Processing\346\263\250\351\207\212.cpp" diff --git a/imu222.cpp "b/maq/preprocess\346\263\250\351\207\212.cpp" similarity index 100% rename from imu222.cpp rename to "maq/preprocess\346\263\250\351\207\212.cpp" diff --git "a/maq/preprocess\346\263\250\351\207\212.h" "b/maq/preprocess\346\263\250\351\207\212.h" new file mode 100644 index 0000000..0d8f95b --- /dev/null +++ "b/maq/preprocess\346\263\250\351\207\212.h" @@ -0,0 +1,141 @@ +#// 包å«æ‰€éœ€çš„头文件 +#include <ros/ros.h> +#include <pcl_conversions/pcl_conversions.h> +#include <sensor_msgs/PointCloud2.h> +#include <livox_ros_driver/CustomMsg.h> + +using namespace std; + +// 定义点云类型和点云指针类型 +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointCloud<PointType> 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<orgtype> 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<orgtype> &types); // æå–点云ä¸çš„特å¾ç‚¹ + void pub_func(PointCloudXYZI &pl, const ros::Time &ct); // å‘布处ç†åŽçš„点云的函数 + int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); // 判æ–å¹³é¢ç‚¹çš„函数 + bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); // 判æ–å°å¹³é¢ç‚¹çš„函数 + bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &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; // 相对è¿åŠ¨çš„速度 diff --git "a/\345\244\264\346\226\207\344\273\266.cpp" "b/\345\244\264\346\226\207\344\273\266.cpp" deleted file mode 100644 index 7ec2b50..0000000 --- "a/\345\244\264\346\226\207\344\273\266.cpp" +++ /dev/null @@ -1,137 +0,0 @@ -#// 包å«æ‰€éœ€çš„头文件 -#include <ros/ros.h> -#include <pcl_conversions/pcl_conversions.h> -#include <sensor_msgs/PointCloud2.h> -#include <livox_ros_driver/CustomMsg.h> - -using namespace std; - -// 定义点云类型和点云指针类型 -typedef pcl::PointXYZINormal PointType; -typedef pcl::PointCloud<PointType> 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) -) - -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, - (float, x, x) - (float, y, y) - (float, z, z) - (float, intensity, intensity) - // use std::uint32_t to avoid conflicting with pcl::uint32_t - (std::uint32_t, t, t) - (std::uint16_t, reflectivity, reflectivity) - (std::uint8_t, ring, ring) - (std::uint16_t, ambient, ambient) - (std::uint32_t, range, 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]; //maximum 128 line lidar - vector<orgtype> typess[128]; //maximum 128 line lidar - 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); - void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); - void give_feature(PointCloudXYZI &pl, vector<orgtype> &types); - void pub_func(PointCloudXYZI &pl, const ros::Time &ct); - int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); - bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); - bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &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; - double edgea, edgeb; - double smallp_intersect, smallp_ratio; - double vx, vy, vz; -}; \ No newline at end of file -- GitLab