Hello,
TL;DR tf::TransformListener gets a different transform than shown in RVIZ
I am having quite a strange problem. I am using a rosbag file to playback recorded data and I am adding new transforms and using them while doing that:
rosparam set use_sim_time true
rosbag play 2016-09-12-14-51-41.bag --clock
The transform is published with a python node, where the origin is static (defined with cfg-values) and the rotation is coming from the values of an IMU. The interesting part how the tf is published should be this (in the callback of the IMU subscription):
br = tf.TransformBroadcaster()
br.sendTransform((frame_offset_x, frame_offset_y, frame_offset_z),
quaternion,
rospy.Time.now(),
frame_child,
frame_parent
)
With the following values:
frame_offset_x = 0
frame_offset_y = 0
frame_offset_z = 1.2
quaternion = rotation of the IMU (only around the x and y axis)
frame_child = level_laser_2
frame_parent = base_footprint
The created Transform in RVIZ looks like expected. It's 1.2m above base_footprint with some rotation on top.
In the picture, the Fixed Frame is set to "base_footprint"
However if I lookup the transform in a ROS node using the tf::TransformListener I get a different result:
#include
#include
...
tf_listener_ = new tf::TransformListener( ros::Duration(10) ); // in the constructor
...
void callback( const sensor_msgs::PointCloud2Ptr& cloud_in ) {
...
if ( tf_listener_->waitForTransform(tf_target_,
cloud_in->header.frame_id,
cloud_in->header.stamp,
ros::Duration( scan_time_ )) ) {
tf::StampedTransform transform;
tf_listener_->lookupTransform(tf_target_,
cloud_in->header.frame_id,
cloud_in->header.stamp,
transform);
ROS_INFO("%s %s -> %s\tat%lf", node_name_.c_str(),
cloud_in->header.frame_id.c_str(),
tf_target_.c_str(),
cloud_in->header.stamp.toSec());
tfScalar tf[16];
transform.getOpenGLMatrix(tf);
ROS_INFO("%s\n"
"/ %lf\t%lf\t%lf\t%lf \\\n"
"| %lf\t%lf\t%lf\t%lf |\n"
"| %lf\t%lf\t%lf\t%lf |\n"
"\\ %lf\t%lf\t%lf\t%lf /", node_name_.c_str(),
tf[0], tf[4], tf[8], tf[12],
tf[1], tf[5], tf[9], tf[13],
tf[2], tf[6], tf[10], tf[14],
tf[3], tf[7], tf[11], tf[15]);
} else {
// display error here
}
...
}
The output of this is:
[ INFO] [1473756806.521699009, 1473684710.202103154]: /level_laser_velodyne: base_footprint -> level_laser_2 at1473684709.395050
[ INFO] [1473756806.521764836, 1473684710.202103154]: /level_laser_velodyne:
/ 0.997645 0.001908 -0.068567 0.002741 \
| 0.001908 0.998454 0.055557 -0.002221 |
| 0.068567 -0.055557 0.996098 -0.039822 |
\ 0.000000 0.000000 0.000000 1.000000 /
But it should be something like:
[ INFO] [1473756806.521764836, 1473684710.202103154]: /level_laser_velodyne:
/ R R R 0 \
| R R R 0 |
| R R R 1.2 |
\ 0 0 0 1 /
Does anybody know what I am doing wrong?
Thank you for your help,
Tobias
↧