# tf problems when combine robot_localization gps & navigation amcl stack

hi, Dear all, I met tf problems when combine robot_localization + navsat + navigation amcl stack. the tf transforms seems are colliding with each other.

according to r_l instruction, I set ekf_template_local.yaml and ekf_template_global.yaml :

publish_tf: true

gps node migration works well, but tf shows collide when i run roswtf, following is error messages:

**ERROR TF multiple authority contention:



so I set ekf_template_global.yaml:

publish_tf: false

just only to let amcl module to publish tf(odom->map), but it often occurs the warnning info as below at last line:

process[ekf_localization_local-1]: started with pid [59323]
process[ekf_localization_global-2]: started with pid [59449]
process[navsat_transform_node-3]: started with pid [59596]
[ INFO] [1465373999.050320208, 1465356233.643751380]: Datum (latitude, longitude, altitude) is (30.587115, 103.987225, 452.160004)
[ INFO] [1465373999.050385885, 1465356233.643751380]: Datum UTM coordinate is (402897.868551, 3384282.066208)
[ INFO] [1465373999.050625052, 1465356233.654742509]: Initial odometry pose is Origin: (-6.2733690279887452945 -0.27920951194753762525 -0.029217864509362003606)   Rotation (RPY): (-0.0066287586623609510289, -0.0080349835834440021948, -0.00022201279494916720432)
[ INFO] [1465373999.050853030, 1465356233.654742509]: Corrected for magnetic declination of 0.000000 and user-specified offset of 0.000000. Transform heading factor is now -0.000222
[ INFO] [1465373999.050906508, 1465356233.654742509]: Transform world frame pose is: Origin: (-6.2733690279887452945 -0.27920951194753762525 -0.029217864509362003606)
Rotation (RPY): (-0.0066287586623609510289, -0.0080349835834440021948, -0.00022201279494916720432)
[ INFO] [1465373999.050948531, 1465356233.654742509]: World frame->utm transform is Origin: (3384148.7263204208575 -402893.25039371155435 29408.271071974020742)
Rotation (RPY): (-0.0066305423673257484971, -0.0080335117269795773554, 2.2864374053448203049e-09)
**[ WARN] [1465373999.084253427, 1465356233.686713222]: Could not obtain transform from map to base_link. Error was Could not find a connection between 'base_link' and 'map' because they are not part of the same tree.Tf has two or more unconnected trees.
[ WARN] [1465373999.084397737, 1465356233.686713222]: Could not obtain map->base_link transform. Will not remove offset of navsat device from robot's origin.
**


but the tf works fine, and the tf data is correct, although a little drift.

so to avoid the warning, i changed another method, i set ekf_template_global.yaml :

publish_tf: true

and change amcl.cfg, set:

<param name="tf_broadcast"    value="false"/>


when I run roswtf:

WARNING The following nodes are unexpectedly connected:
* /ekf_localization_global->/view_frames_61426_1465374081884 (/tf)


the tf collide warning disappeared, but the tf data is wrong, the odom and base_link's z axis data has big issue. tf data is bad.

launch file and bag file is at here:

1. launch files: https://www.dropbox.com/s/ahk7nw9kg1e...
2. rosbag file:
https://www.dropbox.com/s/htth1w41c58...

In bag file, I used following 4 topics:

1. lms1_scan
2. imu_topic
3. imu_odom_topic
4. imu_nav_topic

Thank you for all your help. I really appreciate your help! thanks!

update1:

1. bad effect of video record at here: https://youtu.be/S8ueiIQoQ_A
2. map file at here: https://www.dropbox.com/s/btd1nso7dlq...
3. params file of r_l at here: https://www.dropbox.com/s/zd81adtnx3g ...
edit retag close merge delete

This is quite a long question! I will take a look at it soon. In the meantime, I suggest you make sure all of your sensor data conforms to standards, and then add one sensor at a time to one EKF, verify the results, then add more. Don't just throw it all together at once.

( 2016-06-15 05:41:16 -0600 )edit

