Added 3D twist publishing.

Not sure if this is any of use but I was using this for sending a 3D twist to a quad-rotor so I thought I'd open a PR.