Ask Your Question
0

[tf2] get new transform as soon as possible (callback)?

asked 2020-10-20 04:24:59 -0500

felixN gravatar image

Hi, I'm trying to use April tags for navigation. For this, I would like to be informed as soon as possible when a new transform between the odom frame and a tag fram is awailable (the best solution would be a callback, but any other solution is fine as well).

My tree (or rather forest) looks like that :

map -> odom -> baselink -> camera

TAG_1 -> WA_1

TAG_2 -> WA_2

...

TAG_N -> WA_N

when a given tag (or several) are detected, a link camera->TAG_i is added

N will be between 50 and a few hundreds

I have the following constraints :

  • I would like to be able to get the odom->tag_i transform as soon as it becomes awailable

  • I don't want to get any "old" data (ie if I processed it once, I don't want to process it a second time)

  • I don't want to have something "blocking" (or it has to be in a separated thread

I have thought of several solutions, non of which really satisfies me :

  • the classical polling (like in the tf listener tutorial) : I suppose it is quite expensive to pull up to hundreds of potential links just to get the right one. And if I'm not mistaken, if 0.1 seconds later I start again, I will get a second time the same data if no new one arrived in the meantime (well, I still can check the stamps to reject it if it hasn't changed)

  • I tried putting a callback on the /tf topic, identify the child_id, and if it is tag_i, then I ask for the transform from odom to child_i : if seems to work most of the time, but sometimes I get

[ERROR] [1602233077.384387072]: Lookup would require extrapolation into the past.  Requested time 1602233066.485275640 but the earliest data is at time 1602233067.347147640, when looking up transform from frame [WA_2] to frame [odom]

The code for the /tf listenner is :

void TagsHandler::tfCallback(const tf2_msgs::TFMessageConstPtr &msg)
{
    //cout<<"received tf message"<<endl;;
    int i = 0;
    for(std::vector<geometry_msgs::TransformStamped>::const_iterator it = msg->transforms.begin(); it != msg->transforms.end(); ++it)
    {
        string child_frame_id=it->child_frame_id;
    //  cout<<"transform #"<<i<<": to="<<child_frame_id<<endl;
        if(child_frame_id.substr(0,4)=="TAG_")
        {
            int tag_id;
            try
            {
                tag_id=stoi(child_frame_id.substr(4,child_frame_id.length()-4));
            }
            catch(exception const& e)
            {
                cerr<<"erreur de conversion :"<< e.what()<<endl;
                continue;
            }
            //cout<<"detecting tag :"<<tag_id<<endl;
            string WA_frame_id="WA_"+to_string(tag_id);
            string WS_frame_id="WS_"+to_string(tag_id);
            geometry_msgs::TransformStamped transformStamped_WA_odom;
            geometry_msgs::TransformStamped transformStamped_WS_odom;
            try{
              transformStamped_WA_odom=tfBuffer.lookupTransform("odom", WA_frame_id, ros::Time(0));
              transformStamped_WS_odom=tfBuffer.lookupTransform("odom", WS_frame_id, ros::Time(0));
            }
            catch (tf2::TransformException &ex) {
                cerr<<"on n'a pas réussi à récupérer la transformée"<<endl;
                ROS_ERROR("%s",ex.what());
                continue;
            }
            Point2D position_of_WA_in_odom(transformStamped_WA_odom.transform.translation.x, transformStamped_WA_odom.transform.translation.y);
            rosHandler->publish_debug_message(3,"WA_in_odom: x="+to_string(position_of_WA_in_odom.x)+"y="+to_string(position_of_WA_in_odom.y));
            this->add_visible_tag(tag_id, position_of_WA_in_odom);
        }
        i++;
    }
}

Do you know why the transform is not always awailable? Does it take some time after the tf message arrives until it is processed?

Thanks a ... (more)

edit retag flag offensive close merge delete

Comments

Can't you use tf message_filter instead? Would be much cleaner I believe.

fkie_message_filters also supports this.

gvdhoorn gravatar image gvdhoorn  ( 2020-10-20 05:19:46 -0500 )edit

Thanks for the suggestion. But how do I do it when I have only messages published to tf (and no geometry_msgs/PointStamped)? In addition, I have some waypoints that are defined in the tag frame : how do I handle it? It's not enough to know the tag location, I also need its orientation

felixN gravatar image felixN  ( 2020-10-20 05:28:37 -0500 )edit

The PointStamped is just an example.

It's not limited to that message type at all.

gvdhoorn gravatar image gvdhoorn  ( 2020-10-20 05:29:39 -0500 )edit

OK, thanks. I'm still not sure how I would use it in my situation? Should I try to transform the TAG_i to WA_i transform into odom frame? Or should I try to create a PointStamped for WA_i in TAG_i frame and try to get this point in odom frame? Or something else?

Nb : the transform TAG_i to WA_i is a static one

felixN gravatar image felixN  ( 2020-10-20 06:10:15 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2020-10-20 14:30:23 -0500

tfoote gravatar image

If you want to get data callbacks when a transform becomes available the tf2_ros::MessageFilter is what I'd recommend.

If you don't have a datastream which you're holding you can inject place holder data in one coordinate frame and time, and target it to the other frame of interest. And when that callback is triggered you know the transform is available.

As @gvdhoorn mentions the the PointStamped is a common minmal datatype for the above. It's often used due to being small and low overhead. The system works for any datatype with a header.

edit flag offensive delete link more

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: 2020-10-20 04:24:59 -0500

Seen: 137 times

Last updated: Oct 20 '20