how to publish pitch, roll, yaw to robot_pose_ekf

Question

I have accurate pitch, roll, and yaw values from an IMU, computed on an arduino. I have good serial comm and am already sending odometry data from the encoders and publishing that on /odom. I know that robot_pose_ekf can take in imu_data through sensor_msgs/imu but cannot figure out how to fill in the pitch, roll, yaw data into:

geometry_msgs/Quaternion orientation

float64[9] orientation_covariance

// Row major about x, y, z axes.

Also, do I need to supply values for angular velocity and linear acceleration? I see that those are part of sensor_msgs/imu, but don't want to send serial data at the rate required to accurately keep up with these velocities and it seems that /odom is doing fine with translation.

geometry_msgs/Vector3 angular_velocity

float64[9] angular_velocity_covariance

//Row major about x, y, z axes

geometry_msgs/Vector3

linear_acceleration float64[9] linear_acceleration_covariance

Row major x, y z

I have gone through the nav tutorials, looked (a lot) at turtlebot code, but just cannot decipher the details of computing the quaternion and properly filling in the covariances.

I did use this in /odom for yaw:

//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion> odom_quat => tf::createQuaternionMsgFromYaw(th);

but how do you do that for pitch, roll, and yaw in the context of /imu?

Ultimately, I hope to figure out how to publish TwistWithCovariance and PoseWithCovariance, but for now I'll settle for just getting the imu data to robot_pose_ekf.

Thanks for the help!

---

Originally posted by dan on ROS Answers with karma: 875 on 2013-08-04

Post score: 0

Answer 1

sensor_msgs/Imu use Quaternion, so you have to converter your euler angle to a quaternion angle.

 tf::Quaternion q = tf::createQuaternionFromRPY( roll ,pitch , yaw );

I'm not sure if you have to fill in too velocity and acceleration. I don't think so but if you have it, it's better to supply it.

Covariance is only a float64[9] so you can fill in like a array. You should estimate your accuracy on each axes (roll pitch and yaw) with your sensor. The diagonal terms are the square of your accuracy. Other terms are useful if variables are dependent, if not you can put zero values.

---

Originally posted by jep31 with karma: 411 on 2013-08-05

This answer was ACCEPTED on the original site

Post score: 3

---

Original comments

Comment by dan on 2013-08-05:
So, I should set orientation = tf::createQuaternionFromRPY( roll ,pitch , yaw ); and enter the variance of each of those into the 3 diagonals of orientation_covariance, with the other entries = 0. Is that right? Finally, do I publish this on /imu or on /imu/data or on /imu/data_raw?

Comment by jep31 on 2013-08-05:
Yes you're right. You should publish on /imu_data or you can remap this topic to an other name.