How to use Quaternion from IMU to get the transformation to inertial reference frame?
Im trying to get the Linear Velocity integrating the acceleration from IMu. I know there will be accumulated error due to integration bit before the integration I have to do couple of other steps. First step would be transformation to the inertial reference frame. So basically using Quaternion transformation . Second, using that transformation to get the acceleration on the IMU regarding that inertial reference frame. And third final will be to integrate that transformed acceleration.
So here the steps before the integration part Quaternion-> T(r); a(inertial-reference-frame) = T(r)*a(imu);
So how would be ROS node (C++) that handle these two steps before the integration?
I checked the comments and the tf tutorial so tried to make a node.. But node sure if still correct . Please any help or suggestion about the code? Here is my ROS C++ Callback regarding the TF Frame transformation and Velocity integration
void IMUIntegrator::imu_callback(const sensor_msgs::ImuConstPtr &msg)
{
geometry_msgs::QuaternionStamped imu_quat = geometry_msgs::QuaternionStamped();
tf::StampedTransform transform;
tf::TransformListener listener;
listener.waitForTransform("/base_link",
msg->header.frame_id,
msg->header.stamp,
ros::Duration(3.0));
listener.lookupTransform("/base_link",
msg->header.frame_id,
msg->header.stamp,
transform);
imu_quat.header = msg->header;
imu_quat.quaternion = msg->orientation;
geometry_msgs::QuaternionStamped q_robot;
listener.transformQuaternion("base_link",msg->header.stamp,imu_quat, imu_quat.header.frame_id, q_robot);
tf::Quaternion quatquat;
tf::quaternionMsgToTF(q_robot.quaternion,quatquat);
tf::Matrix3x3(quatquat).getEulerYPR(new_yaw,new_pitch,new_roll);
acceleration_x = (msg->linear_acceleration.x);
//acceleration_x_tf= msg->linear_acceleration.x ;
acceleration_x = transform.getOrigin().x();
if (count2 == 0){
time_prev = ros::Time::now().toSec();
count2=1;
}
float time_now = ros::Time::now().toSec();
float time_dif = time_now - time_prev;
//float dt = timestamp - m_prev_timestamp;
time_prev = time_now;
//m_prev_timestamp = timestamp;
acceleration_x = (msg->linear_acceleration.x );
m_velocity += acceleration_x*time_dif;
send_velocity();
}
Thanks