Running FASTER in a world where the ground is positioned under the coordinate origin (z<0)

Hi @jtorde,
I am trying to run FASTER in a world that has a ground plane with position z<0 and parallel to the x-y-plane. When starting the quad from (z,y,z)=(0,0,1) and giving it the goal (z,y,z)=(20,0,-1) it will begin a downward trajectory until it has reached z=0 and fly on the x-y-plane until it is near the goal when, again, it will move down towards the goal and reach it.
When the quad already has a position with its z component <0 FASTER won't re-plan and display:

state_initialized_= 1state_initialized_= 
kdtree_map_initialized_= 0
kdtree_unk_initialized_= 1
terminal_goal_initialized_= 1
Not publishing new goal!!

Sometimes though, it will hover exactly over the goal at z=0 and the FASTER terminal will display:

************IN REPLAN CB*********
Occupancy Grid received is empty, maybe map is too small?
NeedToComputeSafePath=0

I ran these tests with line 8 set to false and line 18 set to -20 in faster.yaml.
This was not the case for goals with z>=0 where the algorithm followed an optimal trajectory when moving upwards or downwards.
I would appreciate any help!

Start by reproducing the reported trajectory and replanning behavior with line 8 set to false and line 18 set to -20 in faster.yaml. Inspect the planner and terminal replanning path responsible for the displayed initialization and empty occupancy-grid messages. Done means FASTER can re-plan and reach goals when the ground plane is at z<0, including when the vehicle already has a negative z position.