roslaunch issue when running "roslaunch state_machine rviz.launch"

[pengyuwang35]

Hi, thanks for the nice work and code. When I run "roslaunch state_machine rviz.launch", I met such an issue but the trajectory result seemed to be right. Thank you.

[zgyd97]

你好，我在尝试复现算法的时候发现有几个问题，想请教一下解决方法：
1.在每次deform之后，每个deforming node的pos改变了，kdtree里面对应节点的pos要怎么维护，这个可能对deform之后的rewire有影响，knearest node找的不对。要每次kdtree删了，重建一遍吗？
2.虽然是对整个树做gradient descent，我在试验的时候，如果采取了drrtd的做法，先找到可行解之后，再去deform tree好像确实会陷入到局部最优里面，效果感觉不如informed rrt*。理论上来说只要sample的点在空间区域内足够分散，是可以跳到别的拓扑结构上的，但前期节点都已经像主要枝干靠拢之后，想要换拓扑好像很难。具体实现的时候，有没有什么工程trick。