add yaw for control

see https://code.ros.org/trac/wg-ros-pkg/ticket/5118 for original ticket

we're currently testing this on real robot