I pulled down your launch and bag file while I had DropBox access the other day, and am just now looking at them. Here are the problems I see. Note that I don't think I have your latest launch file, as the topic names appear to be very different.
Here is a sample IMU message from the bag file:
header:
seq: 756
stamp:
secs: 1439616291
nsecs: 130379310
frame_id: Imu_link
orientation:
x: 1.0
y: 0.0
z: 0.0
w: 0.0
orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 100.0]
angular_velocity:
x: 0.0
y: 0.0
z: -0.05
angular_velocity_covariance: [1e-06, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 1e-06]
linear_acceleration:
x: 0.0
y: 0.0
z: 0.0
linear_acceleration_covariance: [0.02, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.02]
First, the orientation appears to be the identity quaternion, but the identity quaternion should have x = 0.0 and w = 1.0. Second, if your IMU doesn't report orientation, navsat_transform_node will not function. See "Required Inputs" here. You'll need an earth-referenced heading, e.g., from a compass.
The launch file I'm looking at doesn't appear to show anything to do with the navsat_transform_node, but just looking at the error message above, I think you must have your GPS odometry configuration something like this:
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]</rosparam>
First, make sure that your odom0 setting really is for the GPS odometry. Next, note that the GPS odometry data only has pose information (X, Y, and optionally Z) in it. The configuration settings above are incorrect, because they have X velocity set to true. In your case, the reason I think you have something akin to the configuration above is that the transform failure was from an empty frame_id to base_footprint. The only time the filter attempts to transform into the body frame is when it's attempting to fuse a velocity. Your GPS odometry message has a blank child_frame_id (for the velocity component of the message), and the filter is trying to transform it into the body frame.
In any case, you should first fix your IMU message, and we'll go from there.
EDIT in response to the comments below
We'll pretend for a moment that you have a compass in your IMU so that I can address your question.
The use_odometry_yaw parameter, when set to true, renders the IMU input to navsat_transform_node unnecessary, yes. However, if you were to turn on differential mode or relative mode for your yaw sensor within ekf_localization_node, then you wouldn't be able to use the use_odometry_yaw setting, and you'd have to use your IMU directly with navsat_transform_node. This is because the yaw that you feed to navsat_transform_node must be earth-referenced. In other words, if you make your IMU face directly east, it should always read the same value (preferably 0, but navsat_transform_node lets you specify an offset).
Getting back to your IMU, this is the same problem that you will have. Your IMU reads zero when you first start its driver, regardless of whatever direction you're facing. In order to transform from an earth-referenced coordinate system (WGS-84 or UTM coordinates, as obtained from a GPS device) to your local (map or odom) frame, we need to know which way the robot is facing within that coordinate system at the start of execution. In other words, if your IMU only reports angular velocities, it won't work with navsat_transform_node. You'll get a result, but it won't be correct.
Re: the yaw jumping, I'd have to look at the bag file, which I will check out later tonight.
EDIT in response to question about yaw jumping
- Your
navsat_transform launch file as <param name="use_odometry_yaw" value="true" />. See the use_odometry_yaw description on the wiki. This setting should only be true if your heading coming out of the EKF/UKF is earth-referenced. You aren't feeding your mag data to the UKF, so this needs to be false for you, which will force the node to use the IMU data.
- You should have your Y velocity set to
true for your wheel encoder odometry. Otherwise, nothing is measuring it, so both yaw velocity and yaw itself will have massive variances, and that will have a knock-on effect of making the covariance values explode for all correlated variables.
- Your
navsat_transform launch files is using the wrong UKF output. You have <remap from="/odometry/filtered" to="/odom_ukf" />, but it should be <remap from="/odometry/filtered" to="/odom_gps" />.
I also still don't recommend feeding the output of one UKF into another. It may work just fine, but you're introducing an additional delay before the IMU and odometry data are fused with the map/gps instance of the UKF. Also, the frequency of your sensor data isn't so great that it should really tax your CPU to do the fusion twice.
Anyway, I've uploaded a zip file containing a filtered bag file (with only the raw sensor data in it) and the launch file I put together for your data. Everything appears to be working fine with it, so you can use it as a reference or adapt it to your own needs.
Originally posted by Tom Moore with karma: 13689 on 2015-08-19
This answer was ACCEPTED on the original site
Post score: 2
Original comments
Comment by dan on 2015-08-19:
oops! I swapped the ROS quat calculations for my own and forgot one was (w,x,y,z) and the other (x,y,z,w). Fixed. For navsat, I thought any one of the 3 sources listed under 1.(default) was sufficient, I will now supply all 3. I had GPS odom set to true for X, Y, and vx. I will set vx false.
Comment by dan on 2015-08-19:
launch and bag files updated--same folder. The warnings are gone (now that vx = false), but gps jumping remains. The position jumping may be just gps noise, but I'd like to fix the yaw jumping. I set "use_odometry_yaw" = true for navsat since the filtered yaw should be better than raw IMU yaw.
Comment by dan on 2015-08-19:
(cont) because the IMU only measures angular velocity, IMU yaw is derived from that. The yaw from robot_localization should be better as we add multiple sensors to the filter. Similarly, why would I supply yaw from both robot_localization and IMU to navsat when they both come from the same data?
Comment by dan on 2015-08-20:
I do have a compass on the IMU, but it is so easily led astray (local metal, motors starting) that I am reluctant to use it for absolute orientation. My thought was to get absolute orientation by tracking movement in GPS, as seems standard in phones and GPS devices.
Comment by Tom Moore on 2015-08-21:
But that heading is only obtained through differentiation of GPS positions, so you'll have to have a separate node that is listening to the GPS and computing an absolute heading, then feeding that heading to navsat_transform_node as an absolute heading reference (in an IMU message).
Comment by dan on 2015-08-22:
I am OK with writing a node that differentiates the GPS position to compute an absolute position if that is the best way to go. Did you get a chance to look at the bag files to see what is going on with the yaw jumping all around in odom_gps? Again, appreciate the help.
Comment by dan on 2015-08-30:
comm check on yaw jumping with GPS.
Comment by Tom Moore on 2015-08-30:
Your DropBox link is giving me 500 Internal Server errors at the moment.
Comment by dan on 2015-09-02:
Sorry, the share on that folder timed out. Use this link:
robot_data
Comment by Tom Moore on 2015-09-02:
No, I got the last one to work, and updated my answer (see the bit about "EDIT in response to yaw jumping").
Comment by dan on 2015-09-02:
I am confused. I remapped the odometry/filtered in navsat transform to odom_ukf because that is the odometry (sent from the first robot_localization) to be combined with the gps fix and the imu data to produce odom_navsat which feeds into the second robot_localization to produce odom_gps. No?
Comment by dan on 2015-09-02:
Also, since I have no absolute reference for the IMU, I was trying to use the values for X and Y coming out of robot_localization, but I get that these are not earth referenced. Could I use the datum parameter as an earth ref starting point and, based on that, get an earth ref from UKF?
Comment by dan on 2015-09-02:
FInally, the robot cannot move in the y direction (in the robot's frame), so vy is always = 0. Given that, don't understand why the Y velocity should be set equal to true. Is it better for it to be true and always fed a zero value than to be false? I do see that would help with the covariance.
Comment by dan on 2015-09-02:
I feed one UKF into another because I am using the output of the first one (odom_UKF) as input for processes that need a nice continuous value (like AMCL) while using the second one to improve accuracy via GPS. A smooth localization that includes GPS would be great, then I would use just one UKF.
Comment by dan on 2015-09-03:
ah, I see. Use primary data plus navsat into the gps robot_localization, not the feed from the first robot_localization, I didn't think of that :/ I am still confused about navsat. I thought it took in GPS, odom_UKF, IMU to output odom_navsat as an input into gps_robot_localization, but ....
Comment by dan on 2015-09-03:
it seems navsat actually takes the output of gps_robot_localization (odom_gps) to create odom_navsat. But then odom_navsat is an input to gps_robot_localization. So I don't get what is going on here.
Again, thanks for the help. The bag file with the launch file run well and help a LOT.
Comment by Tom Moore on 2015-09-04:
That circular relationship is necessary. The assumption you're making is that the only pose input to the second EKF should be the GPS. You may have cases where your robot drives around indoors for a while and accumulates odometry, and then drives outside.
Comment by Tom Moore on 2015-09-04:
In any case, it's a question of frames. The output of n_t_n is in the frame_id of its odometry input message. That means you would be feeding a message with a frame_id of odom to your map instance, and that can create other issues.
Comment by dan on 2015-09-04:
ah, ok, now I get it. I was really confused by the circular relationship.
Comment by Jasmin on 2016-11-21:
Hi Tom! I'm using the launch file you provided. My problem is that the navsat transform is not working properly: /odom_navsat is giving zeros and /odom_gps & /odom_ekf are giving exactly the same output. it seems like the gps data is not included in the pose estimation. What could be the problem?
Comment by modotz on 2017-01-18:
Hello dan,
I was trying to use your example, But when I execute the file I always get this error message:
[ WARN] [1484747931.570584811, 1440023599.548365613]: Could not obtain transform from /gps to base_footprint. Error was Invalid argument "/gps" passed to lookupTransform argument
Any help
Comment by Tom Moore on 2017-01-18:
You need to specify a base_link->gps transform using static_transform_publisher. You also should change the name of your GPS frame_id from "/gps" to "gps".
Comment by modotz on 2017-01-18:
Hi Tom,
Thanks for your reply.
I changed the frame_id as you said. But could you help me with, what you mean with to specify a base_link->gps transform?
Comment by Tom Moore on 2017-01-18:
Please ask a new question.
Comment by modotz on 2017-01-19:
Thanks for your reply,
I opened a new question --> click
