2 VIO inputs with robot_localization
I have a robot with 2 VIO cameras, I want to fuse the 2 odometries with robot_localization.
the odometry messages are reported in their respective frames which are correctly translated and rotated according to the sensor position and are children of "odom" frame.
The messages report a position starting from 0,0,0 in their frame
The robot_localization seems not to handle the frames correctly even if the Preparing Sensor Data guide of robot_localization says that:
All pose data is transformed from the message header's frame_id into the coordinate frame specified by the world_frame
parameter (set to "odom" in my case)
So the filtered odometry (base_link
) keeps jumping back and forth the two sources, seems much like the odometry poses are not transformed back in the odom frame before being fused.
Is that correct? How can I handle correctly the transform?
launch:
<node pkg="tf2_ros" type="static_transform_publisher" name="odom2front" args="0.4 0 0.02 0 0 0 1 odom front_vio_odom_frame" />
<node pkg="tf2_ros" type="static_transform_publisher" name="odom2back" args="-0.4 0 0.02 0 0 1 0 odom back_vio_odom_frame" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
<rosparam command="load" file="ekf_config.yml"/>
</node>
ekf_config.yml:
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
two_d_mode: false
publish_tf: true
# inputs
odom0: front_vio/odom/sample
odom1: back_vio/odom/sample
odom0_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
odom0_differential: false
odom0_relative: false
odom1_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
odom1_differential: false
odom1_relative: false
initial_state: [0, 0, 0
0, 0, 0,
0, 0, 0,
0, 0, 0]
dynamic_process_noise_covariance: true
Odometry message sample:
header:
seq: 1655
stamp:
secs: 1619171242
nsecs: 910251379
frame_id: "back_vio_odom_frame"
child_frame_id: "back_vio_pose_frame"
pose:
pose:
position:
x: 0.0742701664567
y: 0.00130270281807
z: 0.325136482716
orientation:
x: 0.0363613590598
y: -0.0208929777145
z: 0.0272305142134
w: 0.998749136925
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
twist:
twist:
linear:
x: 0.0470368626879
y: -0.024768557293
z: 0.0780361119745
angular:
x: -0.0763922602234
y: -0.0473806500658
z: -0.0343299482198
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
EDIT
I've tested the configuration suggested in the @tom 's answer ...