Grid graph movement for units with different size

Look how I modified my code. Now it’s works without exception, but still I can’t setup offsets for 2x2 objects.

By the way, if I use AstarPath.StartPath and his callback, that doesn’t work and throw exceptions