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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.405358445772746e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.4109673241976635e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.295969619620618e-08]
twist:
twist:
linear:
x: -0.00286938739009
y: -0.000428022874985
z: 1.40695738082e-05
angular:
x: 0.000964700710028
y: 5.33973325219e-05
z: 0.000463298812974
covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
Originally posted by zlacelle on ROS Answers with karma: 30 on 2019-06-13
Post score: 0
