# Child rotating around parent axes instead of his own

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 ?

edit retag close merge delete