简介
原理参考我另外两篇文章:
效果
红色圆点是根据采样产生的新节点,箭头由子节点指向父节点,黄色箭头是 rrt 初始产生的方向,紫色箭头是 rrt star 重新布局修改后的方向。绿色路径是找到的初始路径,青色路径是 rrt star smart 优化后的路径。可以看到优化过程中,信标节点附近的采样会更多一点。
test 0
test 1
小结
- 有时候出现 visualization_msgs 没有显示的问题,只发现有时候发布太快,会有两个 mark 时间戳相同的情况,此时第一个 mark 会显示,第二个 mark 不会显示。但 mark 不是通过 id 来区分的吗?这个问题暂时没弄明白。
- new point 的产生需要根据系统而定,本文只是简单地用线性移动产生 new point。
- 信标采样频率不能太高,否则采样都集中在初始路径附近,降低了更短路径被搜索到的可能。