繼上一講介紹了tare_planner算法的安裝與仿真運行之后，本節(jié)主要介紹一下相關開源代碼的具體內容弓柱。
通過源碼我們開源看到10個ROS的package沟堡，這10個功能包的具體內容如圖思維導圖所示。

其中主要的程序功能由terrain_analysis和local_planner這兩個功能包矢空。我們運行上一教程的仿真代碼航罗，在rqt中開源看到如下的節(jié)點運行圖。

下面進行相關代碼的解析

1.1 sensor_scan_generation

sensor_scan_generation的主要的全局變量

pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr laserCLoudInSensorFrame(new pcl::PointCloud<pcl::PointXYZ>());

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<nav_msgs::Odometry> subOdometry;
  message_filters::Subscriber<sensor_msgs::PointCloud2> subLaserCloud;
  typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::PointCloud2> syncPolicy;
  typedef message_filters::Synchronizer<syncPolicy> Sync;
  boost::shared_ptr<Sync> sync_;
  subOdometry.subscribe(nh, "/state_estimation", 1);
  subLaserCloud.subscribe(nh, "/registered_scan", 1);
  sync_.reset(new Sync(syncPolicy(100), subOdometry, subLaserCloud));
  sync_->registerCallback(boost::bind(laserCloudAndOdometryHandler, _1, _2));

  ros::Publisher pubOdometry = nh.advertise<nav_msgs::Odometry> ("/state_estimation_at_scan", 5);
  pubOdometryPointer = &pubOdometry;

  tf::TransformBroadcaster tfBroadcaster;
  tfBroadcasterPointer = &tfBroadcaster;

  pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/sensor_scan", 2);

  ros::spin();
  return 0;

從上述主函數中可以看出，程序對/state_estimation和/registered_scan的話題進行了基于時間的接受酿箭，并且觸發(fā)了laserCloudAndOdometryHandler的回調函數复亏，從而發(fā)布出兩個話題/state_estimation_at_scan和/sensor_scan

對于/state_estimation和/registered_scan兩個話題，其在仿真中由vehicle_simulator發(fā)布缭嫡，對于實際車輛來說缔御，CMU設置了loam_interface功能包進行與SLAM算法的轉換，從而將兩個話題發(fā)布出來妇蛀。
下面我們看一下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 < laserCloudInNum; i++)
  {
    p1 = laserCloudIn->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的坐標系下眷茁，然后發(fā)布了兩個話題和一個tf的轉換關系。

２．terrain_analysis

terrain_analysis是一種進行地面點云分割的算法纵诞，其主要的程序流程如下圖上祈。

對其主要的參數部分進行解析，如果想了解其余部分，記得給樓主留言．

double scanVoxelSize = 0.05;        // 掃描體素大小:  5cm
double decayTime = 2.0;             // 時間閾值：     2.0s
double noDecayDis = 4.0;            // 車輛初始距離閾值：   4.0m
double clearingDis = 8.0;           // 清除距離：     8.0m
bool   clearingCloud = false;       // 清楚點云：     否-false登刺；是-true
bool   useSorting = true;           // 使用排序：     是
double quantileZ = 0.25;            // Z軸分辯數：    0.25m
bool   considerDrop = false;        // 考慮下降：     否
bool   limitGroundLift = false;     // 地面升高高度限制
double maxGroundLift = 0.15;        // 地面上升最大距離 0.15m
bool   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;            // 地面體素的寬度  10
const int terrainVoxelNum = terrainVoxelWidth * terrainVoxelWidth;    // 地面體素的大小  21×21 

// 平面體素參數
float planarVoxelSize = 0.2;                                          // 平面體素網格的尺寸大小 0.2m
const int planarVoxelWidth = 51;                                      // 點云存儲的格子大小
int   planarVoxelHalfWidth = (planarVoxelWidth - 1) / 2;              // 平面體素的寬度  25
const int planarVoxelNum = planarVoxelWidth * planarVoxelWidth;       // 平面體素的大小  51×51

pcl::PointCloud<pcl::PointXYZI>::Ptr
    laserCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
    laserCloudCrop(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
    laserCloudDwz(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
    terrainCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
    terrainCloudElev(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::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<float> planarPointElev[planarVoxelNum];        // 存儲了地面體素網格附近一個平面網格的所有點云的高程信息

double laserCloudTime = 0;                            // 雷達第一幀數據時間
bool   newlaserCloud  = false;                        // 雷達數據接受標志位

double systemInitTime = 0;                            // 系統(tǒng)初始化時間籽腕，根據第一幀點云信息的時間設定
bool   systemInited   = false;                        // 系統(tǒng)初始化標志位 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<pcl::PointXYZI> downSizeFilter;        // 三維體素化下采樣

下面看程序的主要函數

      // 車輛位置X-地面體素中心X < 負的一個體素網格大小
      while (vehicleX - terrainVoxelCenX < -terrainVoxelSize) {
        for (int indY = 0; indY < terrainVoxelWidth; indY++) {
          pcl::PointCloud<pcl::PointXYZI>::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<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
              terrainVoxelCloud[indY];
          for (int indX = 0; indX < terrainVoxelWidth - 1; indX++) {
            terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
                terrainVoxelCloud[terrainVoxelWidth * (indX + 1) + indY];
          }
          terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) +
                            indY] = terrainVoxelCloudPtr;
          terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) + indY]
              ->clear();
        }
        terrainVoxelShiftX++;
        terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX;
      }
      // 車輛位置Y-地面體素中心Y < 負的一個體素網格大小
      while (vehicleY - terrainVoxelCenY < -terrainVoxelSize) {
        for (int indX = 0; indX < terrainVoxelWidth; indX++) {
          pcl::PointCloud<pcl::PointXYZI>::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<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
              terrainVoxelCloud[terrainVoxelWidth * indX];
          for (int indY = 0; indY < terrainVoxelWidth - 1; indY++) {
            terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
                terrainVoxelCloud[terrainVoxelWidth * indX + (indY + 1)];
          }
          terrainVoxelCloud[terrainVoxelWidth * indX +
                            (terrainVoxelWidth - 1)] = terrainVoxelCloudPtr;
          terrainVoxelCloud[terrainVoxelWidth * indX + (terrainVoxelWidth - 1)]
              ->clear();
        }
        terrainVoxelShiftY++;
        terrainVoxelCenY = terrainVoxelSize * terrainVoxelShiftY;
      }

