Error with ATOM calibration on collect data

[JorgeFernandes-Git]

I wasn't able to collect a dataset with more than 3/4 collections until I get this error:

This problem only happened while collection data from a bag file saved on the real robot, using sim_time = false. On bag file made in simulation, this never happened.

3.092456, 1682076965.004035]: bad callback: <bound method InteractiveMarkerServer.processFeedback of <interactive_markers.interactive_marker_server.InteractiveMarkerServer object at 0x7f0d065fc970>>
Traceback (most recent call last):
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/opt/ros/noetic/lib/python3/dist-packages/interactive_markers/interactive_marker_server.py", line 353, in processFeedback
    feedback_cb(feedback)
  File "/opt/ros/noetic/lib/python3/dist-packages/interactive_markers/menu_handler.py", line 143, in processFeedback
    context.feedback_cb(feedback)
  File "/home/jorge/catkin_ws/src/atom/atom_calibration/scripts/collect_data", line 30, in menuFeedback
    data_collector.saveCollection()
  File "/home/jorge/catkin_ws/src/atom/atom_calibration/src/atom_calibration/collect/data_collector_and_labeler.py", line 324, in saveCollection
    transforms = self.getTransforms(self.abstract_transforms, average_time)  # use average time of sensor msgs
  File "/home/jorge/catkin_ws/src/atom/atom_calibration/src/atom_calibration/collect/data_collector_and_labeler.py", line 269, in getTransforms
    self.listener.waitForTransform(ab['parent'], ab['child'], time, rospy.Duration(1.0))
  File "/opt/ros/noetic/lib/python3/dist-packages/tf/listener.py", line 76, in waitForTransform
    raise tf2_ros.TransformException(error_msg or "no such transformation: \"{}\" -> \"{}\"".format(source_frame, target_frame))
tf2.TransformException: Lookup would require extrapolation 0.023380756s into the future.  Requested time 1682076964.993912220 but the latest data is at time 1682076964.970531464, when looking up transform from frame [wrist_3_link] to frame [wrist_2_link]

[JorgeFernandes-Git]

On ATOM I increase the wait time of of rospy.Duration from 1 second to 10 seconds. After that the data collection went okay!

atom_calibration/collect/data_collector_and_labeler.py", line 269

self.listener.waitForTransform(ab['parent'], ab['child'], time, rospy.Duration(1.0))

to:

self.listener.waitForTransform(ab['parent'], ab['child'], time, rospy.Duration(2.0))

seems to work.

[miguelriemoliveira]

Hi @JorgeFernandes-Git ,

When mentioning a line its better to add the link to it. Otherwise you are making others have to spend time searching.

Like this:

https://github.com/lardemua/atom/blob/0fc14cd683236faf8cf8389f77a0e4e77585a940/atom_calibration/src/atom_calibration/collect/data_collector_and_labeler.py#L262-L272

This is a strange problem, because tf messages typically have 100Hz of frequency, so its not normal to have to wait for more than 1 second. Can you please share the bagfile so I can take a look?

[JorgeFernandes-Git]

Hello @miguelriemoliveira. I revert the changes on ATOM and I'll try a different route.

As we talked yesterday, here is the calibration package on the branch created to test eye on hand calibration on the real arm: https://github.com/JorgeFernandes-Git/zau_bot/tree/Zau_INESCTEC_eye_on_hand_calibration/Zau_INESCTEC/zau_eye_on_hand_calibration

and the test bag file: https://drive.google.com/file/d/1d0-Hl0GLqqLgdX9rENO81j4Rey8qtMXs/view?usp=share_link

Here is the readme with some commands: https://github.com/JorgeFernandes-Git/zau_bot/tree/Zau_INESCTEC_eye_on_hand_calibration/Zau_INESCTEC#readme

[miguelriemoliveira]

OK, so the bagfile has the following data:

➜  zaubot rosbag info zau_eye_on_hand_test_bag.bag 
path:        zau_eye_on_hand_test_bag.bag
version:     2.0
duration:    54.5s
start:       Apr 21 2023 10:25:03.27 (1682069103.27)
end:         Apr 21 2023 10:25:57.74 (1682069157.74)
size:        156.5 MB
messages:    19058
compression: none [188/188 chunks]
types:       sensor_msgs/CameraInfo      [c9a58c1b0b154e0e6da7578cb991d214]
             sensor_msgs/CompressedImage [8f7a12909da2c9d3332d540a0977563f]
             sensor_msgs/JointState      [3066dcd76a6cfaef579bd0f34173e9fd]
             tf2_msgs/TFMessage          [94810edda583a504dfda3829e70d7eec]
topics:      /camera/color/camera_info             1043 msgs    : sensor_msgs/CameraInfo     
             /camera/color/image_raw/compressed    1043 msgs    : sensor_msgs/CompressedImage
             /joint_states                         6808 msgs    : sensor_msgs/JointState     
             /tf                                  10162 msgs    : tf2_msgs/TFMessage          (4 connections)
             /tf_static                               2 msgs    : tf2_msgs/TFMessage          (2 connections)

I also played the bag and captured the frequency of the /tf messages:

➜  zaubot rostopic hz /tf
subscribed to [/tf]
average rate: 194.732
    min: 0.000s max: 0.049s std dev: 0.01277s window: 184

200Hz is more than enough, so the error you report above does not make sense.

[miguelriemoliveira]

It could be that the different transformations are published at different frequencies, one being much slower. To inspect this I looked into the tf tree.

This tree has a problem. It must be fully connected. In the xacro for the zau you should have a transforms to connect all groups in the tree.

[miguelriemoliveira]

So my feeling is that the zaubot drivers have some problems. Where is the code with the zau xacro (for the real robot)?

[miguelriemoliveira]

@JorgeFernandes-Git , please use the ATOM repo for the issues to do with calibration. Its better to have all the calibration related issues there. I created an issue in ATOM to mirror this one:

https://github.com/lardemua/atom/issues/570

And will close this one.

[miguelriemoliveira]

BTW, I noticed you have all your issues in zaubot and not in ATOM. It would be nice to move them to ATOM, even the ones that are closed. When we are together, lets try to transfer them to ATOM ok?

[JorgeFernandes-Git]

So my feeling is that the zaubot drivers have some problems. Where is the code with the zau xacro (for the real robot)?

I don't have the xacro file of the real system. I made my own based on the CAD drawings.

So we should create a joint connecting camera_link to ee_link on the real robot and record the bag?

[JorgeFernandes-Git]

@miguelriemoliveira

On this tree: https://user-images.githubusercontent.com/11350998/233836653-231ffba7-2cb3-413a-af00-c985aae7318c.png

Which node should be acquiring data?

camera_color_optical_frame or camera_color_frame?

[miguelriemoliveira]

Hi @JorgeFernandes-Git ,

Which node should be acquiring data?

camera_color_optical_frame or camera_color_frame?

Not sure that I understand the question. There are names of links, i.e. coordinate frames. The nodes of the tf tree.

Frames do not acquire data, only ROS nodes ... I am confused. Can you try to explain better? Thanks.