Use 3D point to create a goal for move_base