tf_tree not aligned with trajectory

[BoomFan]

Hi,

Thank you very much for your fantastic work.

When I was trying to run googleCartographer for my robot, I found that my tf tree does not align with the trajectory_node_list in RVIZ, unlike what your demo shows.

You may take a look at my screen shot here: https://drive.google.com/open?id=1aSLKyzCwZJ_hsQWVw-Db-ZLsv6pbJiXF

It confuses me a lot, so I update cartographer_ros into the latest version and post the configuration files online so that you can reproduce my trouble.

1, I uploaded my rosbag here: https://drive.google.com/open?id=16qh_OzVbFOqkddDD5Eb1pdY9Ylg3e3jT

2, I also run rosbag_validate to check my rosbag data. The result is posted here https://gist.github.com/BoomFan/5c75e857e10881a935c982203f18e883

3, My cartographer_ros brach was uploaded here https://github.com/BoomFan/cartographer_ros_for_rover.git

You may run my configuration by roslaunch cartographer_ros rover_offline_no_state_estimator.launch bag_filename:=${HOME}/Downloads/rover_lab_joy_20171205.bag

I know that my imu_frame is not located at the same position as my robot base_link and it is facing downward so it has a raw angle = 3.1416. Could this be a reason why tf_tree is not aligned with trajectory? BTW I also noticed that my map got flipped after the first few seconds. Does that also come from a down-facing IMU?

Thanks again for your time.

look forward to your reply.

[gaschler]

I'll try to give a quick, partial answer for now and hope that helps you to setup things.

Your validate results for the bag file look good.

Looking at your https://github.com/BoomFan/cartographer_ros_for_rover/blob/master/cartographer_ros/configuration_files/rover_no_state_estimator.lua I'd recommend not to change so many parameters at once. Yeah, you need to set the transform between base_link and imu_frame correctly. Your map should definitely not flip. Please disable IMU TRAJECTORY_BUILDER_2D.use_imu_data = false and try to tune the scenario without IMU first, only if this works well try to add IMU again with correct urdf settings.

