utility.h文件
utility.h文件中的内容还是比较容易理解的，其引用了各文件中所需的第三方库文件，构建了一个ParamServer类，读取各种参数。
// PCL相关 #include <pcl/point_cloud.h> #include <pcl/point_types.h> #include <pcl/search/impl/search.hpp> #include <pcl/range_image/range_image.h> #include <pcl/kdtree/kdtree_flann.h> #include <pcl/common/common.h> #include <pcl/common/transforms.h> #include <pcl/registration/icp.h> #include <pcl/io/pcd_io.h> #include <pcl/filters/filter.h> #include <pcl/filters/voxel_grid.h> #include <pcl/filters/crop_box.h> #include <pcl_conversions/pcl_conversions.h> // TF相关 #include <tf/LinearMath/Quaternion.h> #include <tf/transform_listener.h> #include <tf/transform_datatypes.h> #include <tf/transform_broadcaster.h> // C++相关 #include <vector> #include <cmath> #include <algorithm> #include <queue> #include <deque> #include <iostream> #include <fstream> #include <ctime> #include <cfloat> #include <iterator> #include <sstream> #include <string> #include <limits> #include <iomanip> #include <array> #include <thread> #include <mutex> using namespace std; // 定义点类型 typedef pcl::PointXYZI PointType; // C++11 中，枚举 enum class SensorType { VELODYNE, OUSTER }; // 定义一个类，读取一系列参数 class ParamServer { public: ros::NodeHandle nh; std::string robot_id; //Topics string pointCloudTopic; string imuTopic; string odomTopic; string gpsTopic; //Frames string lidarFrame; string baselinkFrame; string odometryFrame; string mapFrame; // GPS Settings bool useImuHeadingInitialization; bool useGpsElevation; float gpsCovThreshold; float poseCovThreshold; // Save pcd bool savePCD; string savePCDDirectory; // Lidar Sensor Configuration SensorType sensor; int N_SCAN; int Horizon_SCAN; int downsampleRate; float lidarMinRange; float lidarMaxRange; // IMU float imuAccNoise; float imuGyrNoise; float imuAccBiasN; float imuGyrBiasN; float imuGravity; float imuRPYWeight; vector<double> extRotV; vector<double> extRPYV; vector<double> extTransV; Eigen::Matrix3d extRot; Eigen::Matrix3d extRPY; Eigen::Vector3d extTrans; Eigen::Quaterniond extQRPY; // LOAM 特征点相关 float edgeThreshold; float surfThreshold; int edgeFeatureMinValidNum; int surfFeatureMinValidNum; // voxel filter paprams float odometrySurfLeafSize; float mappingCornerLeafSize; float mappingSurfLeafSize ; float z_tollerance; float rotation_tollerance; // CPU Params int numberOfCores; // seconds, regulate mapping frequency（参数文件中默认设置为0.15 s） double mappingProcessInterval; // Surrounding map float surroundingkeyframeAddingDistThreshold; float surroundingkeyframeAddingAngleThreshold; float surroundingKeyframeDensity; float surroundingKeyframeSearchRadius; // Loop closure bool loopClosureEnableFlag; float loopClosureFrequency; int surroundingKeyframeSize; float historyKeyframeSearchRadius; float historyKeyframeSearchTimeDiff; int historyKeyframeSearchNum; float historyKeyframeFitnessScore; // global map visualization radius 全局地图可视化半径 float globalMapVisualizationSearchRadius; float globalMapVisualizationPoseDensity; float globalMapVisualizationLeafSize; // 读取一系列参数 ParamServer() { nh.param<std::string>("/robot_id", robot_id, "roboat"); nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw"); nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct"); nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu"); nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps"); nh.param<std::string>("lio_sam/lidarFrame", lidarFrame, "base_link"); nh.param<std::string>("lio_sam/baselinkFrame", baselinkFrame, "base_link"); nh.param<std::string>("lio_sam/odometryFrame", odometryFrame, "odom"); nh.param<std::string>("lio_sam/mapFrame", mapFrame, "map"); nh.param<bool>("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false); nh.param<bool>("lio_sam/useGpsElevation", useGpsElevation, false); nh.param<float>("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0); nh.param<float>("lio_sam/poseCovThreshold", poseCovThreshold, 25.0); nh.param<bool>("lio_sam/savePCD", savePCD, false); nh.param<std::string>("lio_sam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/"); std::string sensorStr; nh.param<std::string>("lio_sam/sensor", sensorStr, ""); if (sensorStr == "velodyne") { sensor = SensorType::VELODYNE; } else if (sensorStr == "ouster") { sensor = SensorType::OUSTER; } else { ROS_ERROR_STREAM( "Invalid sensor type (must be either 'velodyne' or 'ouster'): " << sensorStr); ros::shutdown(); } nh.param<int>("lio_sam/N_SCAN", N_SCAN, 16); nh.param<int>("lio_sam/Horizon_SCAN", Horizon_SCAN, 1800); nh.param<int>("lio_sam/downsampleRate", downsampleRate, 1); nh.param<float>("lio_sam/lidarMinRange", lidarMinRange, 1.0); nh.param<float>("lio_sam/lidarMaxRange", lidarMaxRange, 1000.0); nh.param<float>("lio_sam/imuAccNoise", imuAccNoise, 0.01); nh.param<float>("lio_sam/imuGyrNoise", imuGyrNoise, 0.001); nh.param<float>("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002); nh.param<float>("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003); nh.param<float>("lio_sam/imuGravity", imuGravity, 9.80511); nh.param<float>("lio_sam/imuRPYWeight", imuRPYWeight, 0.01); nh.param<vector<double>>("lio_sam/extrinsicRot", extRotV, vector<double>()); nh.param<vector<double>>("lio_sam/extrinsicRPY", extRPYV, vector<double>()); nh.param<vector<double>>("lio_sam/extrinsicTrans", extTransV, vector<double>()); extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3); extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3); extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1); extQRPY = Eigen::Quaterniond(extRPY); nh.param<float>("lio_sam/edgeThreshold", edgeThreshold, 0.1); nh.param<float>("lio_sam/surfThreshold", surfThreshold, 0.1); nh.param<int>("lio_sam/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10); nh.param<int>("lio_sam/surfFeatureMinValidNum", surfFeatureMinValidNum, 100); nh.param<float>("lio_sam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2); nh.param<float>("lio_sam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2); nh.param<float>("lio_sam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2); // FLT_MAX最大的float值 nh.param<float>("lio_sam/z_tollerance", z_tollerance, FLT_MAX); nh.param<float>("lio_sam/rotation_tollerance", rotation_tollerance, FLT_MAX); nh.param<int>("lio_sam/numberOfCores", numberOfCores, 2); nh.param<double>("lio_sam/mappingProcessInterval", mappingProcessInterval, 0.15); nh.param<float>("lio_sam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0); nh.param<float>("lio_sam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2); // 当前帧相关关键帧位置控制，使其不太密 nh.param<float>("lio_sam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0); nh.param<float>("lio_sam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0); nh.param<bool>("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false); nh.param<float>("lio_sam/loopClosureFrequency", loopClosureFrequency, 1.0); nh.param<int>("lio_sam/surroundingKeyframeSize", surroundingKeyframeSize, 50); nh.param<float>("lio_sam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0); nh.param<float>("lio_sam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0); nh.param<int>("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25); nh.param<float>("lio_sam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3); nh.param<float>("lio_sam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3); nh.param<float>("lio_sam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0); nh.param<float>("lio_sam/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0); usleep(100); }
此外，其还包括几个常用的函数，方便在其他文件中进行使用：
// 发布点云函数 sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame) { sensor_msgs::PointCloud2 tempCloud; pcl::toROSMsg(*thisCloud, tempCloud); tempCloud.header.stamp = thisStamp; tempCloud.header.frame_id = thisFrame; if (thisPub->getNumSubscribers() != 0) thisPub->publish(tempCloud); return tempCloud; } // C++中用于定义模板的固定格式 // 实现输入任何类型的msg消息，输出double类型的时间 template<typename T> double ROS_TIME(T msg) { return msg->header.stamp.toSec(); } /** * @brief 输入IMU消息，输出角速度 * * @tparam T * @param thisImuMsg * @param angular_x * @param angular_y * @param angular_z */ template<typename T> void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z) { *angular_x = thisImuMsg->angular_velocity.x; *angular_y = thisImuMsg->angular_velocity.y; *angular_z = thisImuMsg->angular_velocity.z; } // 输入IMU消息，输出任何类型的加速度 template<typename T> void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z) { *acc_x = thisImuMsg->linear_acceleration.x; *acc_y = thisImuMsg->linear_acceleration.y; *acc_z = thisImuMsg->linear_acceleration.z; } /** * @brief 获取IMU消息中的磁力计朝向数据，转换成RPY * IMU消息中的朝向，转换为欧拉角，rpy * @tparam T * @param thisImuMsg * @param rosRoll * @param rosPitch * @param rosYaw */ template<typename T> void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw) { double imuRoll, imuPitch, imuYaw; tf::Quaternion orientation; tf::quaternionMsgToTF(thisImuMsg->orientation, orientation); tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw); *rosRoll = imuRoll; *rosPitch = imuPitch; *rosYaw = imuYaw; } // 返回点到坐标系原点的距离 float pointDistance(PointType p) { return sqrt(p.x*p.x + p.y*p.y + p.z*p.z); } // 两个点之间的距离 float pointDistance(PointType p1, PointType p2) { return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z)); } #endif
cloud_info.msg文件
cloud_info.msg文件主要作用是构建了一个自定义的ROS Message类型。其中的各个变量包含不同的含义，在各函数中发挥不同的作用。
# Cloud Info Header header # 激光点的起始索引与终止索引（在所有激光点云中的序号） int32[] startRingIndex int32[] endRingIndex # 当前点对应的列号与距离 int32[] pointColInd # point column index in range image float32[] pointRange # point range # 是否可以使用IMU原始数据以及IMU里程计数据进行去除运动畸变 int64 imuAvailable int64 odomAvailable # Attitude for LOAM initialization # IMU磁力计测量值 float32 imuRollInit float32 imuPitchInit float32 imuYawInit # Initial guess from imu pre-integration # IMU里程计发布的位姿，对应每一帧激光点云的起始时刻 float32 initialGuessX float32 initialGuessY float32 initialGuessZ float32 initialGuessRoll float32 initialGuessPitch float32 initialGuessYaw # Point cloud messages sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed sensor_msgs/PointCloud2 cloud_corner # extracted corner feature sensor_msgs/PointCloud2 cloud_surface # extracted surface feature
