Work.

src/axis_imu.cc

@@ -109,28 +109,43 @@ private:
       imu_msg.header.stamp = ros::Time::now();
       imu_msg.header.frame_id = frame_id;
 
-      // Fill orientation quaternion
-      //      imu_msg.orientation.w = data.q[0];
-      //      imu_msg.orientation.x = -data.q[1];
-      //      imu_msg.orientation.y = -data.q[2];
-      //      imu_msg.orientation.z = -data.q[3];
-
-      // Fill angular velocity data
-      // - scale from deg/s to rad/s
-      imu_msg.angular_velocity.x = gx*3.1415926/180;
-      imu_msg.angular_velocity.y = gy*3.1415926/180;
-      imu_msg.angular_velocity.z = gz*3.1415926/180;
-
-      // Fill linear acceleration data
+      imu_msg.orientation.x = 0.0;
+      imu_msg.orientation.y = 0.0;
+      imu_msg.orientation.z = 0.0;
+      imu_msg.orientation.w = 1.0;
+      
+      imu_msg.orientation_covariance[0] = -1.0;
+
+      imu_msg.angular_velocity.x = gx*M_PI/180.0;
+      imu_msg.angular_velocity.y = gy*M_PI/180.0;
+      imu_msg.angular_velocity.z = gz*M_PI/180.0;
+
+      imu_msg.angular_velocity_covariance[0] = 0.0;
+      imu_msg.angular_velocity_covariance[1] = 0.0;
+      imu_msg.angular_velocity_covariance[2] = 0.0;
+      
+      imu_msg.angular_velocity_covariance[3] = 0.0;
+      imu_msg.angular_velocity_covariance[4] = 0.0;
+      imu_msg.angular_velocity_covariance[5] = 0.0;
+      
+      imu_msg.angular_velocity_covariance[6] = 0.0;
+      imu_msg.angular_velocity_covariance[7] = 0.0;
+      imu_msg.angular_velocity_covariance[8] = 0.0;
+
       imu_msg.linear_acceleration.x = -ax*9.81;
       imu_msg.linear_acceleration.y = -ay*9.81;
       imu_msg.linear_acceleration.z = -az*9.81;
-      
-      // \TODO: Fill covariance matrices
-      // msg.orientation_covariance = ...
-      // msg.angular_velocity_covariance = ...
-      // msg linear_acceleration_covariance = ...
-      
+
+      imu_msg.linear_acceleration_covariance[0] = 0.0;
+      imu_msg.linear_acceleration_covariance[1] = 0.0;
+      imu_msg.linear_acceleration_covariance[2] = 0.0;
+      imu_msg.linear_acceleration_covariance[3] = 0.0;
+      imu_msg.linear_acceleration_covariance[4] = 0.0;
+      imu_msg.linear_acceleration_covariance[5] = 0.0;
+      imu_msg.linear_acceleration_covariance[6] = 0.0;
+      imu_msg.linear_acceleration_covariance[7] = 0.0;
+      imu_msg.linear_acceleration_covariance[8] = 0.0;
+
       /* Fill the magnetometer message */
       //      mag_msg.header.stamp = imu_msg.header.stamp;
       //      mag_msg.header.frame_id = frame_id;