changed variable names to keep with ROSCPP style
changed to write to separate bagfile