canTransform timeout problem: RTAB RGBD with ZED on Jetson TX1

[heimsi]

Hello,
I'm trying to use the RGBD version of RTAB on a Jetson TX1 with a ZED Cam. the ZED is running VGA resolution at 15fps. I using the exactly the steps indicated in the Handheld Mapping Tutorial .

After  /rtabmap/rgbd_odometry subscribes to its topics , I get a seemingly stable odometry, with quality number of around 300. From this I conclude that all TF's and topics should be fine. But as soon as the /rtabmap/rtabmapviz node is started, warnings and subsequent errors start to pop up (lines marked below with starting with @@@@@'s ) saying that the transform from camera_link to zed_depth_frame arrived too late.  I tried to increase the parameters wait_for_transform and queue_size, but the errors would stay and e.g. for  a increased value of wait_for_transform = 0.5, canTransform would just return after 0.502345...

Does anybody know what could be the problem here? Is it maybe a performance problem on the Jetson?

Hereafter the output from the rosconsole:

[ INFO] [1498727546.848731083]: Starting node...
[ INFO] [1498727546.889448767]: Initializing nodelet with 4 worker threads.
[ INFO] [1498727546.958097819]: Initializing nodelet with 4 worker threads.
[ INFO] [1498727547.495351875]: Starting node...
[ INFO] [1498727547.807523233]: /rtabmap/rtabmap(maps): grid_cell_size             = 0.050000
[ INFO] [1498727547.807607504]: /rtabmap/rtabmap(maps): grid_incremental           = false
[ INFO] [1498727547.807660785]: /rtabmap/rtabmap(maps): grid_size                  = 0.000000
[ INFO] [1498727547.807701461]: /rtabmap/rtabmap(maps): grid_eroded                = false
[ INFO] [1498727547.807742190]: /rtabmap/rtabmap(maps): grid_footprint_radius      = 0.000000
[ INFO] [1498727547.807788961]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1498727547.807829377]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1498727547.807872554]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1498727547.807906356]: /rtabmap/rtabmap(maps): map_negative_poses_ignored = false
[ INFO] [1498727547.807940001]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1498727547.807977345]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1498727547.808021667]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1498727547.814690423]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1498727547.814782245]: /rtabmap/rtabmap(maps): octomap_occupancy_thr      = 0.500000
[ INFO] [1498727547.982161075]: rtabmap: frame_id      = camera_link
[ INFO] [1498727547.982246126]: rtabmap: map_frame_id  = map
[ INFO] [1498727547.982303470]: rtabmap: tf_delay      = 0.050000
[ INFO] [1498727547.982354980]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1498727547.982398886]: rtabmap: odom_sensor_sync   = false
[ INFO] [1498727548.007399961]: rtabmapviz: Using configuration from "/home/ubuntu/.ros/rtabmap_gui.ini"
[ INFO] [1498727549.146655693]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1498727549.150887018]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
[ INFO] [1498727549.927320651]: 
/rtabmap/rgbd_odometry subscribed to (exact sync):
   /camera/rgb/image_rect_color,
   /camera/depth/depth_registered,
   /camera/rgb/camera_info
