Update AStar.m

If the target is not a walkable node, the target will be substituted with the closest node to the target