0
$\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

$\endgroup$
0
$\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.

$\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 at 13:11
  • $\begingroup$ ROS tf transform library use linear translations and rotation matrices to apply the transform to your data. $\endgroup$ Oct 14 at 0:10
  • $\begingroup$ would it be like in the question I edit? $\endgroup$
    – bob
    Oct 14 at 13:07
  • $\begingroup$ Yes, I think you are on the right track $\endgroup$ Oct 14 at 15:33

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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