开源了很久的局部规划器算法，实际使用效果也非常不错，不过一直没仔细去阅读，最近有时间对这部分代码仔细梳理一下。
在阅读localPlanner局部路径规划代码之前，首先需要了解其局部路径的生成过程。
详细解析可参考博客：Matlab用采样的离散点做前向模拟三次样条生成路径点
GitHub地址：localPlanner.cpp
Autonomous Exploration Development Environment and the Planning Algorithms 这篇论文好像还没发表，所以关于这部分只能通过代码慢慢啃了。
通过对该部分代码的阅读，在我的理解中，该路径规划器的主要思想，就是通过点云数据寻找障碍物，然后剔除被障碍物遮挡的路径线条。保留可通行的路径，在所有可通行的路径中，选择一条更适合的局部路径！
第一部分：对参数的理解
在 localPlanner 中定义了太多的参数，根据自己的理解介绍一下部分参数。有些参数理解错误或者不理解的地方，欢迎指正～
string pathFolder; --- 使用matlab生成路径集合的文件路径
double vehicleLength = 0.6; --- 车辆的长度，单位m
double vehicleWidth = 0.6; --- 车辆的宽度，单位m
double sensorOffsetX = 0; --- 传感器坐标系与车体中心的偏移量
double sensorOffsetY = 0; --- 传感器坐标系与车体中心的偏移量
bool twoWayDrive = true; --- 双向驱动,可以倒车
double laserVoxelSize = 0.05; --- 下采样体素栅格叶大小
double terrainVoxelSize = 0.2; --- 下采样体素栅格叶大小
bool useTerrainAnalysis = false; --- 是否使用地面分割后的点云信息
bool checkObstacle = true;
bool checkRotObstacle = false;
double adjacentRange = 3.5; --- 裁剪点云时的距离
double obstacleHeightThre = 0.2;
double groundHeightThre = 0.1;
double costHeightThre = 0.1;
double costScore = 0.02;
bool useCost = false;
const int laserCloudStackNum = 1; --- 缓存的激光点云帧数量
int laserCloudCount = 0; --- 当laserCloudStackNum = 1时,暂时没用到
int pointPerPathThre = 2;
double minRelZ = -0.5; --- 裁剪点云时的最小高度
double maxRelZ = 0.25; --- 裁剪点云时的最大高度
double maxSpeed = 1.0; --- 最大速度
double dirWeight = 0.02; --- 计算得分时转向角度的权重
double dirThre = 90.0; --- 最大转向角度
bool dirToVehicle = false;
double pathScale = 1.0; --- 路径尺度
double minPathScale = 0.75; --- 最小路径尺度
double pathScaleStep = 0.25; --- 路径尺度的调整步长
bool pathScaleBySpeed = true; --- 是否根据速度调整路径尺度
double minPathRange = 1.0; --- 最小路径距离
double pathRangeStep = 0.5; --- 路径范围的调整步长
bool pathRangeBySpeed = true; --- 是否根据速度调整路径的范围
bool pathCropByGoal = true;
bool autonomyMode = false;
double autonomySpeed = 1.0;
double goalClearRange = 0.5;
double goalX = 0; --- 局部路径目标点
double goalY = 0; --- 局部路径目标点
----- 以下参数是在生成路径采样信息时设置,另一篇博客中介绍 -----
const int pathNum = 343;
const int groupNum = 7;
float gridVoxelSize = 0.02;
float searchRadius = 0.45;
float gridVoxelOffsetX = 3.2;
float gridVoxelOffsetY = 4.5;
const int gridVoxelNumX = 161;
const int gridVoxelNumY = 451;
const int gridVoxelNum = gridVoxelNumX * gridVoxelNumY;
第二部分：数据的输入
首先关于 localPlanner 中的坐标系问题，由于是在仿真环境下运行，点云数据是在map下的，但是作为一个局部的规划器，在用于现实小车时，点云数据应该是在激光雷达的坐标系下，或者说是在base_link下的，后面应用时会对其进行更改。
从数据的输入开始，localPlanner 可接收原始点云数据，也可接收经过地面分割后的点云数据，可通过useTerrainAnalysis参数进行调整，后面在代码中会介绍两种输入数据类型的差异。
在terrainCloudHandler激光点云的回调函数中，前面已经说到，在该仿真环境下，给到该局部规划器的是一副完整的在map坐标系的点云地图，因此在这里对完整的点云图根据车辆位置进行裁剪，只保留在车辆附近 dis < adjacentRange 的点云数据。在实际使用过程中，这部分的代码是可以删去的。对地面分割后的点云数据的处理也是一样的。
void terrainCloudHandler(const sensor_msgs::PointCloud2ConstPtr& terrainCloud2)
{
...
for (int i = 0; i < terrainCloudSize; i++) {
point = terrainCloud->points[i];
float pointX = point.x;
float pointY = point.y;
float pointZ = point.z;
float dis = sqrt((pointX - vehicleX) * (pointX - vehicleX) + (pointY - vehicleY) * (pointY - vehicleY));
if (dis < adjacentRange && (point.intensity > obstacleHeightThre || useCost)) {
point.x = pointX;
point.y = pointY;
point.z = pointZ;
terrainCloudCrop->push_back(point);
}
}
...
}
最终所得到的原始点云数据或者是经过地面分割后的点云数据，都会被添加到plannerCloud中。
不过当选择使用经过地面分割后的点云数据时，即当 newTerrainCloud = true 时，便不会再使用原始的点云数据了。
if (newLaserCloud) {
newLaserCloud = false;
plannerCloud->clear();
*plannerCloud = *laserCloudStack[i];
}
if (newTerrainCloud) {
newTerrainCloud = false;
plannerCloud->clear();
*plannerCloud = *terrainCloudDwz;
}
因为在仿真环境中，提供的时在map下的点云数据，但是在局部路径选取和规划时，所用到的局部路径目标点都是在base_link下的，所以对所有的点云信息都旋转变换到车体坐标系下了。
这部分代码实际使用中，也是不需要进行旋转变换的，不过如果想对实时点云数据进行裁剪，也可以根据距离过滤一下，不过意义不大，因为这个系统的实时性已经很高了。
for (int i = 0; i < plannerCloudSize; i++) {
//平移
float pointX1 = plannerCloud->points[i].x - vehicleX;
float pointY1 = plannerCloud->points[i].y - vehicleY;
float pointZ1 = plannerCloud->points[i].z - vehicleZ;
//旋转
point.x = pointX1 * cosVehicleYaw + pointY1 * sinVehicleYaw;
point.y = -pointX1 * sinVehicleYaw + pointY1 * cosVehicleYaw;
point.z = pointZ1;
point.intensity = plannerCloud->points[i].intensity;
//过滤
float dis = sqrt(point.x * point.x + point.y * point.y);
if (dis < adjacentRange && ((point.z > minRelZ && point.z < maxRelZ) || useTerrainAnalysis)) {
plannerCloudCrop->push_back(point);
}
}
输入的数据除了实时的点云信息外，还有离线生成的路径信息，在Matlab用采样的离散点做前向模拟三次样条生成路径点这篇博客中，已经对路径信息进行了详细的分析。
主要输入的路径信息包括：
（1）第一次采样的路径点startPaths（2）路径点集合 paths
（3）每条路径的最后一个路径点 pathList（4）路径点和碰撞体素网格的索引关系 correspondences
这样以来，整个系统的初始化工作也就已经完成了，数据也已经完全准备就绪。
第三部分：路径集合的筛选
首先确定几个基本的参数，路径的范围pathRange，目标点relativeGoal坐标和角度，路径的尺度pathScale。这里还是默认目标点是在map坐标系下的，所以需要把它转换到车体坐标系下，也就是一个相对的目标点坐标。
float pathRange = adjacentRange;
if (pathRangeBySpeed) pathRange = adjacentRange * joySpeed;
if (pathRange < minPathRange) pathRange = minPathRange;
float relativeGoalDis = adjacentRange;
float relativeGoalX = ((goalX - vehicleX) * cosVehicleYaw + (goalY - vehicleY) * sinVehicleYaw);
float relativeGoalY = (-(goalX - vehicleX) * sinVehicleYaw + (goalY - vehicleY) * cosVehicleYaw);
relativeGoalDis = sqrt(relativeGoalX * relativeGoalX + relativeGoalY * relativeGoalY);
joyDir = atan2(relativeGoalY, relativeGoalX) * 180 / PI;
float defPathScale = pathScale;
if (pathScaleBySpeed) pathScale = defPathScale * joySpeed;
if (pathScale < minPathScale) pathScale = minPathScale;
接下来便进入到该局部规划器的主要循环，遍历所有的采样路径集合，选取出一条可通行且代价最小的路径作为当前时刻的局部路径。
未完待更…