Added sensor pose to write. Fixes #9
Removed print statement