Your yaw calculation looks wrong. If you already have a pose, the yaw in radians should already be available in msg.pose.pose.orientation.z.
The goal assignment is in error because you are trying to assign a quaternion to the w component of another quaternion. Maybe you mean this:
goal.target_pose.pose.orientation = gth;