navsat_transform broadcasting strange map->utm tf
Hello there !
I'm trying to fuse data from different sensors (IMU, GPS and a rotating lidar) into an extended Kalman filter provided by robot_localization in order to drive my rover using move_base. I'm running ros indigo on ubuntu 14.04.
With the right packages i was able to publish three topics for my three sensors :
- /gps/fix : sensor_msgs/NavSatFix
- /imu/data : sensor_msgs/Imu
- /pose_with_covariance_stamped : geometry_msgs/PoseWithCovarianceStamped (let's call it /pose)
Here's how my navsat_transform_node and ekf_localization node are set :
navsat_transform_node
- Input : /imu/data, /gps/fix, /odometry/filtered
- Output : /odometry/gps
ekf_gps
- Input : /imu/data, /pose, /odometry/gps
- Output : /odometry/filtered
According to REP105 and what I've read in this forum I think my frames are set correctly, see the tf tree below:
image: (in the link below if not working)
Now when i plot my topics in rviz i see /odometry/filtered drifting to infinity and beyond and i don't understand why ... Looking at my tf tree it looks like the map->utm transform is bugged, however i got this message in my navsat_transform terminal :
INFO[1470904382.578813798]: World frame->utm transform is Origin: (-4945736.2295890189707 677781.95941346196923 -470427.19763114472153) Rotation (RPY): (0.085877088202917686854, -0.066475908266541719471, -1.5762575291089457874)
My initial pose doesn't look right, as for my orientation i've offseted my imu and my robot is almost pointing north so that looks correct.
I have rosbag all my topics and put my launch files link:here or link:here , i'd be glad if you could have a look at it and give me a clue of what i'm doing wrong ! I made the rosbag while the robot wasn't moving and pointing almost north.
Thank you for your time.
EDIT 1
Correcting the coma to a dot in value="0,00436332" (in navsat.launch) didn't change the drifting