(Also, for testing you could disable global SLAM MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0 even though I don't think this is the issue here.)

[BoomFan]

@gaschler Thanks for your reply!

I tried to turn off the use_imu_data and I also tried to set MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0, but none of these slove my tf_tree problem.

I'm pretty sure that my urdf file makes sense. So I take a look at this file https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/node.cc and I print out the variables in line 230. I found that *trajectory_state.published_to_tracking is always 0. Do you think that might be a problem? Do you mind pointing out where am I able to fix this issue, please? I tried to go to MapBuilderBridge::GetTrajectoryStates() but I am not sure what I can do over there.

[gaschler]

I checked your bag file with (the latest version of) cartographer_rosbag_validate, see below. The error says that topic "scan" has contradicting information about sensor time stamp and serialization time, by more than a second. Cartographer always uses the sensor time. Rviz may use the first one and extrapolate wildly, which explains your observation. I also looked at rqt_bag rover_lab_joy_20171205.bag. The IMU topic has the same time stamp contradiction. Also, the sensor data drops many frames between 30--35 seconds which is not ideal.

There no easy way to fix your input data other than recording it again properly, because you wouldn't know how to correct your time stamps (which one is correct).

E1213 17:43:33.715487 147045 rosbag_validate_main.cc:286] frame_id "lidar_link" on topic /scan has serialization time 1512509790.709794048 but sensor time 1512509788.816328785 differing by -1.89347 s.
E1213 17:43:33.716337 147045 rosbag_validate_main.cc:286] frame_id "lidar_link" on topic /scan has serialization time 1512509790.709823381 but sensor time 1512509788.841436233 differing by -1.86839 s.
E1213 17:43:33.716467 147045 rosbag_validate_main.cc:286] frame_id "lidar_link" on topic /scan has serialization time 1512509790.709847013 but sensor time 1512509788.866149514 differing by -1.8437 s.
I1213 17:43:33.969271 147045 rosbag_validate_main.cc:332] Time delta histogram for consecutive messages on topic "/imu/imu" (frame_id: "imu"):
Count: 6327  Min: 0.003908  Max: 0.014986  Mean: 0.010000
[0.003908, 0.005016)                            Count: 1 (0.015805%)    Total: 1 (0.015805%)
[0.005016, 0.006123)                       #    Count: 466 (7.365260%)  Total: 467 (7.381065%)
[0.006123, 0.007231)                            Count: 60 (0.948317%)   Total: 527 (8.329382%)
[0.007231, 0.008339)                            Count: 12 (0.189663%)   Total: 539 (8.519046%)
[0.008339, 0.009447)                            Count: 6 (0.094832%)    Total: 545 (8.613877%)
[0.009447, 0.010555)        ################    Count: 4991 (78.884148%)    Total: 5536 (87.498024%)
[0.010555, 0.011662)                      ##    Count: 754 (11.917180%) Total: 6290 (99.415207%)
[0.011662, 0.012770)                            Count: 32 (0.505769%)   Total: 6322 (99.920975%)
[0.012770, 0.013878)                            Count: 4 (0.063221%)    Total: 6326 (99.984192%)
[0.013878, 0.014986]                            Count: 1 (0.015805%)    Total: 6327 (100.000000%)
I1213 17:43:33.969413 147045 rosbag_validate_main.cc:332] Time delta histogram for consecutive messages on topic "/scan" (frame_id: "lidar_link"):
Count: 2557  Min: 0.019652  Max: 0.030235  Mean: 0.025048
[0.019652, 0.020710)                            Count: 3 (0.117325%)    Total: 3 (0.117325%)
[0.020710, 0.021768)                            Count: 1 (0.039108%)    Total: 4 (0.156433%)
[0.021768, 0.022827)                            Count: 4 (0.156433%)    Total: 8 (0.312867%)
[0.022827, 0.023885)                            Count: 16 (0.625733%)   Total: 24 (0.938600%)
[0.023885, 0.024943)                 #######    Count: 952 (37.231129%) Total: 976 (38.169731%)
[0.024943, 0.026002)            ############    Count: 1553 (60.735237%)    Total: 2529 (98.904968%)
[0.026002, 0.027060)                            Count: 20 (0.782167%)   Total: 2549 (99.687134%)
[0.027060, 0.028118)                            Count: 4 (0.156433%)    Total: 2553 (99.843567%)
[0.028118, 0.029177)                            Count: 2 (0.078217%)    Total: 2555 (99.921783%)
[0.029177, 0.030235]                            Count: 2 (0.078217%)    Total: 2557 (100.000000%)
I1213 17:43:33.969516 147045 rosbag_validate_main.cc:332] Time delta histogram for consecutive messages on topic "/rover/odometry" (frame_id: "odom"):
Count: 1262  Min: 0.025073  Max: 0.071399  Mean: 0.049998
[0.025073, 0.029706)                            Count: 3 (0.237718%)    Total: 3 (0.237718%)
[0.029706, 0.034338)                            Count: 5 (0.396197%)    Total: 8 (0.633914%)
[0.034338, 0.038971)                            Count: 29 (2.297940%)   Total: 37 (2.931854%)
[0.038971, 0.043603)                      ##    Count: 96 (7.606973%)   Total: 133 (10.538827%)
[0.043603, 0.048236)                   #####    Count: 323 (25.594296%) Total: 456 (36.133121%)
[0.048236, 0.052869)                 #######    Count: 465 (36.846275%) Total: 921 (72.979401%)
[0.052869, 0.057501)                    ####    Count: 235 (18.621237%) Total: 1156 (91.600632%)
[0.057501, 0.062134)                       #    Count: 67 (5.309033%)   Total: 1223 (96.909668%)
[0.062134, 0.066766)                            Count: 31 (2.456418%)   Total: 1254 (99.366089%)
[0.066766, 0.071399]                            Count: 8 (0.633914%)    Total: 1262 (100.000000%)

[gaschler]

Closing as answered, feel free to reopen if needed.

[BoomFan]

Thanks for your answer. My laser scanner is not installed on the same computer as cartographer is. So I guess this unsynchronized timing issue lead to my problem here.

Thanks again.