tf2 static transform publisher vs tf2 transfrom broadcaster
Dear people,
I have the following problem: I send a transform via the commandline rosrun tf2_ros static_transform_publisher and once via tf2_ros::TransformBroadcaster. Both should give me the same tf but they don't. I visualized it in the following image with my ground truth. The tf via the commandline is correct (close to ground truth), the one via the broadcaster is different in rotation not translation. The values (x,y,z, roll, pitch, yaw) and frame ids are exactly the same for both.
How is the following different from the command line?
ROS tf2 transfrom broadcaster:
geometry_msgs::TransformStamped transform_;
transform_.transform.translation.x = x;
transform_.transform.translation.y = y;
transform_.transform.translation.z = z;
tf2::Quaternion q;
q.setRPY(roll, pitch , yaw);
transform_.transform.rotation.x = q.x();
transform_.transform.rotation.y = q.y();
transform_.transform.rotation.z = q.z();
transform_.transform.rotation.w = q.w();
transform_.header.stamp = ros::Time::now();
transform_.header.frame_id = source_frame;
transform_.child_frame_id = dest_frame;
static tf2_ros::TransformBroadcaster br;
br.sendTransform(transform_);
Command line:
rosrun tf2_ros static_transform_publisher x y z yaw pitch roll source_frame dest_frame
Does someone know what could be wrong. The frame_id is the same for both.
Edit: I computed the quaternion myself using:
double qx = sin(roll) * cos(pitch) * cos(yaw) - cos(roll) * sin(pitch) * sin(yaw);
double qy = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * cos(pitch) * sin(yaw);
double qz = cos(roll) * cos(pitch) * sin(yaw) - sin(roll) * sin(pitch) * cos(yaw);
double qw = cos(roll) * cos(pitch) * cos(yaw) + sin(roll) * sin(pitch) * sin(yaw);
This seems to be correct (same result as the tf static publisher). The used function q.setRPY(roll, pitch, yaw) does something wrong. Does someone know why it's not correct or why I use it wrong?
Thank you
For the static_transform_publisher, did you notice the order of
yaw pitch rollis reversed?There are many different quaternion coordinate spaces, and ros uses a specific one. I would guess that your sin/cos equations are implementing the wrong one; the
setRPY()method is what you should use (in radians) because it will do the correct calculation for ros.Thanks for the answer. 1.) Yes, I noted that. 2.) ok, but
q.setRPY()should be the same as thestatic_transform_publisher?If you are asking about the
rotationfield, then yes.Pedantic nitpick: there are multiple values of the xyzw quaternion that map to the same euler (roll, pitch, yaw) - but for what you have described, I'd expect that the static publisher uses the same math as
setRPY(), so the values should be identical.I would check the source code and see what was the base coordinate system for rotation - maybe there was a difference?