[ INFO] [1498727550.196709393]: Odom: quality=0, std dev=0.010000m|0.010000rad, update time=0.049775s
[ INFO] [1498727550.320157957]: Odom: quality=368, std dev=0.003575m|0.001298rad, update time=0.118949s
[ INFO] [1498727550.409371291]: Odom: quality=358, std dev=0.004696m|0.001817rad, update time=0.084627s
[ INFO] [1498727550.491424832]: Odom: quality=353, std dev=0.004143m|0.001541rad, update time=0.077552s
[ INFO] [1498727550.571873905]: Odom: quality=348, std dev=0.004586m|0.001872rad, update time=0.075809s
[ INFO] [1498727550.673369289]: Odom: quality=355, std dev=0.006122m|0.002537rad, update time=0.097961s
[ INFO] [1498727550.784316169]: Reading parameters from the ROS server...
[ INFO] [1498727550.787028703]: Odom: quality=347, std dev=0.003021m|0.001110rad, update time=0.108831s
[ INFO] [1498727550.873159507]: Odom: quality=362, std dev=0.004188m|0.001409rad, update time=0.081666s
[ INFO] [1498727550.947772266]: Odom: quality=349, std dev=0.003306m|0.001231rad, update time=0.070634s
[ INFO] [1498727551.024747301]: Odom: quality=338, std dev=0.005660m|0.002259rad, update time=0.073007s
[ INFO] [1498727551.101052861]: Odom: quality=363, std dev=0.003964m|0.001607rad, update time=0.072387s
[ INFO] [1498727551.181420632]: Odom: quality=344, std dev=0.004050m|0.001395rad, update time=0.073045s
[ INFO] [1498727551.279132710]: Odom: quality=352, std dev=0.004273m|0.001703rad, update time=0.094239s
[ INFO] [1498727551.379615288]: Odom: quality=351, std dev=0.005105m|0.002074rad, update time=0.091673s
[ INFO] [1498727551.453863103]: Odom: quality=345, std dev=0.005856m|0.002367rad, update time=0.070673s
[ INFO] [1498727551.536146694]: Odom: quality=353, std dev=0.006360m|0.002468rad, update time=0.078723s
[ INFO] [1498727551.649104237]: Odom: quality=350, std dev=0.002249m|0.000916rad, update time=0.099946s
[ INFO] [1498727551.759257634]: Odom: quality=344, std dev=0.003304m|0.001349rad, update time=0.106027s
[ INFO] [1498727551.872754549]: Odom: quality=371, std dev=0.005217m|0.002139rad, update time=0.107742s
[ INFO] [1498727551.977230537]: Odom: quality=344, std dev=0.004191m|0.001695rad, update time=0.101590s
[ INFO] [1498727552.077546710]: Odom: quality=325, std dev=0.004375m|0.001696rad, update time=0.096228s
[ INFO] [1498727552.172861409]: Odom: quality=354, std dev=0.002991m|0.001204rad, update time=0.087140s
[ INFO] [1498727552.261458185]: Odom: quality=347, std dev=0.002912m|0.001232rad, update time=0.072426s
[ INFO] [1498727552.335519646]: Odom: quality=357, std dev=0.004103m|0.001613rad, update time=0.070256s
[ INFO] [1498727552.402279023]: Odom: quality=347, std dev=0.004394m|0.001615rad, update time=0.063200s
[ INFO] [1498727552.468216635]: Odom: quality=343, std dev=0.005011m|0.001932rad, update time=0.062399s
[ INFO] [1498727552.539452075]: Odom: quality=335, std dev=0.003969m|0.001502rad, update time=0.067622s
[ INFO] [1498727552.613198799]: Odom: quality=342, std dev=0.005282m|0.002041rad, update time=0.069816s
[ INFO] [1498727552.687618695]: Odom: quality=353, std dev=0.003750m|0.001511rad, update time=0.070496s
[ INFO] [1498727552.763441029]: Odom: quality=355, std dev=0.004315m|0.001728rad, update time=0.072256s
[ INFO] [1498727552.839790235]: Odom: quality=353, std dev=0.003191m|0.001048rad, update time=0.072633s
[ INFO] [1498727552.925200055]: Odom: quality=352, std dev=0.004698m|0.001944rad, update time=0.081528s
[ INFO] [1498727553.036401360]: Odom: quality=340, std dev=0.001781m|0.000765rad, update time=0.107545s
[ INFO] [1498727553.134558226]: Odom: quality=342, std dev=0.004572m|0.001643rad, update time=0.093322s
[ INFO] [1498727553.144060660]: Parameters read = 0
[ INFO] [1498727553.243787202]: Odom: quality=339, std dev=0.005439m|0.001917rad, update time=0.104524s
[ INFO] [1498727553.341633862]: Odom: quality=349, std dev=0.005467m|0.002166rad, update time=0.091443s
[ INFO] [1498727553.437988188]: Odom: quality=349, std dev=0.003736m|0.001456rad, update time=0.089829s
[ INFO] [1498727553.540829813]: Odom: quality=337, std dev=0.004797m|0.001977rad, update time=0.097112s
[ INFO] [1498727553.638675847]: Odom: quality=343, std dev=0.005674m|0.002285rad, update time=0.079714s
[ INFO] [1498727553.737010889]: Odom: quality=358, std dev=0.004862m|0.001827rad, update time=0.092874s
[ INFO] [1498727553.831632415]: Odom: quality=344, std dev=0.004040m|0.001583rad, update time=0.089577s
[ INFO] [1498727553.949783932]: Odom: quality=361, std dev=0.003806m|0.001465rad, update time=0.108909s
[ INFO] [1498727554.054603668]: Odom: quality=336, std dev=0.004441m|0.001135rad, update time=0.099453s
[ INFO] [1498727554.060099515]: /rtabmap/rtabmapviz: queue_size    = 10
[ INFO] [1498727554.060178629]: /rtabmap/rtabmapviz: rgbd_cameras = 1
[ INFO] [1498727554.060217379]: /rtabmap/rtabmapviz: approx_sync   = false
[ INFO] [1498727554.060300868]: Setup depth callback
[ INFO] [1498727554.158480650]: Odom: quality=332, std dev=0.004049m|0.001066rad, update time=0.099636s
[ INFO] [1498727554.252282651]: Odom: quality=330, std dev=0.004997m|0.001158rad, update time=0.089601s
[ INFO] [1498727554.347163498]: Odom: quality=338, std dev=0.003774m|0.010000rad, update time=0.090748s
[ INFO] [1498727554.439961130]: Odom: quality=337, std dev=0.004667m|0.001744rad, update time=0.088036s
[ INFO] [1498727554.512756402]: Odom: quality=345, std dev=0.005149m|0.002116rad, update time=0.069562s
[ INFO] [1498727554.586253024]: Odom: quality=349, std dev=0.005782m|0.002411rad, update time=0.067957s
[ INFO] [1498727554.722350562]: Odom: quality=349, std dev=0.003970m|0.001680rad, update time=0.126799s
[ INFO] [1498727554.723643157]: 
/rtabmap/rtabmapviz subscribed to (exact sync):
   /rtabmap/odom,
   /camera/rgb/image_rect_color,
   /camera/depth/depth_registered,
   /camera/rgb/camera_info,
   /rtabmap/odom_info