hi, dear@Tom Moore, I'm sorry for so long question. yes, I check data conform and confirm it is ENU frame, according to your wiki knowledge. and I tried many time step by step, one sensor and another integrate, I'm still failed, the odom drift very large at last about163sec, especially when UAV turn

( 2016-06-16 02:19:52 -0600 )edit

Sort by » oldest newest most voted

OK, I took a look at this, and you have a few things wrong:

1. Your IMU topic has an empty quaternion of all zeros. That's going to break navsat_transform_node, and is the reason you were seeing NaN values. Since your IMU odometry topic appears to have a working orientation, I used that instead. Note that you will need to add a value for magnetic_declination for the settings for navsat_transform_node for your location. I'm assuming the IMU has a magnetometer from which it derives its yaw, correct? If not, then navsat_transform_node won't work, as you need an earth-referenced yaw measurement.
2. Your base_link->nav_link transform has an orientation in it. Note that this is meaningless for a navsat device, as it mounting orientation doesn't affect its measurement. You only need the linear offset in the static transform. That is not documented, and I've updated navsat_transform_node to remove the orientation component of that transform before applying it.
3. Your IMU is clearly measuring acceleration due to gravity, but you didn't enable imu0_remove_gravitational_acceleration. You can read more about that parameter on the wiki. This means that you may experience drift, particularly in the Z axis.

I put together a launch and config file here. Note that it also automatically plays your bag file at 10x speed, though you will need to update the path to the file itself. You will need to download the latest r_l source for this to work. It worked just fine for me. Here's a screenshot of the data in the map frame:

...and in the odom frame:

If you see any remaining drift (like at the bottom of the odom frame image), then it's a result of your imu_odom topic, which is likely integrating accelerations onboard the IMU, in which case drift may be inevitable. If you feed the filter velocity data that is incorrect, it won't fix it for you. For example, right at the beginning of the bag file, here's what your imu_odom topic looks like:

header:
seq: 11903
stamp:
secs: 1465356225
nsecs: 286038297
frame_id: odom
pose:
pose:
position:
x: -93.3040241217
y: 8.20038348484
z: 0.0
orientation:
x: -0.0
y: 0.0
z: 0.0303902439879
w: -0.999538109864
covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
twist:
twist:
linear:
x: -2.91783475405
y: -0.091434393804
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0 ...
more

dear @Tom Moore, I want to set up awesome robot_localization package and with amcl, navigation stack to do outdoor UAV study. I have make a map with gmapping . my case is same as the question you instructed : http://answers.ros.org/question/21813...

( 2016-06-14 04:18:59 -0600 )edit

( 2016-06-14 04:21:17 -0600 )edit

sorry. type mistake. For UGV study. can you help me dear @Tom Moore?

( 2016-06-17 01:33:05 -0600 )edit

When I get some time, yes. As I said, this is quite a large question and is not something that will be trivial for me to look into. One thing that I notice is that you have something called "imu_odom_topic." Do you only have an IMU and a GPS? Also, turn off linear acceleration in your IMU config.

( 2016-06-17 04:04:16 -0600 )edit

dear @Tom Moore, Thank you very much for your answer. I really appreciate your help, really. Yes, I have only 1 equipment: INS/GPS, it can output IMU data & GPS data & velocity data, so I feed velocity data into imu_odom_topic to make a odometry data. Could you please tell me why turn off accelerati

( 2016-06-17 05:04:36 -0600 )edit

I turn off linear acceleration in both local and global EKF cfg, but it still seems too drift.. please see: https://www.dropbox.com/s/blixgcome5i...

( 2016-06-17 05:15:16 -0600 )edit

I'm guessing that's because your IMU odometry topic is using the accelerometers anyway. Have you tried just plotting the imu_odom topic?

( 2016-06-17 07:54:01 -0600 )edit

Also, do a rostopic echo on the imu_odom topic, and watch the linear velocities.

( 2016-06-17 07:54:40 -0600 )edit