Switched repo to ompl_visual_tools