robot_localization not producing data while fusing Odometry
I have a robot_localization node, and for testing purposes I want to fuse in a very accurate GPS receiver as if it was a normal smooth odometry sensor (e.g. wheel encoders). I have a nav_msgs/Odometry topic producing an ENU-formatted UTM position, as well as a sensor_msgs/Imu topic filled with FLU-formatted rates and accelerations.
However, I get the following error, and robot_localization doesn't seem to produce data:
Could not obtain transform from gps_enu to odom. Error was "odom" passed to lookupTransform argument target_frame does not exist.
This doesn't make sense, because I'm running this node specifically to produce this transform!
EDIT: I've found the lines in robot_localization that are the culprit, but I'm not sure how to proceed. In ros_filter.cpp:
if (poseCallbackData.updateSum_ > 0)
{
// Grab the pose portion of the message and pass it to the poseCallback
geometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped();
posPtr->header = msg->header;
posPtr->pose = msg->pose; // Entire pose object, also copies covariance
geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
poseCallback(pptr, poseCallbackData, worldFrameId_, false);
}
For Odometry messages, it's taking the Pose part and attempting to transform it all the way to the "world" frame (which is odom).
END EDIT
EDIT #2: Switching on the "_differential" flag works because it integrates the pose as a twist instead. However, then my covariances grow with time, which I don't want (they should remain bounded, as the covariances of the poses are bounded).
END EDIT #2
Here's my configuration:
frequency: 50 sensor_timeout: 0.1 transform_time_offset: 0.1 transform_timeout: 0 odom_frame: odom base_link_frame: base_link world_frame: odom two_d_mode: false imu0: /sensor/imu/data_enu imu0_config: [false, false, false, false, false, false, false, false, false, true, true, true, false, false, false ] imu0_differential: false imu0_relative: false imu0_queue_size: 10 imu0_remove_gravitational_acceleration: false odom0: /sensor/gps/utm_enu odom0_config: [true, true, true, true, true, true, false, false, false, false, false, false, false, false, false] odom0_differential: false odom0_relative: true odom0_queue_size: 10 publish_tf: true publish_acceleration: true print_diagnostics: true debug: false debug_out_file: /mnt/ramdisk/relative_localization_filter_debug.txt use_control: false alpha: 0.001 kappa: 0 beta: 2
Here's an example output of each message:
---
header:
seq: 77976
stamp:
secs: 1560451949
nsecs: 823664380
frame_id: "imu_link_enu"
orientation:
x: -0.00890241184106
y: -0.0122976751383
z: 0.941373781502
w: -0.337023616012
orientation_covariance: [2.4716113409053997e-07, 0.0, 0.0, 0.0, 2.466269938791087e-07, 0.0, 0.0, 0.0, 7.467046920414323e-08]
angular_velocity:
x: -0.00106572255027
y: 0.00128244573716
z: -7.25421487004e-05
angular_velocity_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 0.0310777239501
y: 0.000748675898649
z: -0.0431123189628
linear_acceleration_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
---
header:
seq: 79161
stamp:
secs: 1560451990
nsecs: 483678060
frame_id: "gps_enu"
child_frame_id: "imu_link_flu"
pose:
pose:
position:
x: 308876.393439
y: 4310470.5311
z: -77.3288650513
orientation:
x: -0.00886148846847
y: -0.0123107011776
z: 0.941312561823
w: -0.337195168431
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0 ...
