Skip to content
Snippets Groups Projects
Commit c3853a5a authored by Tommy Persson's avatar Tommy Persson
Browse files

Work.

parent f7227d56
Branches main
No related tags found
No related merge requests found
...@@ -109,28 +109,43 @@ private: ...@@ -109,28 +109,43 @@ private:
imu_msg.header.stamp = ros::Time::now(); imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id; imu_msg.header.frame_id = frame_id;
// Fill orientation quaternion imu_msg.orientation.x = 0.0;
// imu_msg.orientation.w = data.q[0]; imu_msg.orientation.y = 0.0;
// imu_msg.orientation.x = -data.q[1]; imu_msg.orientation.z = 0.0;
// imu_msg.orientation.y = -data.q[2]; imu_msg.orientation.w = 1.0;
// imu_msg.orientation.z = -data.q[3];
imu_msg.orientation_covariance[0] = -1.0;
// Fill angular velocity data
// - scale from deg/s to rad/s imu_msg.angular_velocity.x = gx*M_PI/180.0;
imu_msg.angular_velocity.x = gx*3.1415926/180; imu_msg.angular_velocity.y = gy*M_PI/180.0;
imu_msg.angular_velocity.y = gy*3.1415926/180; imu_msg.angular_velocity.z = gz*M_PI/180.0;
imu_msg.angular_velocity.z = gz*3.1415926/180;
imu_msg.angular_velocity_covariance[0] = 0.0;
// Fill linear acceleration data 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.x = -ax*9.81;
imu_msg.linear_acceleration.y = -ay*9.81; imu_msg.linear_acceleration.y = -ay*9.81;
imu_msg.linear_acceleration.z = -az*9.81; imu_msg.linear_acceleration.z = -az*9.81;
// \TODO: Fill covariance matrices imu_msg.linear_acceleration_covariance[0] = 0.0;
// msg.orientation_covariance = ... imu_msg.linear_acceleration_covariance[1] = 0.0;
// msg.angular_velocity_covariance = ... imu_msg.linear_acceleration_covariance[2] = 0.0;
// msg linear_acceleration_covariance = ... 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 */ /* Fill the magnetometer message */
// mag_msg.header.stamp = imu_msg.header.stamp; // mag_msg.header.stamp = imu_msg.header.stamp;
// mag_msg.header.frame_id = frame_id; // mag_msg.header.frame_id = frame_id;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment