informed rrt算法中的c_best的bug问题

[Ceg411]

在测试informed rrt*算法时，我想得到找到的路径长度，应该在路径更新的时候直接输出c_best的值就可以，但是通过测试发现c_best的值要比它发布的路径长度要大。在路径更新时，除了更新 c_best，我添加了根据path计算路径长度的代码，正常情况下，打印出来的
c_best和t_length应该是一样的，但结果却是前面几次打印出来的两个数值大小不一样，后面几次又都一样了。

1. 代码部分

2.结果输出

[realHarvey]

look at your loop for caculating t_length, the ending i will be path.size()-1, path[i+1] is out of range