Tare_Planner的学习笔记(二）
Tare_Planner的学习笔记(二）
继上一讲介绍了tare_planner算法的安装与仿真运行之后，本节主要介绍一下相关开源代码的具体内容。
通过源码我们开源看到10个ROS的package，这10个功能包的具体内容如图思维导图所示。
其中主要的程序功能由terrain_analysis和local_planner这两个功能包。我们运行上一教程的仿真代码，在rqt中开源看到如下的节点运行图。
下面进行相关代码的解析
1. sensor_scan_generation
sensor_scan_generation的主要的全局变量
pcl::PointCloud::Ptr laserCloudIn(new pcl::PointCloud());pcl::PointCloud::Ptr laserCLoudInSensorFrame(new pcl::PointCloud());double robotX = 0;double robotY = 0;double robotZ = 0;double roll = 0;double pitch = 0;double yaw = 0;bool newTransformToMap = false;nav_msgs::Odometry odometryIn;ros::Publisher *pubOdometryPointer = NULL;tf::StampedTransform transformToMap;tf::TransformBroadcaster *tfBroadcasterPointer = NULL;ros::Publisher pubLaserCloud;从上述全局变量中可以看到定义了：
- 两个激光雷达的数据指针:
laserCloudIn和laserCLoudInSensorFrame - 机器人的xyz坐标和roll、pitch和yaw的角度值。
newTransformToMap在程序中并未使用pubOdometryPointer和tfBroadcasterPointer是ros::Publisher的指针odometryIn是一个odom的消息类型transformToMap是一个tf的消息类型
主函数内容：
ros::init(argc, argv, "sensor_scan"); ros::NodeHandle nh; ros::NodeHandle nhPrivate = ros::NodeHandle("~"); // ROS的消息时间同步 message_filters::Subscriber从上述主函数中可以看出，程序对/state_estimation和/registered_scan的话题进行了基于时间的接受，并且触发了laserCloudAndOdometryHandler的回调函数，从而发布出两个话题/state_estimation_at_scan和/sensor_scan
对于/state_estimation和/registered_scan两个话题，其在仿真中由vehicle_simulator发布，对于实际车辆来说，CMU设置了loam_interface功能包进行与SLAM算法的转换，从而将两个话题发布出来。
下面我们看一下laserCloudAndOdometryHandler回调函数主要处理了什么内容。
void laserCloudAndOdometryHandler(const nav_msgs::Odometry::ConstPtr& odometry, const sensor_msgs::PointCloud2ConstPtr& laserCloud2){ laserCloudIn->clear(); laserCLoudInSensorFrame->clear(); pcl::fromROSMsg(*laserCloud2, *laserCloudIn); odometryIn = *odometry; transformToMap.setOrigin( tf::Vector3(odometryIn.pose.pose.position.x, odometryIn.pose.pose.position.y, odometryIn.pose.pose.position.z)); transformToMap.setRotation(tf::Quaternion(odometryIn.pose.pose.orientation.x, odometryIn.pose.pose.orientation.y, odometryIn.pose.pose.orientation.z, odometryIn.pose.pose.orientation.w)); int laserCloudInNum = laserCloudIn->points.size(); pcl::PointXYZ p1; tf::Vector3 vec; for (int i = 0; i points[i]; vec.setX(p1.x); vec.setY(p1.y); vec.setZ(p1.z); // 获得逆变换 vec = transformToMap.inverse() * vec; p1.x = vec.x(); p1.y = vec.y(); p1.z = vec.z(); laserCLoudInSensorFrame->points.push_back(p1); } odometryIn.header.stamp = laserCloud2->header.stamp; odometryIn.header.frame_id = "/map"; odometryIn.child_frame_id = "/sensor_at_scan"; pubOdometryPointer->publish(odometryIn); transformToMap.stamp_ = laserCloud2->header.stamp; transformToMap.frame_id_ = "/map"; transformToMap.child_frame_id_ = "/sensor_at_scan"; tfBroadcasterPointer->sendTransform(transformToMap); sensor_msgs::PointCloud2 scan_data; pcl::toROSMsg(*laserCLoudInSensorFrame, scan_data); scan_data.header.stamp = laserCloud2->header.stamp; scan_data.header.frame_id = "/sensor_at_scan"; pubLaserCloud.publish(scan_data); ｝从上述代码可以看出，主要是将激光雷达信息进行了逆变换，转化到map的坐标系下，然后发布了两个话题和一个tf的转换关系。
2.terrain_analysis
terrain_analysis是一种进行地面点云分割的算法，其主要的程序流程如下图。
对其主要的参数部分进行解析，如果想了解其余部分，记得给楼主留言.
double scanVoxelSize = 0.05; // 扫描体素大小: 5cmdouble decayTime = 2.0; // 时间阈值： 2.0sdouble noDecayDis = 4.0; // 车辆初始距离阈值： 4.0mdouble clearingDis = 8.0; // 清除距离： 8.0mbool clearingCloud = false;// 清楚点云： 否-false；是-truebool useSorting = true; // 使用排序： 是double quantileZ = 0.25; // Z轴分辩数： 0.25mbool considerDrop = false; // 考虑下降： 否bool limitGroundLift = false; // 地面升高高度限制double maxGroundLift = 0.15; // 地面上升最大距离 0.15mbool clearDyObs = false; // 清楚障碍标志位 double minDyObsDis = 0.3; // 最小的障碍物距离阈值double minDyObsAngle = 0; // 通过障碍物的最小角度double minDyObsRelZ = -0.5; // 通过障碍物最小的Ｚ轴相对高度double minDyObsVFOV = -16.0; // 左侧最大转向角double maxDyObsVFOV = 16.0; // 右侧最大转向角int minDyObsPointNum = 1; // 障碍物点的数量bool noDataObstacle = false; // 无障碍物数据int noDataBlockSkipNum = 0; // 无障碍物阻塞跳过的点数int minBlockPointNum = 10;// 最小阻塞的点数double vehicleHeight = 1.5; // 车辆的高度int voxelPointUpdateThre = 100; // 同一个位置的雷达点数阈值double voxelTimeUpdateThre = 2.0; // 同一个位置的雷达点时间阈值double minRelZ = -1.5;// Z轴最小的相对距离double maxRelZ = 0.2; // Z轴最大的相对距离double disRatioZ = 0.2; // 点云处理的高度与距离的比例-与激光雷达性能相关// 地面体素参数float terrainVoxelSize = 1.0;// 地面体素网格的大小int terrainVoxelShiftX = 0;// 地面体素网格翻转时的X位置int terrainVoxelShiftY = 0;// 地面体素网格翻转时的Y位置const int terrainVoxelWidth = 21; // 地面体素的宽度int terrainVoxelHalfWidth = (terrainVoxelWidth - 1) / 2; // 地面体素的宽度 10const int terrainVoxelNum = terrainVoxelWidth * terrainVoxelWidth; // 地面体素的大小 21×21 // 平面体素参数float planarVoxelSize = 0.2;// 平面体素网格的尺寸大小 0.2mconst int planarVoxelWidth = 51; // 点云存储的格子大小int planarVoxelHalfWidth = (planarVoxelWidth - 1) / 2;// 平面体素的宽度 25const int planarVoxelNum = planarVoxelWidth * planarVoxelWidth;// 平面体素的大小 51×51pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud());pcl::PointCloud::Ptr laserCloudCrop(new pcl::PointCloud());pcl::PointCloud::Ptr laserCloudDwz(new pcl::PointCloud());pcl::PointCloud::Ptr terrainCloud(new pcl::PointCloud());pcl::PointCloud::Ptr terrainCloudElev(new pcl::PointCloud());pcl::PointCloud::Ptr terrainVoxelCloud[terrainVoxelNum]; // 每个像素对应存储一个点云指针int terrainVoxelUpdateNum[terrainVoxelNum] = {0}; // 记录每一个体素网格中存入点云的数量float terrainVoxelUpdateTime[terrainVoxelNum] = {0}; // 地形高程点云更新时间存储数组float planarVoxelElev[planarVoxelNum] = {0}; // 保存了id附近点云高程的最小值int planarVoxelEdge[planarVoxelNum] = {0}; int planarVoxelDyObs[planarVoxelNum] = {0}; // 障碍物信息存储数组vector planarPointElev[planarVoxelNum]; // 存储了地面体素网格附近一个平面网格的所有点云的高程信息double laserCloudTime = 0;// 雷达第一帧数据时间bool newlaserCloud = false; // 雷达数据接受标志位double systemInitTime = 0;// 系统初始化时间，根据第一帧点云信息的时间设定bool systemInited = false; // 系统初始化标志位 false-未初始化；true-已经初始化int noDataInited = 0;// 车辆初始位置的标志位 0-未赋值，将收到的第一个车辆位置赋值；1-表示已经初始化；2-车辆初始距离误差大于初始阈值float vehicleRoll = 0, vehiclePitch = 0, vehicleYaw = 0;float vehicleX = 0, vehicleY = 0, vehicleZ = 0;float vehicleXRec = 0, vehicleYRec = 0;float sinVehicleRoll = 0, cosVehicleRoll = 0;float sinVehiclePitch = 0, cosVehiclePitch = 0;float sinVehicleYaw = 0, cosVehicleYaw = 0;pcl::VoxelGrid downSizeFilter; // 三维体素化下采样下面看程序的主要函数
// 车辆位置X-地面体素中心X < 负的一个体素网格大小 while (vehicleX - terrainVoxelCenX < -terrainVoxelSize) { for (int indY = 0; indY < terrainVoxelWidth; indY++) { pcl::PointCloud::Ptr terrainVoxelCloudPtr =terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) + indY]; for (int indX = terrainVoxelWidth - 1; indX >= 1; indX--) { terrainVoxelCloud[terrainVoxelWidth * indX + indY] = terrainVoxelCloud[terrainVoxelWidth * (indX - 1) + indY]; } terrainVoxelCloud[indY] = terrainVoxelCloudPtr; terrainVoxelCloud[indY]->clear(); } terrainVoxelShiftX--; terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX; } // 车辆位置X-地面体素中心X > 正的一个体素网格大小 while (vehicleX - terrainVoxelCenX > terrainVoxelSize) { for (int indY = 0; indY < terrainVoxelWidth; indY++) { pcl::PointCloud::Ptr terrainVoxelCloudPtr =terrainVoxelCloud[indY]; for (int indX = 0; indX clear(); } terrainVoxelShiftX++; terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX; } // 车辆位置Y-地面体素中心Y < 负的一个体素网格大小 while (vehicleY - terrainVoxelCenY < -terrainVoxelSize) { for (int indX = 0; indX < terrainVoxelWidth; indX++) { pcl::PointCloud::Ptr terrainVoxelCloudPtr =terrainVoxelCloud[terrainVoxelWidth * indX + (terrainVoxelWidth - 1)]; for (int indY = terrainVoxelWidth - 1; indY >= 1; indY--) { terrainVoxelCloud[terrainVoxelWidth * indX + indY] = terrainVoxelCloud[terrainVoxelWidth * indX + (indY - 1)]; } terrainVoxelCloud[terrainVoxelWidth * indX] = terrainVoxelCloudPtr; terrainVoxelCloud[terrainVoxelWidth * indX]->clear(); } terrainVoxelShiftY--; terrainVoxelCenY = terrainVoxelSize * terrainVoxelShiftY; } // 车辆位置Y-地面体素中心Y > 正的一个体素网格大小 while (vehicleY - terrainVoxelCenY > terrainVoxelSize) { for (int indX = 0; indX < terrainVoxelWidth; indX++) { pcl::PointCloud::Ptr terrainVoxelCloudPtr =terrainVoxelCloud[terrainVoxelWidth * indX]; for (int indY = 0; indY clear(); } terrainVoxelShiftY++; terrainVoxelCenY = terrainVoxelSize * terrainVoxelShiftY; }上述代码主要是实现车辆与激光雷达坐标的对齐，由于在仿真中激光雷达数据与车辆位置都是基于map的，而在实际使用中需要统一到车辆坐标系下，而由于激光雷达的数据由于延时等原因与车辆位置无法完全对应，所以用上述方式将雷达坐标与车辆坐标进行对齐．而在实际使用中，雷达与车辆坐标系进行统一标定，因此这一部分并不会使用．
// 激光雷达数据栈 // 计算激光雷达数据点在车辆坐标系下的同一坐标的数量 pcl::PointXYZI point; int laserCloudCropSize = laserCloudCrop->points.size(); // 所有激光雷达的点 for (int i = 0; i points[i]; int indX = int((point.x - vehicleX + terrainVoxelSize / 2) / terrainVoxelSize) + terrainVoxelHalfWidth; int indY = int((point.y - vehicleY + terrainVoxelSize / 2) / terrainVoxelSize) + terrainVoxelHalfWidth; // 计算偏移量 if (point.x - vehicleX + terrainVoxelSize / 2 < 0) indX--; if (point.y - vehicleY + terrainVoxelSize / 2 = 0 && indX = 0 && indY push_back(point); terrainVoxelUpdateNum[terrainVoxelWidth * indX + indY]++; // 计数器-记录 } }上述程序主要是将激光雷达的点云数据填充到terrainVoxelCloud,转换为三维的体素网格．
for (int ind = 0; ind 100 * 判断条件2： id数据的时间差大于时间阈值 * 判断条件3： 清除激光雷达数据标志位为true */ if (terrainVoxelUpdateNum[ind] >= voxelPointUpdateThre || laserCloudTime - systemInitTime - terrainVoxelUpdateTime[ind] >= voxelTimeUpdateThre || clearingCloud) { pcl::PointCloud::Ptr terrainVoxelCloudPtr =terrainVoxelCloud[ind]; laserCloudDwz->clear(); downSizeFilter.setInputCloud(terrainVoxelCloudPtr); downSizeFilter.filter(*laserCloudDwz); terrainVoxelCloudPtr->clear(); int laserCloudDwzSize = laserCloudDwz->points.size(); // 同一个(x,y)处点云的数据 for (int i = 0; i points[i]; float dis = sqrt((point.x - vehicleX) * (point.x - vehicleX) + (point.y - vehicleY) * (point.y - vehicleY)); // 对于激光雷达数据的滤波 /* * 在体素栅格中，需要被进行地面分割的点云满足以下要求，这些点云会被放入terrainCloud,用于地面分割 * 点云高度大于最小阈值 * 点云高度小于最大阈值 * 当前点云的时间与要处理的点云时间差小于阈值 decayTime，或者距离小于 noDecayDis * 此时不会清除距离外的点云，或者不在需要被清除的距离之内 */ if (point.z - vehicleZ > minRelZ - disRatioZ * dis && point.z - vehicleZ < maxRelZ + disRatioZ * dis && (laserCloudTime - systemInitTime - point.intensity <decayTime || dis < noDecayDis) && !(dis push_back(point);// 坐标系转换后滤波之后的点云数据 } } terrainVoxelUpdateNum[ind] = 0; // 重置ind的激光雷达点的数量 terrainVoxelUpdateTime[ind] = laserCloudTime - systemInitTime; } }上述程序主要是对激光雷达点云信息进行过滤，从而将有用的点云信息存储到terrainVoxelCloudPtr.
if (useSorting) { for (int i = 0; i 0) { sort(planarPointElev[i].begin(), planarPointElev[i].end()); int quantileID = int(quantileZ * planarPointElevSize); if (quantileID = planarPointElevSize)quantileID = planarPointElevSize - 1; if (planarPointElev[i][quantileID] > planarPointElev[i][0] + maxGroundLift && limitGroundLift) {planarVoxelElev[i] = planarPointElev[i][0] + maxGroundLift; // 最后一个点>第一个点+0.15m,则为第一个点+0.15m } else {planarVoxelElev[i] = planarPointElev[i][quantileID]; } } } }else { for (int i = 0; i 0) { float minZ = 1000.0; int minID = -1; for (int j = 0; j < planarPointElevSize; j++) {if (planarPointElev[i][j] < minZ) { minZ = planarPointElev[i][j]; minID = j;} } if (minID != -1) {planarVoxelElev[i] = planarPointElev[i][minID]; // planarVoxelElev保存了附近点云高程的最小值 } } } }上述程序主要是对点云信息的ｚ轴高度进行筛选，将最小的ｚ轴高度存储到tplanarVoxelElev.
terrainCloudElev->clear(); int terrainCloudElevSize = 0; for (int i = 0; i points[i]; if (point.z - vehicleZ > minRelZ && point.z - vehicleZ < maxRelZ) { int indX = int((point.x - vehicleX + planarVoxelSize / 2) / planarVoxelSize) +planarVoxelHalfWidth; int indY = int((point.y - vehicleY + planarVoxelSize / 2) / planarVoxelSize) +planarVoxelHalfWidth; if (point.x - vehicleX + planarVoxelSize / 2 < 0) indX--; if (point.y - vehicleY + planarVoxelSize / 2 = 0 && indX = 0 &&indY < planarVoxelWidth) { /*** 如果当前点云的高程比附近最小值大，小于车辆的高度* 并且计算高程时的点云数量也足够多，就把当前点云放* 入到地形高程点云中。其中点云强度为一个相对的高度*/ if (planarVoxelDyObs[planarVoxelWidth * indX + indY] = 0 && disZ = minBlockPointNum) { terrainCloudElev->push_back(point); terrainCloudElev->points[terrainCloudElevSize].intensity = disZ; terrainCloudElevSize++;} } } } }上述程序将有用的信息存储到````中，然后将地面的点云信息发布出去.下面是发布的消息．
// publish points with elevation sensor_msgs::PointCloud2 terrainCloud2; pcl::toROSMsg(*terrainCloudElev, terrainCloud2); terrainCloud2.header.stamp = ros::Time().fromSec(laserCloudTime); terrainCloud2.header.frame_id = "/map"; pubLaserCloud.publish(terrainCloud2);其实这部分功能主要的思路是将点云转换到车辆坐标系下，根据需要建立地面体素网格，然后将点云信息填充网格，根据车辆的特性筛选出合适的点云，求解最小的点云的店面高度，基于此，将小于车辆高度，大于附近的最小值，将这部分点云信息放到地面的点云信息中，从而将地面的点云提取出来发布出去．