上述代碼主要是實現車輛與激光雷達坐標的對齊掉蔬，由于在仿真中激光雷達數據與車輛位置都是基于map的廊宪，而在實際使用中需要統(tǒng)一到車輛坐標系下，而由于激光雷達的數據由于延時等原因與車輛位置無法完全對應女轿，所以用上述方式將雷達坐標與車輛坐標進行對齊．而在實際使用中箭启，雷達與車輛坐標系進行統(tǒng)一標定，因此這一部分并不會使用．

      // 激光雷達數據棧
      // 計算激光雷達數據點在車輛坐標系下的同一坐標的數量
      pcl::PointXYZI point;
      int laserCloudCropSize = laserCloudCrop->points.size();  // 所有激光雷達的點
      for (int i = 0; i < laserCloudCropSize; i++) {
        point = laserCloudCrop->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)
          indY--;

        if (indX >= 0 && indX < terrainVoxelWidth && indY >= 0 &&
            indY < terrainVoxelWidth) {
          terrainVoxelCloud[terrainVoxelWidth * indX + indY]->push_back(point);
          terrainVoxelUpdateNum[terrainVoxelWidth * indX + indY]++;  // 計數器-記錄
        }
      }

上述程序主要是將激光雷達的點云數據填充到terrainVoxelCloud,轉換為三維的體素網格．

      for (int ind = 0; ind < terrainVoxelNum; ind++) {
        /**
         * @brief 處理激光雷達數據蛉迹，重置地面體素網格
         * 判斷條件1： 同一個位置的雷達點數 > 100 
         * 判斷條件2： id數據的時間差大于時間閾值
         * 判斷條件3： 清除激光雷達數據標志位為true
         */
        if (terrainVoxelUpdateNum[ind] >= voxelPointUpdateThre ||
            laserCloudTime - systemInitTime - terrainVoxelUpdateTime[ind] >=
                voxelTimeUpdateThre ||
            clearingCloud) {
          pcl::PointCloud<pcl::PointXYZI>::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 < laserCloudDwzSize; i++) {
            point = laserCloudDwz->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 < clearingDis && clearingCloud)) {
              terrainVoxelCloudPtr->push_back(point);       // 坐標系轉換后濾波之后的點云數據
            }
          }

          terrainVoxelUpdateNum[ind] = 0;  // 重置ind的激光雷達點的數量
          terrainVoxelUpdateTime[ind] = laserCloudTime - systemInitTime;
        }
      }

上述程序主要是對激光雷達點云信息進行過濾，從而將有用的點云信息存儲到terrainVoxelCloudPtr.

      if (useSorting) {
        for (int i = 0; i < planarVoxelNum; i++) {
          int planarPointElevSize = planarPointElev[i].size();
          if (planarPointElevSize > 0) {
            sort(planarPointElev[i].begin(), planarPointElev[i].end());

            int quantileID = int(quantileZ * planarPointElevSize);
            if (quantileID < 0)
              quantileID = 0;
            else 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 < planarVoxelNum; i++) {
          int planarPointElevSize = planarPointElev[i].size();
          if (planarPointElevSize > 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 < terrainCloudSize; i++) {
        point = terrainCloud->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)
            indY--;

          if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
              indY < planarVoxelWidth) {
            /**
              * 如果當前點云的高程比附近最小值大托启，小于車輛的高度
              * 并且計算高程時的點云數量也足夠多，就把當前點云放
              * 入到地形高程點云中攘宙。其中點云強度為一個相對的高度
              */
            if (planarVoxelDyObs[planarVoxelWidth * indX + indY] <
                    minDyObsPointNum ||
                !clearDyObs) {
              float disZ =
                  point.z - planarVoxelElev[planarVoxelWidth * indX + indY];
              if (considerDrop)
                disZ = fabs(disZ);
              int planarPointElevSize =
                  planarPointElev[planarVoxelWidth * indX + indY].size();
              if (disZ >= 0 && disZ < vehicleHeight &&
                  planarPointElevSize >= minBlockPointNum) {
                terrainCloudElev->push_back(point);
                terrainCloudElev->points[terrainCloudElevSize].intensity = disZ;

                terrainCloudElevSize++;
              }
            }
          }
        }
      }

上述程序將有用的信息存儲到````中屯耸，然后將地面的點云信息發(fā)布出去.下面是發(fā)布的消息．

      // 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);

其實這部分功能主要的思路是將點云轉換到車輛坐標系下，根據需要建立地面體素網格蹭劈，然后將點云信息填充網格疗绣，根據車輛的特性篩選出合適的點云，求解最小的點云的店面高度铺韧，基于此多矮，將小于車輛高度，大于附近的最小值哈打，將這部分點云信息放到地面的點云信息中塔逃，從而將地面的點云提取出來發(fā)布出去．