Added visualization mode for RRT