Added changes to make it work as intended as Dirk Thomas said in the ros-users mailing list (and posted in the ros wiki)