Transform pose from object of one frame to another frame without publishing/subscribing to tf

asked 2019-09-03 10:54:18 -0500

autonomobil gravatar image

I have the following (3D-)situation:

There is the main coordinate system "base_link", which has the x-axis go up and the y-axis go left. There is a 3D box-shaped object. The pose (x, y, z, rot_x, rot_y, rot_z) of the object referenced to the middle point of the lower rear edge of the object is measured/known (in the coordinate system "base_link"). The object has 6DOF (rotation order z,y,x). I want to get the pose of the object referenced to the middle point of the middle line (at box_length/2) in the coordinate system "base_link". See topdown-view here (imgur link). I know in the (not implemented) coordinate-system of the object(originated at the measured point) the point of interest is at length/2.

Right now I'm doing the transformation manually:

object.Length = measured_object.Length; //double 
object.Width = measured_object.Width; //double 
object.Height = measured_object.Height; //double 

// get rotation of measured object, rotation order z,y,x
double rot_x = measured_object.rot_x;       
double rot_y = measured_object.rot_y;
double rot_z = measured_object.rot_z;

double x_old = measured_object.Length / 2;
double y_old = 0;
double z_old = 0;
double x, y, z = 0;

// Rotation around z-axis
x = x_old * cos(rot_z) - y_old * sin(rot_z);
y = x_old * sin(rot_z) + y_old * cos(rot_z);
x_old = x;
y_old = y;
// Rotation around y-axis
x = x_old * cos(rot_y) + z_old * sin(rot_y);
z = -x_old * sin(rot_y) + z_old * cos(rot_y);
x_old = x;
z_old = z;
// Rotation around x-axis
y = y_old * cos(rot_x) - z_old * sin(rot_x);
z = y_old * sin(rot_x) + z_old * cos(rot_x);
y_old = y;
z_old = z;

object.x = measured_object.x + x;
object.y = measured_object.y + y;
object.y = measured_object.z + z;

1.question: Is this even correct?

2.question: How can I do this with the ros tf library without setting a frame for every object and broadcasting/listening to the transform?

I found this :, there is a function tf::transformPose, but the link is dead. Is it something like this?

tf::TransformListener tf_listener;
tf::Transform transform;
transform.setOrigin(tf::Vector3(measured_object.x , measured_object.y, measured_object.z));
transform.setRotation(tf::createQuaternionFromRPY(measured_object.rot_x, measured_object.rot_y, measured_object.rot_z));

tf::StampedTransform stamped_transform = tf::StampedTransform(transform, ros::Time(0), "base_link", "obj_frame");
tf::Transform transform_obj(tf::Quaternion(0, 0, 0, 1), tf::Vector3(measured_object.Length / 2, 0.0, 0.0));
tf::Stamped<tf::Pose> transform_obj_pose(transform_obj, ros::Time(0), "obj_frame");
tf::Stamped<tf::Pose> new_obj_pose;
tf_listener.transformPose("base_link", transform_obj_pose, new_obj_pose);

It builds, but when I run the corresponding node, frame "base_link" can not be found.

edit retag flag offensive close merge delete