Added Subscribers and Publishers back, moved gatherOdom function, added gatherGPS function.