Convert roll pitch yaw to quaternion

In ROS, we could use tf::createQuaternionFromRPY to convert roll pitch yaw to quaternion. We need to include tf in the find_package, and  <build_depend>tf</build_depend <run_depend>tf</run_depend> in the package.xml.

In the source file, add #include "tf/transform_datatypes.h", the use tf::Quaternion q = tf::createQuaternionFromRPY( roll, pitch, yaw); for conversion.

To publish the quaternion in IMU message, use

imu_msg.orientation.x = q.x();

imu_msg.orientation.y = q.y();

imu_msg.orientation.z = q.z();

imu_msg.orientation.w = q.w();