继上一讲介绍了tare_planner算法的安装与仿真运行之后，本节主要介绍一下相关开源代码的具体内容。
通过源码我们开源看到10个ROS的package，这10个功能包的具体内容如图思维导图所示。
其中主要的程序功能由terrain_analysis和local_planner这两个功能包。我们运行上一教程的仿真代码，在rqt中开源看到如下的节点运行图。
下面进行相关代码的解析
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的话题进行了基于时间的接受，并且触发了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 < 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的坐标系下，然后发布了两个话题和一个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; // 系统初始化时间，根据第一帧点云信息的时间设定
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<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的，而在实际使用中需要统一到车辆坐标系下，而由于激光雷达的数据由于延时等原因与车辆位置无法完全对应，所以用上述方式将雷达坐标与车辆坐标进行对齐．而在实际使用中，雷达与车辆坐标系进行统一标定，因此这一部分并不会使用．
// 激光雷达数据栈
// 计算激光雷达数据点在车辆坐标系下的同一坐标的数量
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++;
}
}
}
}
}
上述程序将有用的信息存储到````中，然后将地面的点云信息发布出去.下面是发布的消息．
// 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);
其实这部分功能主要的思路是将点云转换到车辆坐标系下，根据需要建立地面体素网格，然后将点云信息填充网格，根据车辆的特性筛选出合适的点云，求解最小的点云的店面高度，基于此，将小于车辆高度，大于附近的最小值，将这部分点云信息放到地面的点云信息中，从而将地面的点云提取出来发布出去．
