Add position printers and use the imuTime rather than ros::Time.now.