Introduced functions for ensuring a pose transformation and simplified code

The ability to transform a pose via tf can now be ensured using `ensure-pose-stamped-transformable`. To ensure that a pose is actually transformed, `ensure-pose-stamped-transformed` is available.