search

JorgeFernandes-Git / zau_bot

4 stars 1 forks source link

Correcting the recorded bag file #21

Closed JorgeFernandes-Git closed 1 year ago

JorgeFernandes-Git commented 1 year ago

Hi @miguelriemoliveira.

The idea is to correct the camera transformations using a python script, generating a new bag to be used in the calibration.

If instead you could run the set initial estimate(https://github.com/lardemua/atom/issues/578), this could resolve our problem.

Bag file

Zau branch

cam2_frame

Should be pointing forward.

image

Should be aligned and not tilted.

miguelriemoliveira commented 1 year ago

Working on this ...

miguelriemoliveira commented 1 year ago

Hi @JorgeFernandes-Git ,

I added an example script to show you how you can generate a new bagfile from an original bagfile while changing the values of specific transforms.

I suggest that you now improve the script by adding the correct transformations and corresponding values.

Do you know what you must change?

Let me know if you need more help.

JorgeFernandes-Git commented 1 year ago

I'll try it later today. Thanks for the help.

JorgeFernandes-Git commented 1 year ago

Hi @miguelriemoliveira.

So far I have this on a new script:

https://github.com/JorgeFernandes-Git/zau_bot/blob/22b6516ceb25b9de68accd9db310b63ee8c170ae/zau_bot_bringup/scripts/test_fix_static_tfs.py#L66-L92

I'm going to test it now.

JorgeFernandes-Git commented 1 year ago

I've tried some experiments but without good results. The new bag is always fragmented.

Here is the current stage of the script, probably something is wrong:

https://github.com/JorgeFernandes-Git/zau_bot/blob/Zau_INESCTEC/zau_bot_bringup/scripts/test_fix_static_tfs.py

miguelriemoliveira commented 1 year ago

Hi @JorgeFernandes-Git ,

I am running your script and then playing the bag.

Then I run rqt and try to visualize the tf tree. I get an error:

Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame zau/base_link at time 1682696949.829705 according to authority /play_1684609794294057719
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp

So I think in your script we need to update the time stamp as well. I will try.

miguelriemoliveira commented 1 year ago

I just tested with the updating of stamps and it appears to work. I can see the tf tree and I get no error messages. @JorgeFernandes-Git can you try from your side to see if it fixes the problem?

JorgeFernandes-Git commented 1 year ago

Hi @miguelriemoliveira.

I was able to run the script and create a 10 seconds bag.

Bag testing copying from /tf_static to /tf

In rqt looks good. The tree is connected.

However, in rviz still gives the message:

[ WARN] [1684663708.126206824]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame zau/base_link (parent zau/base_footprint) at time 1682697209,129757 according to authority unknown_publisher
[ WARN] [1684663708.126373071]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame zau/base_laser_link_0 (parent zau/base_link) at time 1682697209,129757 according to authority unknown_publisher
[ WARN] [1684663708.126554950]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame zau/base_laser_link_1 (parent zau/base_link) at time 1682697209,129757 according to authority unknown_publisher
[ WARN] [1684663708.126743871]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame base_laser_link_3 (parent zau/base_link) at time 1682697209,129757 according to authority unknown_publisher
[ WARN] [1684663708.126910142]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame base (parent base_link) at time 1682697209,129757 according to authority unknown_publisher
[ WARN] [1684663708.127074333]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame ee_link (parent wrist_3_link) at time 1682697209,129757 according to authority unknown_publisher
[ WARN] [1684663708.127223571]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame tool0 (parent wrist_3_link) at time 1682697209,129757 according to authority unknown_publisher
[ WARN] [1684663708.127318179]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame zau/base_footprint (parent zau/odom) at time 1682697209,102262 according to authority unknown_publisher
[ WARN] [1684663708.127488607]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame cam_1_depth_frame (parent cam_1_link) at time 1682697209,134490 according to authority unknown_publisher
[ WARN] [1684663708.127627595]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame cam_1_depth_optical_frame (parent cam_1_depth_frame) at time 1682697209,134490 according to authority unknown_publisher
[ WARN] [1684663708.127771294]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame cam_1_color_frame (parent cam_1_link) at time 1682697209,134490 according to authority unknown_publisher
[ WARN] [1684663708.127928137]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame cam_1_color_optical_frame (parent cam_1_color_frame) at time 1682697209,134490 according to authority unknown_publisher
[ WARN] [1684663708.128077165]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame cam_2_depth_frame (parent cam_2_link) at time 1682697209,134490 according to authority unknown_publisher
[ WARN] [1684663708.128253376]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame cam_2_depth_optical_frame (parent cam_2_depth_frame) at time 1682697209,134490 according to authority unknown_publisher
[ WARN] [1684663708.128426536]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame cam_2_color_frame (parent cam_2_link) at time 1682697209,134490 according to authority unknown_publisher
[ WARN] [1684663708.128585979]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame cam_2_color_optical_frame (parent cam_2_color_frame) at time 1682697209,134490 according to authority unknown_publisher

I'm thinking of adding something like this in the second for loop of the code:

        if topic == '/tf_static': # ignore this topic, since we already have its messages
            continue

For now, I'm going to try this small bag in the calibration anyway and see the results.

JorgeFernandes-Git commented 1 year ago

Updates:

Using the first script to rotate the cameras still destroy the tf tree...

However, with the new bag I was able to launch the set initial estimate

image

Maybe with this I can correct the initial guess and proceed with the calibration. I'll be giving updates.

miguelriemoliveira commented 1 year ago

I think that warning is a problem but not responsible for the crash in set_initial_estimate. Check what I wrote also in https://github.com/lardemua/atom/issues/578.

JorgeFernandes-Git commented 1 year ago

Hi @miguelriemoliveira.

This is not related to the other script, so I'll continue on this issue.

So far I was able to generate this bad - BAG FILE

image

The tf tree is okay.

This bag was generated by playing the bag generated by the script dynamic_to_static_tf_rosbag, after that running this launch file with all the static tfs needed and then recording running rosbag record -a on top of it. Just experimenting. The results seem okay at first.

Running the configuration using this bag, works:

rosrun atom_calibration configure_calibration_pkg -n zau_full_calibration -utf

Set initial estimate also works.

Collect data gave the error:

Waiting for transformation from zau/base_link to zau/odom(max 3 secs)02050               
Traceback (most recent call last):83613   Duration: 18.014762 / 631.302050               
  File "/home/jorge/catkin_ws/devel/lib/atom_calibration/collect_data", line 15, in <module>
    exec(compile(fh.read(), python_script, 'exec'), context)
  File "/home/jorge/catkin_ws/src/atom/atom_calibration/scripts/collect_data", line 145, in <module>
    data_collector = DataCollectorAndLabeler(args, server, menu_handler)
  File "/home/jorge/catkin_ws/src/atom/atom_calibration/src/atom_calibration/collect/data_collector_and_labeler.py", line 177, in __init__
    self.abstract_transforms = self.getAllAbstractTransforms()
  File "/home/jorge/catkin_ws/src/atom/atom_calibration/src/atom_calibration/collect/data_collector_and_labeler.py", line 398, in getAllAbstractTransforms
    self.listener.waitForTransform(frame, self.world_link, now, rospy.Duration(3))
  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 2156418.259938716s into the future.  Requested time 1684853651.852202415 but the latest data is at time 1682697233.592263699, when looking up transform from frame [zau/odom] to frame [zau/base_link]
================================================================================REQUIRED process [collect_data-8] has died!
process has died [pid 61685, exit code 1, cmd /home/jorge/catkin_ws/devel/lib/atom_calibration/collect_data -s 0.5 -o ~/datasets/zau_full_calibration_vf -c /home/jorge/catkin_ws/src/zau_bot/Zau_INESCTEC/zau_full_calibration/calibration/config.yml -ssl lambda sensor_name: False --overwrite __name:=collect_data __log:=/home/jorge/.ros/log/7587aa7a-f99f-11ed-96ac-81355c016778/collect_data-8.log].
log file: /home/jorge/.ros/log/7587aa7a-f99f-11ed-96ac-81355c016778/collect_data-8*.log
Initiating shutdown!
================================================================================

If we can't fix the other bag, fix this can be an alternative. I don't know...

miguelriemoliveira commented 1 year ago

Hi @JorgeFernandes-Git , since https://github.com/lardemua/atom/issues/584 and https://github.com/lardemua/atom/issues/578 are fixed, perhaps this one is not needed anymore?

JorgeFernandes-Git commented 1 year ago

@miguelriemoliveira. Thanks a lot for all the help so far.

I managed to use the different scripts and repair the bag file.

So far the process was:

ATM the tf tree looks like this: image

I'm going to try the calibration now.

miguelriemoliveira commented 1 year ago

Look great. Thanks for detailing the solution.

miguelriemoliveira commented 1 year ago

Hi @JorgeFernandes-Git ,

Can you add your scripts to this repo?

That way we have all rosbag scripts in one place.

I will add you as a collaborator.

Thanks, Miguel

JorgeFernandes-Git commented 1 year ago

Hi @JorgeFernandes-Git ,

Can you add your scripts to this repo?

That way we have all rosbag scripts in one place.

I will add you as a collaborator.

Thanks, Miguel

Done :)