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

edit

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)