IMU and GPS fusion without odom - robot_localization
I'm using ros Indigo, and trying to fuse IMU and GPS, I don't have any odom source. Reading forum and the roscon presentation about the robot_localization package, I tried the following configuration:
<launch>
<!-- gps and imu fusion -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<param name="magnetic_declination_radians" value="0.36878808"/>
<param name="zero_altitude" value="false"/>
<param name="publish_filtered_gps" value="true"/>
<param name="broadcast_utm_transform" value="true"/>
<param name="wait_for_datum" value="true"/>
<remap from="/gps/fix" to="/gps"/>
<remap from="/imu/data" to="/imu"/>
<remap from="/odometry/filtered" to="/odometry/map"/>
</node>
<!-- map frame -->
<node pkg="robot_localization" type="ekf_localization_node" name="map_transform" clear_params="true">
<param name="odom0" value="odometry/gps"/>
<param name="imu0" value="/imu"/>
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="2"/>
<param name="two_d_mode" value="true"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="map"/>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]</rosparam>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<rosparam param="odom0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="print_diagnostics" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="$(env HOME)/adroit_files/debug_ekf_localization.txt"/>
<remap from="odometry/filtered" to="/odometry/map"/>
</node>
<!-- odom frame -->
<node pkg="robot_localization" type="ekf_localization_node" name="odom_transform" clear_params="true">
<param name="imu0" value="/imu"/>
<param name="odom0" value="/odometry/map"/>
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="2"/>
<param name="two_d_mode" value="true"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]</rosparam>
<rosparam param="odom0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
true, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="print_diagnostics" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="$(env HOME)/debug_ekf_localization.txt"/>
</node>
</launch>
as result, I'm getting a drift that increases in time, and the vector is always perpendicular to robot.
This warning appear several times too
[ WARN] [1465502165.359329185, 20.510000000]: Could not obtain base_link-> transform. Will not remove offset of navsat device from robot's origin.
[ WARN] [1465502167.550982133, 21.310000000]: Could not obtain transform from to base_link. Error was Invalid argument passed to lookupTransform argument source_frame in tf2 frame_ids cannot be empty
I'm not using a real robot yet, for now I'm on gazebo, using hector_gazebo plugins for IMU and GPS.
IMU sample message:
header:
seq: 10464
stamp:
secs: 10
nsecs: 918000000
frame_id: base_link
orientation:
x: 0.019510418191
y: -0.00138183083974
z: -0.00971101353096
w: 0.999761536739
orientation_covariance: [2.6030820491461885e-07, 0.0, 0.0, 0.0, 2.6030820491461885e-07, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: -0.00310991562662
y: -0.00912178750215
z ...
How exactly did you get the gps plugin to work..?
added the plugin configuration at the topic, to big to post here
Hmmm...this may be an issue. Can you post a bag?
I generated without any node open, just the gazebo with the plugins https://www.dropbox.com/s/11tcvbdze5a...