Added collision and proximity measurements; Use is_navigable for a more robust...

Added collision and proximity measurements; Use is_navigable for a more robust check for top-down map