Transform pose from object of one frame to another frame without publishing/subscribing to tf
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 : http://wiki.ros.org/tf/data%20types, 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.