Hello,

I'm trying to get robotposeekf 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 “libgazeborosdiffdrive.so”, a GPS sensor using “libhectorgazeborosgps.so”, and an IMU sensor using “libgazeborosimu.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/229722/how-to-stop-gazebo-publishing-tf/), I have set the output of robotposeekf 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 baselink, IMU, GPS, and my two laser sensors. The odom at the head is the odometry output from robotpose_ekf:

For my GPS sensor, per this page (http://wiki.ros.org/robot_pose_ekf/Tutorials/AddingGpsSensor) I remapped the input to the robotposeekf 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 robotposeekf:

<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 voused, imuused, and odom_used to true/false to hopefully give a better indication of the problem.

After I had odometry(with robotposeekf subscribed to the odometry topic from the differential controller), IMU, and GPS setup, I performed several test. For the first test, I ran robotposeekf 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 robotekf 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 robotpose_ekf page, but I don't see a problem. The two sensors have nearly identical timestamps, and I assume this falls under the category of “very different information about robot pose” since the wheels seem to still be moving which causes the odometry to show the robot moving, yet the IMU shows no motion or acceleration.

The following are the (x,y) plots of the robot position according to the differential drive odometry (red), the odometry combined of robotposeekf (green), the GPS coordinates translated to (x,y) position from the node I wrote (blue), and the actual model position in Gazebo using getmodelstate (pink). Due to currently having little noise, the model state and GPS are overlapped on all the graphs.

As can be seen in the graph, the green line (robotposeekf) continues after the model stops in the simulator (purple,blue lines). While the odometry line (red) also continues due to the wheels spinning against the obstacle.

The next test I performed was with odometry, and GPS. IMU was deactivated. I set the GPS covariance to have a higher confidence than the odometry. This caused a new problem. Now the output for robotposeekf shows a fairly straight line with minimal changes when the robot turns. However, if it encounters an obstacle and stops, the robotposeekf registers this and the output stops at that point.

As can be seen in the graph, the robotposeekf (green) is giving the wrong robot location, but when it hits the obstacle, the green like stops like it should as can be seen by how the red odometry line continues moving.

Next I tried equal confidence in the sensors, and this produced a result somewhat halfway between the previous two. The robotposeekf output doesn't stop when it hits an obstacle, but it only slightly registers the positon changes when the robot turns so the robot drift is extremely large too.

In this graph we see that the robotekfoutput (green) follows slightly better on the turns, however, it continues plotting once the robot hits an obstacle as can be seen by where the purple line stops plotting.

I hope I have provided all of the relevant information. I'm not sure where I am going wrong with this, can anyone offer any suggestions?

Thank you.