I am using robot_pose_ekf, but I am getting incorrect output, and I'm not sure why.

edit

Hello,

I'm trying to get robot_pose_ekf working in ROS Indigo with a model I have in Gazebo, but I can't seem to get it to output the correct values. I have searched through documentation and various help threads for a while, and I can't seem to narrow down my problem.

I have a model in gazebo that has an differential drive controller using the plugin “libgazebo_ros_diff_drive.so”, a GPS sensor using “libhector_gazebo_ros_gps.so”, and an IMU sensor using “libgazebo_ros_imu.so”.

As far as I can tell, all of these sensors are producing correct values on their topics, so this led me to believe that my problem lies somewhere in my ROS code rather than anything with Gazebo.

Per this thread ( http://answers.ros.org/question/22972... ), I have set the output of robot_pose_ekf to the head of my tf tree and remapped the gazebo transform output to a different topic. Here is my current frame tree with the base_link, IMU, GPS, and my two laser sensors. The odom at the head is the odometry output from robot_pose_ekf:

For my GPS sensor, per this page ( http://wiki.ros.org/robot_pose_ekf/Tu... ) I remapped the input to the robot_pose_ekf vo topic. I have also written a node that converts the GPS coordinates to the Gazebo simulator coordinates in the local area of the Gazebo world I am using. So, if my robot model state is at (x=0,y=0), then the GPS coordinates are transformed and sent as (x=0,y=0). I plan to add some noise to that, but currently, it is very small in order to test.

Here is my launch file section that launches robot_pose_ekf:

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
       <param name="base_footprint_frame" value="base_link"/>
       <remap from="imu_data" to="irobot/imu_data"/>
       <remap from="vo" to="irobot/gps_odom"/>
       <remap from="odom" to="irobot/odom"/>
       <param name="output_frame" value="odom"/>
       <param name="freq" value="100.0"/>
       <param name="sensor_timeout" value="1.0"/>
       <param name="odom_used" value="true"/>
       <param name="imu_used" value="true"/>
       <param name="imu_absolute" value="true"/>
       <param name="vo_used" value="true"/>
       <param name="gps_used" value="false"/>
       <param name="debug" value="false"/>
       <param name="self_diagnose" value="true"/>
</node>

I have tested several scenarios described below where I switch vo_used, imu_used, and odom_used to true/false to hopefully give a better indication of the problem.

After I had odometry(with robot_pose_ekf subscribed to the odometry topic from the differential controller), IMU, and GPS setup, I performed several test. For the first test, I ran robot_pose_ekf with only odometry and IMU. As the robot moved, I got good output until the robot hits an obstacle and stops. At that point, the robot_ekf output shows the robot still moving forward. I also get the message “Robot pose ekf diagnostics discovered a potential problem”. I have looked into that per the description on the ROS robot_pose_ekf page, but I don't see a problem. The two sensors have nearly identical timestamps, and I assume this falls ... (more)