2
$\begingroup$

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 have part of the code that think can do that but Im not sure

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 = 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;
    time_prev = time_now;
    acceleration_x = (msg->linear_acceleration.x );
    m_velocity += acceleration_x*time_dif;
    send_velocity();

Is this the correct way?Thanks

Improve this question
$\endgroup$

1 Answer 1

Reset to default
2
$\begingroup$

You should use ROS' inbuilt transforms. Assuming that your IMU data is being read as a ROS message, and you know all the frame id's, this example template explains the basics of using tf's.

Improve this answer
$\endgroup$
4
  • $\begingroup$ Yes my IMU data is build as IMU message. But first step dont think can be achieved using what you proposed (ROS transforms). I think for that need to use translation or rotation matrix with Quaternion $\endgroup$
    – bob
    Oct 13, 2021 at 13:11
  • $\begingroup$ ROS tf transform library use linear translations and rotation matrices to apply the transform to your data. $\endgroup$
    – Akhil Kurup
    Oct 14, 2021 at 0:10
  • $\begingroup$ would it be like in the question I edit? $\endgroup$
    – bob
    Oct 14, 2021 at 13:07
  • $\begingroup$ Yes, I think you are on the right track $\endgroup$
    – Akhil Kurup
    Oct 14, 2021 at 15:33

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.