[pr2_navigation_self_filter] Support ~keep_organized parameter to fill filtered point with NaN

add ~keep_organized parameter to fill filtered-out point with NaN and keep width and height of the input pointcloud