Can't get transforms using waitForTransform

Question

Hello everyone. I recorded a bag file using messages generated by Stage simulator; thus, when bag file is played, both odometric and scanner messages are sent again. I wrote a function that should receive a laser message and find the associated odometric message, using waitForTransform, on professor's suggestion:

void messageListener::laserCallback(const sensor_msgs::LaserScan msg){
  //std::string frame_id = msg->header.frame_id;
  ros::Time t = msg.header.stamp;
  tf::StampedTransform laserPose;
  std::string error;
  if (listener->waitForTransform ("/odom", "/base_scan", t, ros::Duration(0.5), ros::Duration(0.01), &error)){
    listener->lookupTransform("/odom", "/base_scan", t, laserPose);
    double yaw,pitch,roll;
    tf::Matrix3x3 mat =  laserPose.getBasis();
    mat.getRPY(roll, pitch, yaw);

    pos_node pn;
    pn.x = laserPose.getOrigin().x();
    pn.y = laserPose.getOrigin().y();
    pn.angle = yaw;

    const vector<float>& ranges =  msg.ranges;
    scan_node temp_sn;
    temp_sn.cartesian = polar2cart(ranges, msg.angle_min, msg.angle_increment, msg.range_min, msg.range_max);
    vector<scan_node> tempScanVec;
    tempScanVec.push_back(temp_sn);
    extractCorners(&tempScanVec, 0.1, 0.1, 0.3);
    line2angle(&tempScanVec);

    pn.corn_vec = tempScanVec[0].corners_list;
    nodeInfo.push_back(pn);

   }

  else
    printf("Nothing to do\n");
}

The problem is that the IF block where the trasnform should be found is never executed and I always get the nothing to do printing; this makes me understand that somehow transform is never found. I suppose that problem may regard parameters of the function itself. This is the way I perform subscription:

ros::NodeHandle n;
ros::Subscriber sub_n = n.subscribe("/base_scan", 1000, &messageListener::laserCallback, this);

Does anyone know where I'm mistaking? Thanks

---

Originally posted by ubisum on ROS Answers with karma: 23 on 2014-01-22

Post score: 0

Answer 1

Increase the duration on which it is asking until you see something. Also insert a printf() in your if loop, it could be working sometimes but now you won't see it.

Do you also see odom and base_scan when you do a rostopic echo /tf ?

---

Originally posted by davinci with karma: 2573 on 2014-01-23

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by ubisum on 2014-01-23:
thanks for answer. I increased fourth argument of waitForTransform to 1.5 seconds, but this slowed down my program; I added a printing of error returned by waitForTransform, but it never got printed. this suggests IF block is never entered. what about first two arguments? do you think they're right?

Comment by ubisum on 2014-01-23:
they represent the two topics Stage uses for advertising.

Comment by davinci on 2014-01-23:
The first two arguments should be parts of your robot not topics. Do you also see these points when you do a rostopic echo /tf ?

Comment by ubisum on 2014-01-23:
thanks again for answer. my original problem was to couple odometric and laser messages generated from same robot pose. at the beginning, I used two queues, one for each kind of message. then, I took a message from top of each queue, since they were for sure generated from same pose...

Comment by ubisum on 2014-01-23:
... they could be combined, then, in one object. my professor told me to use tf to do the same and I'm trying to adapt a snippet of code he gave me. Anyway rostopic echo /tf gives no output. do you know how to use tf for my goal?

Comment by davinci on 2014-01-23:
If rostopic echo /tf doesn't give any output than the code will not work. This means tf is not running and asking transformations will not work. Your bag file should also contain tf messages or some other way to track transformations between frames.

Comment by ubisum on 2014-01-24:
this is what I suspected, after reading tf tutorial. It seemed clear that robot frames must be inserted into tf tree, before trying to retrieve transforms. Professor told me that Stage automatically publishes frame to /tf, but it seems untrue. moreover, tf is not listed in Stage's published topics..

Comment by ubisum on 2014-01-24:
... do you think there's a way to force Stage to publish on /tf? is it possible to exploit the bag file I'm currently using without generating a new one? thanks

Comment by davinci on 2014-01-26:
I don' know much about stage. Perhaps you should start a new question for that. But with rxbag you can check what is in the bagfile. If there is nothing in the tf stream you have to generate a new bag file.