Finally, the routine is working and correctly integrated into ROS.
(as far as any integration with ROS can be called correct)
Many finalizing changes.