robot_localization, odom topics frame_ids and tfs
I am trying to use ekf_localization_node
from robot_localization
to create a odom -> base_footprint
tf transform. For this I need other nodes (that capture raw odometry) to not publish this tf transform. But from these nodes I need various topics of types Odometry, Pose etc. And data in these topics must have a frame_id associated with them. The nodes providing these topics (gazebo right now, but plan to use turtlebot encoders, turtlebot gyroscope and visual odometry) set frame_id to odom there and apparently make some assumptions about the odom frame.
Is this a correct setup?
Do I need to set some other frame_id there?
What are best practices for this?
Won't these nodes mess up data when the odom transform is published by ekf_localization but not themselves?
Right now, my filtered odometry jumps around like crazy, while odometry from gazebo on which it is based is close to perfect.
Update
This is my ekf.yaml for configuration. All topics seem to be connected to where they should.
frequency: 30
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
publish_tf: true
publish_acceleration: false
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_footprint # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
odom0: /odom
# use vx, vy, vtheta
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1
#use vpitch, vroll, vyaw
imu0: mobile_base/sensors/imu_data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
true, true, true,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8 #
imu0_linear_acceleration_rejection_threshold: 0.8 #
imu0_remove_gravitational_acceleration: true
use_control: false
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# here go huge matrices, I did not change a thing from ekf_template.yaml
This is /odom
topic from gazebo:
header:
seq: 54094
stamp:
secs: 54
nsecs: 245000000
frame_id: "odom"
child_frame_id: "base_footprint"
pose:
pose:
position:
x: 0.638994661158
y: -0.00382325493651
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.524965953165
w: 0.851123227281
covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist:
twist:
linear:
x: -0 ...