how to search neighbor valid goal ,if goal footprint is occupied.
As a service robot, it is common that goal's footprint is occupied by peoples or other obstacles, how to try to search neighbor valid goals.