ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to use Quaternion from IMU to get the transformation to inertial reference frame?

asked 2021-10-13 06:42:56 -0500

Astronaut gravatar image

updated 2021-10-18 02:06:45 -0500

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-10-14 09:35:01 -0500

tryan gravatar image

Have you looked through the tf2 tutorials? I highly recommend them. Specifically, Writing a tf2 listener (C++) and Using Stamped datatypes with tf2_ros::MessageFilter demonstrate how to find the transform between two frames and use the result to transform a point. Extending it to a vector (acceleration) should be fairly straightforward, but feel free to ask for clarification if you run into trouble.

This reference documentation may also be helpful:

edit flag offensive delete link more

Comments

HI. I edit the code in the question. Please can you look if its correct or I missed something as im not so familiar with the TF. I went through the tutorial and checked the link text but still.

Astronaut gravatar image Astronaut  ( 2021-10-18 02:02:53 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2021-10-13 06:42:56 -0500

Seen: 287 times

Last updated: Oct 18 '21