该路径规划器的主要思想，就是通过点云数据寻找障碍物，然后剔除被障碍物遮挡的路径线条。保留可通行的路径，在所有可通行的路径中，根据评分公式，选择一条最可能接近目标点的路径。

问题

导航过程中，路径不断切换，追踪代码发现selectedGroupID出现大量-1，在-1和正数之间不断切换。 -1其实就是没找到路径。 再向上查，发现之所以成为-1，是maxScore和clearPathPerGroupScore[i]比较时，后者一直为0， maxScore不更新。 继续查，clearPathPerGroupScore的赋值是在

float score = (1 - sqrt(sqrt(dirWeight * dirDiff))) * rotDirW * 
               rotDirW * rotDirW * rotDirW * penaltyScore;

dirWeight是自己设置的参数，可见必须大于0

dirDiff: 114.517456, rotDirW: 9.000000, penaltyScore: 1.000000
pathPenaltyList[i]: 0.000000, costHeightThre: 0.100000