Extend HFN to optionally control z-position
Bugfix for OccupancyMap's cspace update
hfn: Print reason for aborting