[robot_localization] Non zero altitude in /gps/filtered when ~zero_altitude & ~two_d_mode are true
I am trying to use robot_localization to fuse data from a 9DOF IMU & a GPS, via one instance of ukf_localization_node & an instance of navsat_transform_node. I think my setup is more or less working, except that the messages published by the navsat_transform_node on /gps/filtered have a non zero altitude value. Altitude is not important for my application & having the altitude as 0 in these messages would make integration/debugging much easier for me.
The NMEA data from my GPS receiver includes altitude, so the messages on /fix contain this information & I can watch it fluctuate up & down as the receiver sits on my desk. The messages on /odometry/gps have a 0 altitude & the messages on /odometry/filtered have an almost 0 altitude (it fluctuates, but is always on the magnitude of E-14 or E-15). The altitude value in the /gps/filtered messages is fixed at what is presumably an old value from /fix (right now it is 48.4) & does not fluctuate at all as I move the IMU or as the altitude seen in /fix changes.
I have ~zero_altitude set to true in the navsat_transform_node & I have ~two_d_mode set to true in the ukf_localization_node. I have tried setting ~initial_state to specify Z as 0 but this had no effect.
Is this simply because ~zero_altitude only affects the Odometry messages on /odometry/gps & not the NavSatFix messages on /gps/filtered? Is there a solution?
<launch>
<!-- ======= ======= ======= ======= ======= ======= ======= -->
<!-- Specify the transform between the 'imu' frame (that IMU data are published on) & the base_link frame (that is rigidly affixed to the robot) -->
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 base_link imu" />
<!-- Specify the transform between the 'gps_link' frame (that GPS data are published on) & the base_link frame (that is rigidly affixed to the robot) -->
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_gps" args="0 0 0 0 0 0 1 base_link gps_link" />
<!-- ======= ======= ======= ======= ======= ======= ======= -->
<!-- subscribes to /nmea_sentence which is raw NMEA data from Arduino/node.js, converts to sensor_msgs/NavSatFix format & publishes on /fix -->
<node pkg="nmea_navsat_driver" type="nmea_topic_driver" name="nmea_topic_driver" respawn="true">
</node>
<!-- ======= ======= ======= ======= ======= ======= ======= -->
<!-- subscribes to /odometry/gps (GPS data as odometry from navsat_transform_node) & /imu/data (IMU data from Arduino/node) & publishes on /odometry/filtered -->
<node pkg="robot_localization" type="ukf_localization_node" name="ukf_localization_node" respawn="true" clear_params="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"/>
<param name="imu0" value="/imu/data" />
<!-- specifies what values the IMU data provides (order is position [F], orientation [T], velocity [F], angular velocity [T], linear acceleration [T] ) -->
<rosparam param="imu0_config">
[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
</rosparam>
<param name="imu0_differential" value="false"/>
<param name="odom0" value="/odometry/gps" />
<!-- specifies what values the odometry data provies (order is position [T], orientation [F]. velocity [F], angular velocity [F], linear acceleration [F] ) -->
<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 ...
please add your launch file again, but this time after pasting, select all lines and press the Preformatted Text button (the one with
101010
on it).Thankyou, my mistake was pressing the Preformatted Text button first & then pasting where it said 'enter code here'.