Straightened init function registration (and removed unnecessary functions)
Save planning scene message to be published when node is not yet started