ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
TL;DR: Calibrate your IMU... stay away from big metal objects... make sure your magnetic declination and yaw have the correct value and sign... and don't touch any of the more advanced parameters like datums until you get the basics working... and don't trust Rviz, if in doubt plot it in Excel or similar.
I've solved the problem. But it was caused by a combination of different factors, some of which were due to key misunderstandings about the way robot_localization
handles reference frames and certain parameters. So I will document everything in full to try and I will update it with examples soon.
When I first began this escapade I identified 5 separate problems:
/map
frame at launch using the 'datum' parameter, navsat_transform
never calculated the position correctly (i.e. the origin is always a least a few meters from where it should be). However, when using rosservice call /datum <tab>
later, it appears to work. navsat_transform
to change the orientation of /map
: yaw offset, mag declination and heading. However nothing seemed to work. (/map
points to Magnetic North by default, I wanted it to point to True North)/imu/magnetic_field
data.ekf_localization
node which fuses odometry, imu and GPS data appeared to be seriously off as the odometry output didn't match reality or any other data inputs.The cause for each of these problems is as follows:
navsat_transform
is getting a bad transform when it subscribes to the /imu/data
topic to calculate the LL -> UTM transform, which then ruins the calculation of /map
's origin. Locked orientation: It is currently impossible for navsat_transform
to in any way change the orientation of the /map
frame. There are however three parameters that may appear to do so. These parameters do not change the visible orientation of the frame but instead add an angle offset to all the odometry that navsat_transform
reports in that frame. So you will never see the 'Y' axis of /map
point anywhere but Magnetic North. The parameters are:
/map
frame. However, the third parameter 'heading' does not affect the actual heading of the /map
frame, instead it simply assumes that your robot starts at that heading within the /map
frame.navsat_transform
can calculate where True North is for the purposes of the LL -> UTM conversion, it has no effect on the orientation of /map whatsoever but it does massively affect the odometry in that frame if you get it wrong (see below)./map
but WILL affect the odometry calculated in that frame. I keep saying that these parameters won't change the orientation, but add an angle to the odometry that navsat_transform
reports in that frame. By this I am referring to /odometry/gps
which is the key output from navsat_transform
that you want to feed to an instance of ekf_localization
if you want to fuse your GPS data with your IMU and odometry (read: wheel encoder) data.
So the most effective solution I have found to each of these problems is:
/odom
and /map
frame align in position and orientation, which will happen naturally if you run all your 'ekf_localizationand
navsat-transform` nodes at launch. /map
frame can't actually be changed with navsat_transform
so don't even try - otherwise you're in for a world of sadness and disappointment.Rviz: Avoid using Rviz to debug issues across different reference frames. The mental gymnastics ain't worth it and will confuse you further. For clarity, print the odometry in each reference frame to a .csv file and plot it in something like Excel. Use this command to output the contents of a ros topic to a .csv file:
rostopic echo -b filename.bag -p /topic > output.csv
Bad EKF output: When running nodes from robot_localization
its best to start with the basics and avoid some of the more complicated parameters like datum
and imu0_relative`. Make sure you steer clear of any interference (metal objects for your IMU, LOS-blocking objects like trees or buildings for your GPS), ideally starting out in the middle of an open park or field. Also triple-check that you have the correct magnetic declination sign and yaw offset. If you're not sure, the easiest way to check is to conduct several test runs, each one changing a different parameter.
2 | No.2 Revision |
TL;DR: Calibrate your IMU... stay away from big metal objects... make sure your magnetic declination and yaw have the correct value and sign... and don't touch any of the more advanced parameters like datums until you get the basics working... and don't trust Rviz, if in doubt plot it in Excel or similar.
I've solved the problem. But it was caused by a combination of different factors, some of which were due to key misunderstandings about the way robot_localization
handles reference frames and certain parameters. So I will document everything in full to try and I will update it with examples soon.
When I first began this escapade I identified 5 separate problems:
/map
frame at launch using the 'datum' parameter, navsat_transform
never calculated the position correctly (i.e. the origin is always a least a few meters from where it should be). However, when using rosservice call /datum <tab>
later, it appears to work. navsat_transform
to change the orientation of /map
: yaw offset, mag declination and heading. However nothing seemed to work. (/map
points to Magnetic North by default, I wanted it to point to True North)/imu/magnetic_field
data.ekf_localization
node which fuses odometry, imu and GPS data appeared to be seriously off as the odometry output didn't match reality or any other data inputs.The cause for each of these problems is as follows:
navsat_transform
is getting a bad transform when it subscribes to the /imu/data
topic to calculate the LL -> UTM transform, which then ruins the calculation of /map
's origin. In addition, there is a bit of natural error in the IMU's heading, so this will only contribute to the problem.Locked orientation: It is currently impossible for navsat_transform
to in any way change the orientation of the /map
frame. There are however three parameters that may appear to do so. These parameters do not change the visible orientation of the frame but instead add an angle offset to all the odometry that navsat_transform
reports in that frame. So you will never see the 'Y' axis of /map
point anywhere but Magnetic North. The parameters are:
/map
frame. However, the third parameter 'heading' does not affect the actual heading of the /map
frame, instead it simply assumes that your robot starts at that heading within the /map
frame.navsat_transform
can calculate where True North is for the purposes of the LL -> UTM conversion, it has no effect on the orientation of /map whatsoever but it does massively affect the odometry in that frame if you get it wrong (see below)./map
but WILL affect the odometry calculated in that frame. I keep saying that these parameters won't change the orientation, but add an angle to the odometry that navsat_transform
reports in that frame. By this I am referring to /odometry/gps
which is the key output from navsat_transform
that you want to feed to an instance of ekf_localization
if you want to fuse your GPS data with your IMU and odometry (read: wheel encoder) data.
So the most effective solution I have found to each of these problems is:
/odom
and /map
frame align in position and orientation, which will happen naturally and
navsat-transform` nodes at launch. /map
frame can't actually be changed with navsat_transform
so don't even try - otherwise you're in for a world of sadness and Rviz: Avoid using Rviz to debug issues across different reference frames. The mental gymnastics ain't worth it and will confuse you further. For clarity, print the odometry in each reference frame to a .csv file and plot it in something like Excel. Use this command to output the contents of a ros topic to a .csv file:
rostopic echo -b filename.bag -p /topic > output.csv
Bad EKF output: When running nodes from robot_localization
its best to start with the basics and avoid some of the more complicated parameters like datum
and imu0_relative`. Make sure you steer clear of any interference (metal objects for your IMU, LOS-blocking objects like trees or buildings for your GPS), ideally starting out in the middle of an open park or field. Also triple-check that you have the correct magnetic declination sign and yaw offset. If you're not sure, the easiest way to check is to conduct several test runs, each one changing a different parameter.
3 | No.3 Revision |
TL;DR: Calibrate your IMU... stay away from big metal objects... make sure your magnetic declination and yaw have the correct value and sign... and don't touch any of the more advanced parameters like datums until you get the basics working... and don't trust Rviz, if in doubt plot it in Excel or similar.
I've solved the problem. But it was caused by a combination of different factors, some of which were due to key misunderstandings about the way robot_localization
handles reference frames and certain parameters. So I will document everything in full to try and I will update it with examples soon.
When I first began this escapade I identified 5 separate problems:
/map
frame at launch using the 'datum' parameter, navsat_transform
never calculated the position correctly (i.e. the origin is always a least a few meters from where it should be). However, when using rosservice call /datum <tab>
later, it appears to work. navsat_transform
to change the orientation of /map
: yaw offset, mag declination and heading. However nothing seemed to work. (/map
points to Magnetic North by default, I wanted it to point to True North)/imu/magnetic_field
data.ekf_localization
node which fuses odometry, imu and GPS data appeared to be seriously off as the odometry output didn't match reality or any other data inputs.The cause for each of these problems is as follows:
navsat_transform
is getting a bad transform when it subscribes to the /imu/data
topic to calculate the LL -> UTM transform, which then ruins the calculation of /map
's origin. In addition, there is a bit of natural error in the IMU's heading, so this will only contribute to the problem.Locked orientation: It is currently impossible for navsat_transform
to in any way change the orientation of the /map
frame. There are however three parameters that may appear to do so. These parameters do not change the visible orientation of the frame but instead add an angle offset to all the odometry that navsat_transform
reports in that frame. So you will never see the 'Y' axis of /map
point anywhere but Magnetic North. The parameters are:
/map
frame. However, the third parameter 'heading' does not affect the actual heading of the /map
frame, instead it simply assumes that your robot starts at that heading within the /map
frame.navsat_transform
can calculate where True North is for the purposes of the LL -> UTM conversion, it has no effect on the orientation of /map whatsoever but it does massively affect the odometry in that frame if you get it wrong (see below)./map
but WILL affect the odometry calculated in that frame. I keep saying that these parameters won't change the orientation, but add an angle to the odometry that navsat_transform
reports in that frame. By this I am referring to /odometry/gps
which is the key output from navsat_transform
that you want to feed to an instance of ekf_localization
if you want to fuse your GPS data with your IMU and odometry (read: wheel encoder) data.
So the most effective solution I have found to each of these problems is:
/map
frame can't actually be changed with navsat_transform
so don't even try - otherwise you're in for a world of sadness and disappointment. Rviz: Avoid using Rviz to debug issues across different reference frames. The mental gymnastics ain't worth it and will confuse you further. For clarity, print the odometry in each reference frame to a .csv file and plot it in something like Excel. Use this command to output the contents of a ros topic to a .csv file:
rostopic echo -b filename.bag -p /topic > output.csv
Bad EKF output: When running nodes from robot_localization
its best to start with the basics and avoid some of the more complicated parameters like datum
and imu0_relative`. Make sure you steer clear of any interference (metal objects for your IMU, LOS-blocking objects like trees or buildings for your GPS), ideally starting out in the middle of an open park or field. Also triple-check that you have the correct magnetic declination sign and yaw offset. If you're not sure, the easiest way to check is to conduct several test runs, each one changing a different parameter.
TL;DR: look at the graphs below.
Basically, all of this boils down to making sure that every sensor input you're trying to fuse is being reported in the same reference frame, or at least in reference frames with the same orientation. The mag declination and yaw offset parameters in navsat_transform exist purely to make sure that the UTM coordinates of the robot can be correctly converted into an odometry stream with the correct orientation. My problem, was that I didn't have these parameters correct, so the /odometry/gps stream was reported at an angle. My system may not be configured correctly, because according to REP-103 and REP-105, the /odom and /map reference frames should be orientated with their Y axis facing Magnetic North and their X axis facing Magnetic East. And indeed, this was the assumption I was operating under and even though I checked this at the time but got it wrong.
As it turns out, for my setup, for whatever reason, the /odom and /map frames align orient themselves at initialization to point with the X axis facing North, and their Y axis facing West. So even though on paper I had the correct mag declination and yaw offset, they didn't produce a odometry stream that matched everything else. This is definitely wrong, but as long as you're consistent with all you angular offsets, everything should come out in the wash (I hope - it has so far). Basically, if you're ever in doubt as to what should be the correct values, do what I did.
The yaw offset will (almost certainly) be either 90 degrees or 0. Your mag declination (if you've got the correct angle fro your region) will be negative or positive. This gives you four possible combinations. SO, drive your robot in a square, log the data from each odometry stream, plot it in excel and just use whatever combination produces the correct graph. E.G:
If you get the sign on the mag declination correct but the yaw offset incorrect, you get:
If you get the sign on the mag declination incorrect but the yaw offset correct, you get:
If you get both incorrect, you get: (Which, annoyingly looks better than getting just one wrong)
And finally, if you get the combination right, everything should line up like so:
4 | No.4 Revision |
TL;DR: Calibrate your IMU... stay away from big metal objects... make sure your magnetic declination and yaw have the correct value and sign... and don't touch any of the more advanced parameters like datums until you get the basics working... and don't trust Rviz, if in doubt plot it in Excel or similar. Also, if you're having a similar issue and you're in a rush, maybe just skip to the graphs at the bottom of this essay and try to replicate them.
I've solved the problem. But it was caused by a combination of different factors, some of which were due to key misunderstandings about the way robot_localization
handles reference frames and certain parameters. So I will document everything in full to try and I will update it with examples soon.
When I first began this escapade I identified 5 separate problems:
/map
frame at launch using the 'datum' parameter, navsat_transform
never calculated the position correctly (i.e. the origin is always a least a few meters from where it should be). However, when using rosservice call /datum <tab>
later, it appears to work. navsat_transform
to change the orientation of /map
: yaw offset, mag declination and heading. However nothing seemed to work. (/map
points to Magnetic North by default, I wanted it to point to True North)/imu/magnetic_field
data.ekf_localization
node which fuses odometry, imu and GPS data appeared to be seriously off as the odometry output didn't match reality or any other data inputs.The cause for each of these problems is as follows:
navsat_transform
is getting a bad transform when it subscribes to the /imu/data
topic to calculate the LL -> UTM transform, which then ruins the calculation of /map
's origin. In addition, there is a bit of natural error in the IMU's heading, so this will only contribute to the problem.Locked orientation: It is currently impossible for navsat_transform
to in any way change the orientation of the /map
frame. There are however three parameters that may appear to do so. These parameters do not change the visible orientation of the frame but instead add an angle offset to all the odometry that navsat_transform
reports in that frame. So you will never see the 'Y' axis of /map
point anywhere but Magnetic North. The parameters are:
/map
frame. However, the third parameter 'heading' does not affect the actual heading of the /map
frame, instead it simply assumes that your robot starts at that heading within the /map
frame.navsat_transform
can calculate where True North is for the purposes of the LL -> UTM conversion, it has no effect on the orientation of /map whatsoever but it does massively affect the odometry in that frame if you get it wrong (see below)./map
but WILL affect the odometry calculated in that frame. I keep saying that these parameters won't change the orientation, but add an angle to the odometry that navsat_transform
reports in that frame. By this I am referring to /odometry/gps
which is the key output from navsat_transform
that you want to feed to an instance of ekf_localization
if you want to fuse your GPS data with your IMU and odometry (read: wheel encoder) data.
So the most effective solution I have found to each of these problems is:
/map
frame can't actually be changed with navsat_transform
so don't even try - otherwise you're in for a world of sadness and disappointment. Rviz: Avoid using Rviz to debug issues across different reference frames. The mental gymnastics ain't worth it and will confuse you further. For clarity, print the odometry in each reference frame to a .csv file and plot it in something like Excel. Use this command to output the contents of a ros topic to a .csv file:
rostopic echo -b filename.bag -p /topic > output.csv
Bad EKF output: When running nodes from robot_localization
its best to start with the basics and avoid some of the more complicated parameters like datum
and imu0_relative`. Make sure you steer clear of any interference (metal objects for your IMU, LOS-blocking objects like trees or buildings for your GPS), ideally starting out in the middle of an open park or field. Also triple-check that you have the correct magnetic declination sign and yaw offset. If you're not sure, the easiest way to check is to conduct several test runs, each one changing a different parameter.
TL;DR: look at the graphs below.
Basically, all of this boils down to making sure that every sensor input you're trying to fuse is being reported in the same reference frame, or at least in reference frames with the same orientation. The mag declination and yaw offset parameters in navsat_transform exist purely to make sure that the UTM coordinates of the robot can be correctly converted into an odometry stream with the correct orientation. My problem, was that I didn't have these parameters correct, so the /odometry/gps stream was reported at an angle. My system may not be configured correctly, because according to REP-103 and REP-105, the /odom and /map reference frames should be orientated with their Y axis facing Magnetic North and their X axis facing Magnetic East. And indeed, this was the assumption I was operating under and even though I checked this at the time but got it wrong.
As it turns out, for my setup, for whatever reason, the /odom and /map frames align orient themselves at initialization to point with the X axis facing North, and their Y axis facing West. So even though on paper I had the correct mag declination and yaw offset, they didn't produce a odometry stream that matched everything else. This is definitely wrong, but as long as you're consistent with all you angular offsets, everything should come out in the wash (I hope - it has so far). Basically, if you're ever in doubt as to what should be the correct values, do what I did.
The yaw offset will (almost certainly) be either 90 degrees or 0. Your mag declination (if you've got the correct angle fro your region) will be negative or positive. This gives you four possible combinations. SO, drive your robot in a square, log the data from each odometry stream, plot it in excel and just use whatever combination produces the correct graph. E.G:
If you get the sign on the mag declination correct but the yaw offset incorrect, you get:
If you get the sign on the mag declination incorrect but the yaw offset correct, you get:
If you get both incorrect, you get: (Which, annoyingly looks better than getting just one wrong)
And finally, if you get the combination right, everything should line up like so: