Forum
Sign up Calendar Latest Topics
 
 
 


Reply
  Author   Comment  
Carter

Avatar / Picture

Senior Member
Registered:
Posts: 132
Reply with quote  #1 
So we at Cabrillo have been experiencing problems with having our "master" system being between 0.03 and .07 seconds ahead of the clock on the Swarmie this causes  a Extrapolation Exception: Lookup would require extrapolation into the future

This is breaking at a deep ros level, this might not be a problem during the competition as all of the code will be running on the rover, but it might be.  
Right now I am running the behavior code on my laptop and that may be causing the problem. 

So the easy solution is to have the Master running a NTP server and have the rovers connect to it and ensure their time is before "Master". 


File "/home/carter/Robotics/Swarmathon-Cabrillo/src/mobility/scripts/dropoff.py", line 48, in get_center_pose_list
    odom_pose = swarmie.xform.transformPose(rovername + '/odom', t.pose)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/tf/listener.py", line 296, in transformPose
    mat44 = self.asMatrix(target_frame, ps.header)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/tf/listener.py", line 183, in asMatrix
    translation,rotation = self.lookupTransform(target_frame, hdr.frame_id, hdr.stamp)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/tf/listener.py", line 104, in lookupTransform
    msg = self._buffer.lookup_transform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/tf2_ros/buffer.py", line 87, in lookup_transform
    return self.lookup_transform_core(target_frame, source_frame, time)
tf2.ExtrapolationException: Lookup would require extrapolation into the future.  Requested time 1517961800.429247618 but the latest data is at time 1517961800.351879120, when looking up transform from frame [lovelace/camera_link] to frame [lovelace/odom]

__________________
-Carter Frost
Senior Student Trouble Maker
Cabrillo College, Aptos CA
Nocare

Senior Member
Registered:
Posts: 194
Reply with quote  #2 
So to me that does not look like a discrepancy between master and the robots code.

Each frame of data is provided to the Tf node by a publisher with a time stamp in tow. To me that looks like the last time the camera or odom provided data was slightly before current time and thus you are asking for information that does not exist yet. This will be expected as the nodes that provided that data are run in parallel and even if they weren't messages cant reach the Tf node and be processed at current time they are always in the past.

The solution for me when ive hit this point is don't transform into the current time instead transform to the past point as the point slightly in the past is the most up to date you can get. I may be able to find an example but it could take a bit. The Tf node is a pain to deal with sometimes.

If you have evidence that contradicts what I said I am all ears

However as far as I am aware masters time is irrelevant beacuse even though Tf runs on master it doesn't use its current time to do transforms or the time it receives date but instead uses the time stamp attached the frame data it receives. That frame data is transmitted over ros topics and is done by each respective node.

__________________
- Jarett Jones UNM Dev Team
Carter

Avatar / Picture

Senior Member
Registered:
Posts: 132
Reply with quote  #3 
If you could provide an example of how to make a transform and shift the time that could be cool.
Right now I would have to iterate though the april tag detection array and shift the time stamp.

So to clarify:
Right now the code that calls for the transform is on the Master system, opencv is running on the Physical Swarmie

We(Cabrillo) don't deploy the same way you do, we compile only what is needed and transfer that to the Swarmie, the rest stays on our systems. So we don't have to recompile/deploy when we make behavior changes on our own systems, we just call it again with rosrun after saving the file that we made changes to.  

If you notice in the previous stacktrace its on my system user `Carter`, the swarmie only has the `robot` user. 
 "/home/carter/Robotics/Swarmathon-Cabrillo/src/mobility/scripts/dropoff.py", line 48, in get_center_pose_list
 

The ros wiki suggests the use of NTP to solve the problem
Scroll to the bottom
http://wiki.ros.org/ROS/NetworkSetup#Debugging_network_problems



__________________
-Carter Frost
Senior Student Trouble Maker
Cabrillo College, Aptos CA
Carter

Avatar / Picture

Senior Member
Registered:
Posts: 132
Reply with quote  #4 
Update: We are currently looking at TransformListener's getLatestCommonTime() http://docs.ros.org/kinetic/api/tf/html/python/tf_python.html#tf.Transformer.getLatestCommonTime


__________________
-Carter Frost
Senior Student Trouble Maker
Cabrillo College, Aptos CA
Nocare

Senior Member
Registered:
Posts: 194
Reply with quote  #5 
This is how we used to do transforms from camera_link to odom. I don't know if this is helpfull to you but it waits for a transform to become available after current time instead of asking for one at current time.

http://docs.ros.org/kinetic/api/tf/html/python/tf_python.html#tf.Transformer.waitForTransform


tagPose.header.stamp = ros::Time(0);
      geometry_msgsPoseStamped odomPose;
      try {
        tfListener->waitForTransform(publishedName + "/odom", publishedName + "/camera_link", ros::Time(0), rosDuration(1.0));
        tfListener->transformPose(publishedName + "/odom", tagPose, odomPose);
      }
      catch(tf::TransformException& ex) {
        ROS_INFO("Received an exception trying to transform a point from \"odom\" to \"camera_link\": %s", ex.what());
      }



If this or getLatestCommonTime doesnt work, or there is a performance concern I can talk to matthew about the possibility of the NPT server. He is very busy right now and it may take a bit before he comes to a decision.

__________________
- Jarett Jones UNM Dev Team
Carter

Avatar / Picture

Senior Member
Registered:
Posts: 132
Reply with quote  #6 
I just noticed that I have a transformPose that I don't wait for in my code.
I will try that out when I get out of class.

Thanks, 

__________________
-Carter Frost
Senior Student Trouble Maker
Cabrillo College, Aptos CA
Carter

Avatar / Picture

Senior Member
Registered:
Posts: 132
Reply with quote  #7 
Woot thanks it works, I just had to relook at that part of my code, we have waitForTransform all over the place, I just had omitted it. 
So I added

swarmie.xform.waitForTransform(swarmie.rover_name + '/odom', t.pose.header.frame_id, t.pose.header.stamp, rospy.Duration(3.0))

__________________
-Carter Frost
Senior Student Trouble Maker
Cabrillo College, Aptos CA
Nocare

Senior Member
Registered:
Posts: 194
Reply with quote  #8 
sweet glad to hear it. Im happy I could help.

Good luck at the competition im looking forward to seeing what you guys have done.

__________________
- Jarett Jones UNM Dev Team
Previous Topic | Next Topic
Print
Reply

Quick Navigation:

Easily create a Forum Website with Website Toolbox.