完成路径规划的RoutePlanner类,实现从点集(datapoints)到路径命令(routecmd)的转换
使用算法
使用的算法参考自参考资料,我们的问题是一个旅行商问题,因此没有一个多项式时间内可以实现的最优算法
但是我们需要注意到,我们所面临的图是一个完全图,并且近似满足三角不等式.因此可以使用最小生成树的方法得到近似解,其结果不会大于最优解的两倍
实现
由于未来需要保留避障措施,但是避障之后的路径可以用一个新的路径代替,因此是通过修改路径获取函数实现(为未来预留接口)
这样做可能导致三角不等式不成立,但是影响应该不大
datapoints属性在修改时会重新计算route,因此不要频繁修改点集