Removed ros time now and added velocity calculation properly