Child rotating around parent axes instead of his own

edit

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;

    transform_from_odometry.setOrigin(new_position_3D);

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

    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;

        Rz90.getRotation(qz90);
        Ry90.getRotation(qy90);

        qz90.normalize();
        qy90.normalize();

        tf::Transform tf_new_rot;

        tf_new_rot.setRotation(qz90);

        transform_from_odometry.setRotation(qy90);

        transform_from_odometry.mult (transform_from_odometry, tf_new_rot);
    }

    broadcast();
}

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 ?