Visualized the RRT graph and optimal path