0
$\begingroup$

Rosanswers logo

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/ahk7nw9kg1eysuc/launch.zip?dl=0
  2. rosbag file:
    https://www.dropbox.com/s/htth1w41c58bcvo/ugv_bag.zip?dl=0

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/btd1nso7dlqodcg/mymap.zip?dl=0
  3. params file of r_l at here: https://www.dropbox.com/s/zd81adtnx3g1bpq/params.zip?dl=0

I also find another question and do as @Tom Moore instruct, question is at: http://answers.ros.org/question/218137/using-robot_localization-with-amcl/ , I do as below:

  1. turn off map->odom tf transform in amcl;
  2. include amcl_pose topic and /odometry/gps topic (it is in map frame) as input to the second ekf_localization_global node;
  3. remap move_base odom topic to /odometry/filtered topic;

node graph pic is at here: https://www.dropbox.com/s/ygo9mqpd3zib67t/222.fw.png?dl=0

But still get very bad effect. Could you please instruct me? Thank you all very much. I really appreciate all your help!

Update 2:

  1. I re-check the origin data of imu_topic, imu_odom_topic, the data shows well and good, origin imu_odom_topic can draw good trajectory as I want.
  2. lauch r_l module and with navsat module, set
 <param name="use_odometry_yaw" value="false"/>

terminal occurs error info snapshot: https://www.dropbox.com/s/rxe4n9fmsiaq8a1/33.png?dl=0 shows nan value of odometry/gps topic.

  1. I returned to rviz, I find in tf, odom drift very huge and jump random, odom->map tf transfer in system maybe something wrong ? please help me . thanks!

video at here: https://youtu.be/r9PJH2DSbhs


Originally posted by asimay_y on ROS Answers with karma: 255 on 2016-06-08

Post score: 2


Original comments

Comment by Tom Moore on 2016-06-15:
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.

Comment by asimay_y on 2016-06-16:
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

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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

You have a linear velocity of nearly -3 meters per second in the X axis, so the state estimate is going to move backwards.

Anyway, I'll let you look at the launch and config files and work through it yourself. The best advice I can offer with amcl in the loop is to make sure you turn of amcl's tf broadcasting, and make sure its state estimate agrees with the GPS. If they diverge over time, you're going to see it jump back and forth rapidly between them.


Originally posted by Tom Moore with karma: 13689 on 2016-06-14

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by asimay_y on 2016-06-14:
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/218137/using-robot_localization-with-amcl/

Comment by asimay_y on 2016-06-14:
i update new info above, please help me, thank you!

Comment by asimay_y on 2016-06-17:
sorry. type mistake. For UGV study. can you help me dear @Tom Moore?

Comment by Tom Moore on 2016-06-17:
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.

Comment by asimay_y on 2016-06-17:
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

Comment by asimay_y on 2016-06-17:
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/blixgcome5i66gm/55.png?dl=0

Comment by Tom Moore on 2016-06-17:
I'm guessing that's because your IMU odometry topic is using the accelerometers anyway. Have you tried just plotting the imu_odom topic?

Comment by Tom Moore on 2016-06-17:
Also, do a rostopic echo on the imu_odom topic, and watch the linear velocities.

Comment by asimay_y on 2016-06-19:
hi, dear @Tom Moore, I tried just integrate imu_odom topic into EKF node, without accelerometer, but get even worse effect.. and I plot the imu_odom velocity, please snapshot: https://www.dropbox.com/s/8vywutd8x0nalm8/linear_image1.png?dl=0

Comment by asimay_y on 2016-06-19:
and accelerometer plot: https://www.dropbox.com/s/8fh75ovgri0nr0o/acc_image.png?dl=0 https://www.dropbox.com/s/8fh75ovgri0nr0o/acc_image.png?dl=0 effect in rviz: https://www.dropbox.com/s/a1xlwrqunzkgpcf/2222.png?dl=0

Comment by asimay_y on 2016-06-21:
dear @Tom Moore, Thank you very much for your help! I'm lucky, and god send you to help me. :) I read your comments again and again and carefully, and modify the related wrong config parameters, and finally get the result the same as you showed to me. The trajectory of base_link in map & odom

Comment by asimay_y on 2016-06-21:
The trajectory of base_link in map & odom are almost same and overlap(purple and green). But I have one more issue puzzled me: why odom origin of tf tree drift so much in map?(I removed acceleration integration of imu) Please see snapshot: https://www.dropbox.com/s/pmvqfvrsv0fxpqx/66.png?dl=0

Comment by asimay_y on 2016-06-21:
Thank you very much for your help!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.