Add a check for FSA memory full

[btdubs]

If the FSA memory cannot allocate more nodes, the search will now fail explicitly.
Before, this behavior would also cause the search to fail but was not an explicit check so it could have other results.

Maybe there should be a different fail state or a reason for failure too, but this works for now.

[lucas-tiz]

Did you test this in sim? We can force another memory failure in the avoidance test by reverting to the x, y, theta hash you originally used.