Ask Your Question
0

Can't get transforms using waitForTransform

asked 2014-01-22 22:56:14 -0500

ubisum gravatar image

updated 2014-01-28 17:19:08 -0500

ngrennan gravatar image

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2014-01-22 23:17:24 -0500

davinci gravatar image

updated 2014-01-23 03:52:52 -0500

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 ?

edit flag offensive delete link more

Comments

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?

ubisum gravatar image ubisum  ( 2014-01-22 23:47:42 -0500 )edit

they represent the two topics Stage uses for advertising.

ubisum gravatar image ubisum  ( 2014-01-22 23:48:22 -0500 )edit

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 ?

davinci gravatar image davinci  ( 2014-01-23 03:51:50 -0500 )edit

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...

ubisum gravatar image ubisum  ( 2014-01-23 04:00:32 -0500 )edit

... 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?

ubisum gravatar image ubisum  ( 2014-01-23 04:02:12 -0500 )edit

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.

davinci gravatar image davinci  ( 2014-01-23 21:35:53 -0500 )edit

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..

ubisum gravatar image ubisum  ( 2014-01-23 23:00:08 -0500 )edit

... 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

ubisum gravatar image ubisum  ( 2014-01-23 23:01:48 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-01-22 22:56:14 -0500

Seen: 662 times

Last updated: Jan 23 '14