Ask Your Question
0

tf problems when combine robot_localization gps & navigation amcl stack

asked 2016-06-08 03:39:38 -0600

asimay_y gravatar image

updated 2016-10-24 08:32:36 -0600

ngrennan gravatar image

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:

 * node [/ekf_localization_global] publishing transform [odom] with parent [map] already published by node [/amcl]

 * node [/amcl] publishing transform [odom] with parent [map] already published by node [/ekf_localization_global]**

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.

how should i do to make all works well? please help me. thank you very much!

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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.

Tom Moore gravatar imageTom Moore ( 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

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

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-06-14 03:38:34 -0600

Tom Moore gravatar image

updated 2016-06-20 02:04:03 -0600

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:

image description

...and in the odom frame:

image description

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
child_frame_id: base_link
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)
edit flag offensive delete link more

Comments

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...

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

i update new info above, please help me, thank you!

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

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

asimay_y gravatar imageasimay_y ( 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.

Tom Moore gravatar imageTom Moore ( 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

asimay_y gravatar imageasimay_y ( 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...

asimay_y gravatar imageasimay_y ( 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?

Tom Moore gravatar imageTom Moore ( 2016-06-17 07:54:01 -0600 )edit

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

Tom Moore gravatar imageTom Moore ( 2016-06-17 07:54:40 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-06-08 03:39:38 -0600

Seen: 1,688 times

Last updated: Jun 20 '16