Using robot_localization navsat_transform_node to fuse IMU and GPS measurements from a drone
Hi, I'm looking throuh the robot_localization package and I'm a bit confused on how to use the navsat_transform node to fuse measurements coming from a drone.
I have three topics:
dji_odom for odometry.
dji_imu for Imu data.
dji_gps for gps data.
which according to the tutorial are the sample inputs. My issue is I'm not quite sure where these inputs are meant to be in the package.
I'm using the sample dual_ekf_navsat_example
file which looks like:
<launch>
<rosparam command="load" file="$(find robot_localization)/params/dual_ekf_navsat_example.yaml" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
</launch>
Initally, I thought this was meant to be added in the dual_ekf_navsat_example.yaml. Mine looks like this:
ekf_se_odom:
frequency: 50
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: m100/base_link
world_frame: odom
odom0: dji_odom
odom0_config: [true, true, true,
true, true, true,
true, true, true,
true, true, true,
true, true, true]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
imu0: dji_imu
imu0_config: [true, true, true,
true, true, true,
true, true, true,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
//covariances removed for brevity
ekf_se_map:
frequency: 30
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
odom0: dji_odom
odom0_config: [true, true, true,
true, true, true,
true, true, true,
true, true, true,
true, true, true]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
odom1: dji_gps
odom1_config: [true, true, true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: false
odom1_relative: false
imu0: dji_imu
imu0_config: [false, false, false,
true, true, false,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
process_noise_covariance: // removed for brevity
initial_estimate_covariance: //removed for brevity
navsat_transform:
frequency: 30
delay: 3.0
magnetic_declination_radians: 0.013945181 # For lat/long 55.944831, -3.186998
yaw_offset: 0 # IMU reads 0 facing magnetic north, not east
zero_altitude: false
broadcast_utm_transform: true
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: false
But I get the following error: Client [/ekf_se_map] wants topic to have datatype/md5sum [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7], but our version has [sensor_msgs/NavSatFix/2d3a8cd499b9b4a0249fb98fd05cfa48]. Dropping connection.
which has to do with odom1
on ekf_se_map
which makes sense I guess. My question is where do you add the GPS data to fuse into the package?