Add script for stamping goals in old bag files
Add script for computing relative odometry