[ INFO] [1498727554.723990238]: rtabmapviz started.
[ INFO] [1498727554.830173766]: Odom: quality=342, std dev=0.005381m|0.002151rad, update time=0.090815s
[ INFO] [1498727554.923388062]: Odom: quality=344, std dev=0.004480m|0.001635rad, update time=0.088828s
[ INFO] [1498727555.028173319]: Odom: quality=338, std dev=0.003312m|0.000978rad, update time=0.086031s
[ INFO] [1498727555.111285915]: Odom: quality=339, std dev=0.004786m|0.001589rad, update time=0.078949s
@@@@@@@@[ WARN] [1498727555.132867431]: Could not get transform from camera_link to /zed_depth_frame after 0.200000 seconds (for stamp=1498727554.779844)! Error=". canTransform returned after 0.20956 timeout was 0.2.".
@@@@@@@@[ERROR] [1498727555.132977222]: TF of received depth image 0 at time 1498727554.779844s is not set!
@@@@@@@@[ERROR] [1498727555.133056596]: Could not convert rgb/depth msgs! Aborting rtabmapviz update...
[ INFO] [1498727555.198572492]: Odom: quality=335, std dev=0.004450m|0.001708rad, update time=0.081769s
[ INFO] [1498727555.286908073]: Odom: quality=337, std dev=0.003212m|0.001221rad, update time=0.083309s
[ INFO] [1498727555.362583012]: Odom: quality=343, std dev=0.003969m|0.001578rad, update time=0.065401s
[ INFO] [1498727555.432179244]: Odom: quality=338, std dev=0.001842m|0.000793rad, update time=0.065948s
@@@@@@@@[ WARN] [1498727555.497574516]: Could not get transform from camera_link to /zed_depth_frame after 0.200000 seconds (for stamp=1498727555.134612)! Error=". canTransform returned after 0.208529 timeout was 0.2.".
@@@@@@@@[ERROR] [1498727555.497679828]: TF of received depth image 0 at time 1498727555.134612s is not set!
@@@@@@@@[ERROR] [1498727555.497736078]: Could not convert rgb/depth msgs! Aborting rtabmapviz update...
[ INFO] [1498727555.535956592]: Odom: quality=333, std dev=0.003764m|0.001216rad, update time=0.083851s
[ INFO] [1498727555.621435994]: Odom: quality=354, std dev=0.003836m|0.001440rad, update time=0.081343s
[ INFO] [1498727555.699719353]: Odom: quality=342, std dev=0.003590m|0.001428rad, update time=0.074328s
[ INFO] [1498727555.800014693]: Odom: quality=341, std dev=0.003459m|0.001421rad, update time=0.083546s
@@@@@@@@[ WARN] [1498727555.827846113]: Could not get transform from camera_link to /zed_depth_frame after 0.200000 seconds (for stamp=1498727555.496694)! Error=". canTransform returned after 0.206217 timeout was 0.2.".
@@@@@@@@[ERROR] [1498727555.827954446]: TF of received depth image 0 at time 1498727555.496694s is not set!
@@@@@@@@[ERROR] [1498727555.828020643]: Could not convert rgb/depth msgs! Aborting rtabmapviz update...
[ INFO] [1498727555.886598203]: Odom: quality=344, std dev=0.004177m|0.001460rad, update time=0.081452s
[ INFO] [1498727555.972174063]: Odom: quality=317, std dev=0.004372m|0.001734rad, update time=0.081268s
[ INFO] [1498727556.055207649]: Odom: quality=347, std dev=0.003236m|0.001171rad, update time=0.079075s
[ INFO] [1498727556.141719700]: Odom: quality=320, std dev=0.004119m|0.000983rad, update time=0.082620s
@@@@@@@@[ WARN] [1498727556.174942489]: Could not get transform from camera_link to /zed_depth_frame after 0.200000 seconds (for stamp=1498727555.832690)! Error=". canTransform returned after 0.202454 timeout was 0.2.".
@@@@@@@@[ERROR] [1498727556.175034519]: TF of received depth image 0 at time 1498727555.832690s is not set!
@@@@@@@@[ERROR] [1498727556.175085040]: Could not convert rgb/depth msgs! Aborting rtabmapviz update...

[matlabbe]

Hi,

It seems related to a fix done on May 17: https://github.com/introlab/rtabmap_ros/commit/26b69581a4d9a0c501b67e8ada17678c7381cd30

Either update/build from source latest rtabmap/rtabmap_ros code or modify zed ros wrapper nodelet so that depth image has RGB frame (replace /zed_depth_frame by /zed_current_frame here).

cheers,
Mathieu

[heimsi]

Hey matlabbe,

Sorry for not answering, your advice had solved the problem (I changed the code of the zed ros wrapper).

Thank you and your team for this great piece of software!

Kind regards,

heimsi