Child rotating around parent axes instead of his own

asked 2018-10-12 11:20:13 -0600

antoineniotna gravatar image

So, I have a C ++ project which is simulating a car. My program is working in 2D( only in the XY plane), it feed with odometries data gave by a rosbag. My odometries values are treated by a node called pose. pose is send the transformation of the vehicle to my map, everything is fine in 2D. My frame_id is odometry and my child_id is vehicle. But when I am in 3D, meaning I can rotate my car around several axes, not only Z anymore. I realized that my car is rotating around the axes of my frame_id(the "world" axes) when I would like them to turn around my vehicle axes.

In order to test it, I did a dummy code where my vehicle is suppose to do a 90deg rotatation one the Z axe and then a 90 deg rotation on Y axe.

Here is a piece of my code to illustrate:

void NodeImpl::callback_odometry(motion_common_msg::Odometry input_msg)
  //Getting Direction3D

    frame_id = input_msg.header.frame_id;
    stamp = input_msg.header.stamp;


    tf::Quaternion q;
    q.setRPY(0.0, 0.0, 0.0);

    if(input_msg.pos_x >= 5.0)
        double cos90 = 0;
        double sin90 = 1;

        tf::Matrix3x3 Rz90;
        tf::Matrix3x3 Ry90;

        Rz90.setValue(cos90, - sin90, 0, sin90, cos90, 0, 0, 0, 1);
        Ry90.setValue(cos90, 0, sin90, 0, 1, 0, -sin90, 0, cos90);

        tf::Quaternion qz90;
        tf::Quaternion qy90;



        tf::Transform tf_new_rot;



        transform_from_odometry.mult (transform_from_odometry, tf_new_rot);


void NodeImpl::broadcast()
     static tf::TransformBroadcaster br;
     br.sendTransform(tf::StampedTransform(transform_from_odometry, stamp, frame_id, "ego_vehicle_rear_axis"));

I can do to have my vehicle rotating around his own axes ?

edit retag flag offensive close merge delete