本文整理汇总了Python中visualizer.Visualizer.trace_path方法的典型用法代码示例。如果您正苦于以下问题:Python Visualizer.trace_path方法的具体用法?Python Visualizer.trace_path怎么用?Python Visualizer.trace_path使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类visualizer.Visualizer
的用法示例。
在下文中一共展示了Visualizer.trace_path方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: Visualizer
# 需要导入模块: from visualizer import Visualizer [as 别名]
# 或者: from visualizer.Visualizer import trace_path [as 别名]
vizer = Visualizer()
vizer.draw_square(q_init)
vizer.draw_square(q_goal, color=vizer.RED)
obstcls = vizer.define_obstacles()
cd = CollisionDetector(obstcls)
obstcls_aabb = cd.compute_AABB()
# Plan path using q_init and obstacles
planner = PathPlanner(q_init, cd)
start = time.time()
# Call algorithm
rrt_tree = planner.build_rrtstar(K=2000, epsilon=5)
end = time.time()
print('Time taken: %f' % (end - start))
# Holonomic
q_nearest, dist, _ = planner.nearest_neighbour(q_goal, np.array(rrt_tree.vertexMap.keys()))
q_goal_vtx = planner.reach_goal(rrt_tree, q_goal)
vizer.plot_graph(rrt_tree, q_init)
vizer.trace_path(q_goal_vtx)
vizer.draw_square(q_init)
vizer.draw_square(q_goal, color=vizer.RED)
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()