Now publishes statistics over ros topic and bug fixes

Now publishes statistics over ros topic /TopologicalNavigation/Statistics and bug fixes