add yaw for control, see https://code.ros.org/trac/wg-ros-pkg/ticket/5118 for original ticket