繼上一講介紹了tare_planner算法的安裝與仿真運(yùn)行之后，本節(jié)主要介紹一下相關(guān)開(kāi)源代碼的具體內(nèi)容。
通過(guò)源碼我們開(kāi)源看到10個(gè)ROS的package，這10個(gè)功能包的具體內(nèi)容如圖思維導(dǎo)圖所示。
其中主要的程序功能由terrain_analysis和local_planner這兩個(gè)功能包。我們運(yùn)行上一教程的仿真代碼，在rqt中開(kāi)源看到如下的節(jié)點(diǎn)運(yùn)行圖。
下面進(jìn)行相關(guān)代碼的解析
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;
從上述全局變量中可以看到定義了：
- 兩個(gè)激光雷達(dá)的數(shù)據(jù)指針:
laserCloudIn和laserCLoudInSensorFrame - 機(jī)器人的xyz坐標(biāo)和roll、pitch和yaw的角度值。
-
newTransformToMap在程序中并未使用 -
pubOdometryPointer和tfBroadcasterPointer是ros::Publisher的指針 -
odometryIn是一個(gè)odom的消息類型 -
transformToMap是一個(gè)tf的消息類型
主函數(shù)內(nèi)容：
ros::init(argc, argv, "sensor_scan");
ros::NodeHandle nh;
ros::NodeHandle nhPrivate = ros::NodeHandle("~");
// ROS的消息時(shí)間同步
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;
從上述主函數(shù)中可以看出，程序?qū)?code>/state_estimation和/registered_scan的話題進(jìn)行了基于時(shí)間的接受，并且觸發(fā)了laserCloudAndOdometryHandler的回調(diào)函數(shù)，從而發(fā)布出兩個(gè)話題/state_estimation_at_scan和/sensor_scan
對(duì)于/state_estimation和/registered_scan兩個(gè)話題，其在仿真中由vehicle_simulator發(fā)布，對(duì)于實(shí)際車輛來(lái)說(shuō)，CMU設(shè)置了loam_interface功能包進(jìn)行與SLAM算法的轉(zhuǎn)換，從而將兩個(gè)話題發(fā)布出來(lái)。
下面我們看一下laserCloudAndOdometryHandler回調(diào)函數(shù)主要處理了什么內(nèi)容。
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);
｝
從上述代碼可以看出，主要是將激光雷達(dá)信息進(jìn)行了逆變換，轉(zhuǎn)化到map的坐標(biāo)系下，然后發(fā)布了兩個(gè)話題和一個(gè)tf的轉(zhuǎn)換關(guān)系。
２．terrain_analysis
terrain_analysis是一種進(jìn)行地面點(diǎn)云分割的算法，其主要的程序流程如下圖。
對(duì)其主要的參數(shù)部分進(jìn)行解析，如果想了解其余部分，記得給樓主留言．
double scanVoxelSize = 0.05; // 掃描體素大小: 5cm
double decayTime = 2.0; // 時(shí)間閾值： 2.0s
double noDecayDis = 4.0; // 車輛初始距離閾值： 4.0m
double clearingDis = 8.0; // 清除距離： 8.0m
bool clearingCloud = false; // 清楚點(diǎn)云： 否-false；是-true
bool useSorting = true; // 使用排序： 是
double quantileZ = 0.25; // Z軸分辯數(shù)： 0.25m
bool considerDrop = false; // 考慮下降： 否
bool limitGroundLift = false; // 地面升高高度限制
double maxGroundLift = 0.15; // 地面上升最大距離 0.15m
bool clearDyObs = false; // 清楚障礙標(biāo)志位
double minDyObsDis = 0.3; // 最小的障礙物距離閾值
double minDyObsAngle = 0; // 通過(guò)障礙物的最小角度
double minDyObsRelZ = -0.5; // 通過(guò)障礙物最小的Ｚ軸相對(duì)高度
double minDyObsVFOV = -16.0; // 左側(cè)最大轉(zhuǎn)向角
double maxDyObsVFOV = 16.0; // 右側(cè)最大轉(zhuǎn)向角
int minDyObsPointNum = 1; // 障礙物點(diǎn)的數(shù)量
bool noDataObstacle = false; // 無(wú)障礙物數(shù)據(jù)
int noDataBlockSkipNum = 0; // 無(wú)障礙物阻塞跳過(guò)的點(diǎn)數(shù)
int minBlockPointNum = 10; // 最小阻塞的點(diǎn)數(shù)
double vehicleHeight = 1.5; // 車輛的高度
int voxelPointUpdateThre = 100; // 同一個(gè)位置的雷達(dá)點(diǎn)數(shù)閾值
double voxelTimeUpdateThre = 2.0; // 同一個(gè)位置的雷達(dá)點(diǎn)時(shí)間閾值
double minRelZ = -1.5; // Z軸最小的相對(duì)距離
double maxRelZ = 0.2; // Z軸最大的相對(duì)距離
double disRatioZ = 0.2; // 點(diǎn)云處理的高度與距離的比例-與激光雷達(dá)性能相關(guān)
// 地面體素參數(shù)
float terrainVoxelSize = 1.0; // 地面體素網(wǎng)格的大小
int terrainVoxelShiftX = 0; // 地面體素網(wǎng)格翻轉(zhuǎn)時(shí)的X位置
int terrainVoxelShiftY = 0; // 地面體素網(wǎng)格翻轉(zhuǎn)時(shí)的Y位置
const int terrainVoxelWidth = 21; // 地面體素的寬度
int terrainVoxelHalfWidth = (terrainVoxelWidth - 1) / 2; // 地面體素的寬度 10
const int terrainVoxelNum = terrainVoxelWidth * terrainVoxelWidth; // 地面體素的大小 21×21
// 平面體素參數(shù)
float planarVoxelSize = 0.2; // 平面體素網(wǎng)格的尺寸大小 0.2m
const int planarVoxelWidth = 51; // 點(diǎn)云存儲(chǔ)的格子大小
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]; // 每個(gè)像素對(duì)應(yīng)存儲(chǔ)一個(gè)點(diǎn)云指針
int terrainVoxelUpdateNum[terrainVoxelNum] = {0}; // 記錄每一個(gè)體素網(wǎng)格中存入點(diǎn)云的數(shù)量
float terrainVoxelUpdateTime[terrainVoxelNum] = {0}; // 地形高程點(diǎn)云更新時(shí)間存儲(chǔ)數(shù)組
float planarVoxelElev[planarVoxelNum] = {0}; // 保存了id附近點(diǎn)云高程的最小值
int planarVoxelEdge[planarVoxelNum] = {0};
int planarVoxelDyObs[planarVoxelNum] = {0}; // 障礙物信息存儲(chǔ)數(shù)組
vector<float> planarPointElev[planarVoxelNum]; // 存儲(chǔ)了地面體素網(wǎng)格附近一個(gè)平面網(wǎng)格的所有點(diǎn)云的高程信息
double laserCloudTime = 0; // 雷達(dá)第一幀數(shù)據(jù)時(shí)間
bool newlaserCloud = false; // 雷達(dá)數(shù)據(jù)接受標(biāo)志位
double systemInitTime = 0; // 系統(tǒng)初始化時(shí)間，根據(jù)第一幀點(diǎn)云信息的時(shí)間設(shè)定
bool systemInited = false; // 系統(tǒng)初始化標(biāo)志位 false-未初始化；true-已經(jīng)初始化
int noDataInited = 0; // 車輛初始位置的標(biāo)志位 0-未賦值，將收到的第一個(gè)車輛位置賦值；1-表示已經(jīng)初始化；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; // 三維體素化下采樣
下面看程序的主要函數(shù)
// 車輛位置X-地面體素中心X < 負(fù)的一個(gè)體素網(wǎng)格大小
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 > 正的一個(gè)體素網(wǎng)格大小
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 < 負(fù)的一個(gè)體素網(wǎng)格大小
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 > 正的一個(gè)體素網(wǎng)格大小
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;
}
上述代碼主要是實(shí)現(xiàn)車輛與激光雷達(dá)坐標(biāo)的對(duì)齊，由于在仿真中激光雷達(dá)數(shù)據(jù)與車輛位置都是基于map的，而在實(shí)際使用中需要統(tǒng)一到車輛坐標(biāo)系下，而由于激光雷達(dá)的數(shù)據(jù)由于延時(shí)等原因與車輛位置無(wú)法完全對(duì)應(yīng)，所以用上述方式將雷達(dá)坐標(biāo)與車輛坐標(biāo)進(jìn)行對(duì)齊．而在實(shí)際使用中，雷達(dá)與車輛坐標(biāo)系進(jìn)行統(tǒng)一標(biāo)定，因此這一部分并不會(huì)使用．
// 激光雷達(dá)數(shù)據(jù)棧
// 計(jì)算激光雷達(dá)數(shù)據(jù)點(diǎn)在車輛坐標(biāo)系下的同一坐標(biāo)的數(shù)量
pcl::PointXYZI point;
int laserCloudCropSize = laserCloudCrop->points.size(); // 所有激光雷達(dá)的點(diǎn)
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;
// 計(jì)算偏移量
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]++; // 計(jì)數(shù)器-記錄
}
}
上述程序主要是將激光雷達(dá)的點(diǎn)云數(shù)據(jù)填充到terrainVoxelCloud,轉(zhuǎn)換為三維的體素網(wǎng)格．
for (int ind = 0; ind < terrainVoxelNum; ind++) {
/**
* @brief 處理激光雷達(dá)數(shù)據(jù)，重置地面體素網(wǎng)格
* 判斷條件1： 同一個(gè)位置的雷達(dá)點(diǎn)數(shù) > 100
* 判斷條件2： id數(shù)據(jù)的時(shí)間差大于時(shí)間閾值
* 判斷條件3： 清除激光雷達(dá)數(shù)據(jù)標(biāo)志位為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(); // 同一個(gè)(x,y)處點(diǎn)云的數(shù)據(jù)
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));
// 對(duì)于激光雷達(dá)數(shù)據(jù)的濾波
/*
* 在體素柵格中，需要被進(jìn)行地面分割的點(diǎn)云滿足以下要求，這些點(diǎn)云會(huì)被放入terrainCloud,用于地面分割
* 點(diǎn)云高度大于最小閾值
* 點(diǎn)云高度小于最大閾值
* 當(dāng)前點(diǎn)云的時(shí)間與要處理的點(diǎn)云時(shí)間差小于閾值 decayTime，或者距離小于 noDecayDis
* 此時(shí)不會(huì)清除距離外的點(diǎn)云，或者不在需要被清除的距離之內(nèi)
*/
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); // 坐標(biāo)系轉(zhuǎn)換后濾波之后的點(diǎn)云數(shù)據(jù)
}
}
terrainVoxelUpdateNum[ind] = 0; // 重置ind的激光雷達(dá)點(diǎn)的數(shù)量
terrainVoxelUpdateTime[ind] = laserCloudTime - systemInitTime;
}
}
上述程序主要是對(duì)激光雷達(dá)點(diǎn)云信息進(jìn)行過(guò)濾，從而將有用的點(diǎn)云信息存儲(chǔ)到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; // 最后一個(gè)點(diǎn)>第一個(gè)點(diǎn)+0.15m,則為第一個(gè)點(diǎn)+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保存了附近點(diǎn)云高程的最小值
}
}
}
}
上述程序主要是對(duì)點(diǎn)云信息的ｚ軸高度進(jìn)行篩選，將最小的ｚ軸高度存儲(chǔ)到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) {
/**
* 如果當(dāng)前點(diǎn)云的高程比附近最小值大，小于車輛的高度
* 并且計(jì)算高程時(shí)的點(diǎn)云數(shù)量也足夠多，就把當(dāng)前點(diǎn)云放
* 入到地形高程點(diǎn)云中。其中點(diǎn)云強(qiáng)度為一個(gè)相對(duì)的高度
*/
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++;
}
}
}
}
}
上述程序?qū)⒂杏玫男畔⒋鎯?chǔ)到````中，然后將地面的點(diǎn)云信息發(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);
其實(shí)這部分功能主要的思路是將點(diǎn)云轉(zhuǎn)換到車輛坐標(biāo)系下，根據(jù)需要建立地面體素網(wǎng)格，然后將點(diǎn)云信息填充網(wǎng)格，根據(jù)車輛的特性篩選出合適的點(diǎn)云，求解最小的點(diǎn)云的店面高度，基于此，將小于車輛高度，大于附近的最小值，將這部分點(diǎn)云信息放到地面的點(diǎn)云信息中，從而將地面的點(diǎn)云提取出來(lái)發(fā)布出去．
