Should be obstacle subtraction properly now