GPS, IMU, and odometry fusion results zero output

asked 2020-08-12 17:42:06 -0500

SA92 gravatar image

Hi all and Tom Moore,

I am trying to fuse IMU ("/zed2/zed_node/imu/data"), GPS("/piksi/navsatfix_best_fix"), and visual odometry ("/zed2/zed_node/odom") data. To start and for the sake of simplicity I started with one EKF instance along with navsat node. Although it needs improvement, the overall behavior is OK (I guess! Please correct me if I am wrong). Here is the launch file for that:

   <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 base_link zed2_imu_link" />
   <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
     <param name="frequency" value="20"/>
     <param name="sensor_timeout" value="0.1"/>
     <param name="two_d_mode" value="true"/>
     <param name="odom_frame" value="odom"/>
     <param name="base_link_frame" value="base_link"/>
     <param name="world_frame" value="odom"/>
     <param name="transform_time_offset" value="0.0"/>
     <param name="odom0" value="/zed2/zed_node/odom"/>
     <param name="imu0" value="/zed2/zed_node/imu/data"/>
     <rosparam param="odom0_config">[true, true, false,
                                     false, false, true,
                                     true, true, false,
                                     false, false, true,
                                     false, false, false]</rosparam>
     <rosparam param="imu0_config">[false, false, false,
                                    false,  false,  true,
                                    false, false, false,
                                    false,  false,  true,
                                    false,  false,  false]</rosparam>
     <param name="odom0_differential" value="false"/>
     <param name="imu0_differential" value="false"/>
     <param name="odom0_relative" value="true"/>
     <param name="imu0_relative" value="false"/>
     <param name="print_diagnostics" value="true"/>
     <param name="odom0_queue_size" value="10"/>
     <param name="imu0_queue_size" value="10"/>
     <param name="odom1" value="/odometry/gps"/>
     <rosparam param="odom1_config">[true, true, false,
                                     false, false, false,
                                     false, false, false,
                                     false, false, false,
                                     false, false, false]</rosparam>
     <param name="odom1_differential" value="false"/>
     <param name="odom1_relative" value="true"/>
     <param name="print_diagnostics" value="true"/>
     <param name="odom1_queue_size" value="10"/>
   <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" output="screen">
     <param name="frequency" value="20"/>
     <param name="delay" value="3"/>
     <param name="magnetic_declination_radians" value="0"/>
     <param name="yaw_offset" value="-0.523599"/>
     <param name="zero_altitude" value="true"/>
     <param name="broadcast_utm_transform" value="true"/>
     <param name="publish_filtered_gps" value="false"/>
     <param name="use_odometry_yaw" value="false"/>
     <remap from="/imu/data" to="/zed2/zed_node/imu/data"/>
     <remap from="/gps/fix" to="/piksi/navsatfix_best_fix" />
     <remap from="/odometry/filtered" to="/odometry/filtered" />

A screenshot of RVIZ for the above launch file is here: RVIZ

The bag file that I am using is here: bagfile

Next, I wanted to extend the above launch file and use two instances of EKF (local/odom and global/map) as it is recommended by the documentation. However, all the odometries (/odometry/filtered/local, /odometry/filtered/global, /odometry/gps) output 0 with this warning:

[ WARN] [1597270000.787711668]: Could not obtain transform from base_link to map. Error was Lookup would require extrapolation into the past. Requested time 1597246048.676564455 but the earliest data is at time 1597269998.196213722, when looking up transform from frame [base_link] to frame [map]

[ WARN] [1597270000.787805375]: Could ...

edit retag flag offensive close merge delete