Updated tf_listener to include yaw. Yaw control should come from mo-cap. Still need to fix the issue with casting std_msgs::Float32 into double in the callback.