Added tf publishing. The behaviour is not as expected, have to investigate.
Cleaning up/refactoring code
Moved static member to field
Moved ROS_INFO-s to ROS_DEBUG or removed. Added typedef for iterator type.