bfs implemented, pose initialized