filled world model and changed to PoseStamped
Merge remote-tracking branch 'origin-ipa-fmw/master' into automerge