Strange results with robot_localization and navsat_transform_node

asked 2017-03-21 13:00:49 -0600

Marcus Barnet
277 ●63 ●74 ●83

updated 2017-05-04 05:02:13 -0600

Hi to all, hi @tom-moore

I finally solved all the problems about my system configuration and now I'm able to integrate my GPS data in ekf_localization_node by using the navsat_transform_node. I'm using two ekf_localization_node instances: the first which includes only continuous data (/imu/data, /husky_velocity_controller/odom) and the second which includes also the gps data (/imu/data, /husky_velocity_controller/odom, /fix).

The /odometry/gps topic publishes non-zero values, so I think it is working fine. The problem is that I obtain strange results in RVIZ and I can't understand the reason. It seems that the localization is not working properly, but I can't understand the reason.

In this test, the robot is moving forward, but RVIZ is showing random behavior.

This is my OLD recorded bag file, it contains only the related topics.

*EDIT 4 - May 04 2017 *

rosrun xsens_driver mtnode.py _frame_id:="base_imu"
[WARN] [WallTime: 1493887014.830545] Cannot find value for parameter: ~device, assigning default: auto
[WARN] [WallTime: 1493887014.832072] Cannot find value for parameter: ~baudrate, assigning default: 0
[WARN] [WallTime: 1493887014.833512] Cannot find value for parameter: ~timeout, assigning default: 0.002
[INFO] [WallTime: 1493887014.867422] Detected MT device on port /dev/ttyUSB0 @ 115200 bps
[INFO] [WallTime: 1493887014.867611] MT node interface: /dev/ttyUSB0 at 115200 bd.
[INFO] [WallTime: 1493887014.877234] Found parameter: ~frame_id, value: base_imu
[WARN] [WallTime: 1493887014.878095] Cannot find value for parameter: ~frame_local, assigning default: ENU

I used the compass on my Android device in order to align the IMU so the readings are not accurate at 100%, but they should give the idea about the output I get:

    **NORTH**: 
    X:  0.00150562881026,
    Y: 0.00796461943537, 
    Z: -0.605336606503, 
    W: 0.795928359032
    Degree: -74 degrees 



**EST**: 
X:  -0.00401881383732
Y: 0.00927101075649
Z: -0.929714620113
W: -0.368142217398
Degree: +136 degrees

**SOUTH**:
X:  0.00913885235786
Y: -0.0002562062582
Z: -0.989742994308
W: -0.142566725612
Degree: +163 degrees

**WEST**:
X:  0.00891534984112
Y: -0.00214970414527
Z: -0.877696692944
W: 0.479128926992
Degree: -122 degrees


----------

**NORTH**: 
X:  0.00407045986503
Y: 0.0111895436421
Z: 0.44247585535
W: 0.896631300449
Degree: +52 degrees  


**WEST**: 
X:  -0.00214476278052
Y: 0.0111895436421
Z: -0.225669115782
W: 0.974137365818
Degree: -26 degrees


**SOUTH**:
X: 0.0114099625498
Y: 0.00554236397147
Z:  -0.679173827171
W: 0.733830630779
Degree: -85 degrees  


**EST**: 
X: 0.0114099625498
Y: 0.00554236397147
Z:  0.0801176950336
W: 0.996705293655
Degree: +9 degrees

Output by using the IMU software:

North: -108 degrees
WEST: -40 degrees
SOUTH: +38 degrees
EST: +135 degrees

END EDIT 4

EDIT3 - May 01 2017

This screenshot has been made by using this bag file and this launch file.

During this test, the robot was commanded to follow a straight path.

I expect to see the odometry/gps and the /husky_velocity_controller/odom to be parallel (or coincident) to the /odometry/filtered/global and /odometry/filtered/local output.

Fixed frame: map

Pink: /husky_velocity_controller/odom

Red: /odometry/gps

Green: /odometry/filtered/global

Blue: /odometry/filtered/local

Figure 1: results with odom and map frame changed like in ... (more)

edit retag flag offensive close merge delete

Comments

Can you please be more descriptive about what exactly is not right about the behavior? Maybe post a video, and say what you are expecting to see vs. what you are seeing? Apologies, it's just that viewing bag files can be very time-consuming.

Tom Moore ( 2017-04-11 04:36:54 -0600 )edit

Thanks for your help. I upload a video on Youtube. In the real test, the robot was moving straight forward for few meters while on RVIZ I see that it is drifting randomly like if there was an error on the odometry calculation. I can't explain why.

Marcus Barnet ( 2017-04-11 07:38:16 -0600 )edit

Pleases define "not working". Produce an rviz image (not video) like I did below. Tell me what each color represents, and please tell me what the "Fixed Frame" is in rviz.

Tom Moore ( 2017-05-01 08:38:38 -0600 )edit

I added the details as a new answer in order to make it readable.

Marcus Barnet ( 2017-05-01 08:57:01 -0600 )edit

Much clearer, and I have answers for you, but can you first make the view orthographic for me (TopDownOrtho setting under Views)? It will help a bit.

Tom Moore ( 2017-05-01 09:00:30 -0600 )edit

I also note that there are two sets of green arrows: one aligns precisely with the blue arrows (they are "mixed" together), and the other is slightly offset.

Tom Moore ( 2017-05-01 09:01:49 -0600 )edit

I edited the answer in order to add the TopDownOrtho view. /odometry/filtered/global is affected by some jumps during the execution in RVIZ and this results in the green arrows with the offset close to the green/blue mixed arrows.

Marcus Barnet ( 2017-05-01 09:13:45 -0600 )edit

If you watch this video you can see the jumps of the /odometry/filtered/global with the green arrows.

Marcus Barnet ( 2017-05-01 09:16:19 -0600 )edit

see more comments

answered 2017-04-11 09:03:37 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

updated 2017-05-04 03:22:26 -0600

The behavior you're seeing is not drift, and isn't random. The sudden jumps in your pose are from the GPS. As for the path not being straight, have you plotted your raw GPS points? This is what they look like:

image description

The robot starts on the left side and moves to the right. You can see there the GPS points are quite noisy and jump around a lot, especially in the middle (the largest single jump is over 3 meters laterally).

Here is the output I get with your launch file and bag. Red is odom EKF, blue is the map EKF, yellow is the GPS odometry. The first image is in the odom frame:

image description

The second image is in the map frame:

image description

The issue in this case is that your IMU does not agree with your GPS tracks. Your GPS tracks clearly show that the robot had a heading of around +8 degrees with respect to East, and -82 degrees with respect to North. However, the IMU, which I believe you said points 0 at North, is reporting between -14 and -30 degrees during the bag file.

EDIT in response to updates:

I generated the images above based on the last bag and launch file you produced. Note that the odom and map EKF instances are consistent. The only thing that is misbehaving is the location of the GPS odometry points. This is because your IMU itself or the settings you used in navsat_transform_node were wrong/not conformant. In other words, everything about that launch file was absolutely fine. The only thing wrong was the IMU data. You don't need to modify the launch configurations or tweak anything else from the launch file that I used. You just need to get your IMU to conform.

EDIT2 in response to updates:

That all looks correct to me. Explanations:

Why the husky wheel odometry doesn't match This is because you are integrating velocities in the EKF. The EKF starts at (0, 0) and integrates the Husky's velocity data. The Husky's wheel odometry will not have started at (0, 0), and isn't using an IMU, so the heading won't agree either. As long as they have the same general "shape," it's fine. In other words, it's completely normal for the wheel odometry to not agree with the EKF output when you are only fusing velocities.

Why the GPS odometry doesn't line up perfectly First, note that the direction and path shape do match for the GPS odometry data, so I think you've got the heading sorted out nicely. I think it looks pretty good; that static offset could just be error from the GPS itself. I would plot the raw lat/longs using Google Earth and the GPS Visualizer website.

Note that the GPS poses have no heading, so they are all facing a heading of 0.

EDIT3

Your IMU data is still not correct. Here is a plot of your ... (more)

Comments

Thanks for your help, Tom! I fixed my GPS problem, so now there are no jumps. I added EDIT2 on my first message with a new video. I did another test, but I'm still having the same results even if the GPS is more accurate and I also changed the IMU with another one and I placed it better.

Marcus Barnet ( 2017-04-12 07:40:57 -0600 )edit

I performed this new test with another IMU (XSENS mti-300) which is better than the previous one and now the new IMU topic is /imu/data. This is my launch file, when I run it I have no errors about tf or other stuff, but I still have the odom problem.

Marcus Barnet ( 2017-04-12 07:44:14 -0600 )edit

I added a photo which shows how the sensors (GPS and IMU) are placed on the robot. Can this be a problem related to the tf? The second edit provides accurate GPS data, but the problem is still there.

Marcus Barnet ( 2017-04-18 11:31:09 -0600 )edit

If I remove the GPS, then the robot seems to move correctly in RVIZ even if the result of /husky_velocity_controller/odom is not aligned with the local and global results. I can't understand why this happens.

Marcus Barnet ( 2017-04-23 15:05:52 -0600 )edit

I'm still struggling to find a solution to my problem. I hope you can help me, Tom! :) I can't obtain a good result even if the GPS is accurate now.

Marcus Barnet ( 2017-04-30 14:48:58 -0600 )edit

I compared the X and Y coordinates between /odometry/filtered/local, /odometry/filtered/global and /odometry/gps and they are all different! is it a normal behavior?

Marcus Barnet ( 2017-04-30 15:14:41 -0600 )edit

Yes, it's normal. The first is in the odom frame and the second is in the map frame. When you display them (the first two) in rviz, they ought to align, however, because rviz is doing the transform. /odometry/gps ought to be near /odometry/filtered/global, but not exactly the same.

Tom Moore ( 2017-05-01 03:28:31 -0600 )edit

If /odometry/gps moves in a very different direction to /odometry/filtered/global, then you have something wrong with your IMU setup. Make sure it adheres to the standards in the wiki.

Tom Moore ( 2017-05-01 03:29:45 -0600 )edit

see more comments

Strange results with robot_localization and navsat_transform_node

EDIT3 - May 01 2017

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Strange results with robot_localization and navsat_transform_node edit

EDIT3 - May 01 2017

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Strange results with robot_localization and navsat_transform_node