robot_pose_ekf wrong output with "good input"
I'm not be able to understand why I get a wrong pose from robot_pose_ekf. I'm using a kinect on a table and connected to the workstation to find the turtlebot position orientation and publish the result in /vo. Kinect is tracking a pattern marker and find a pose. I want to use odometry and imu to have a better result and have a pose when kinect can't see the marker.
My issue is /vo, /odom, and imu/data has similar orientation but ekf find something very different and I can't understand why.
Here is my tf tree to see how I work:
/vo:
header: seq: 479 stamp:
secs: 1370526479
nsecs: 1198623 frame_id: odom child_frame_id: base_footprint pose: pose:
position:
x: 0.109848357611
y: 0.0184851177529
z: 0.00108856573807
orientation:
x: -0.0155068317083
y: 0.00236792210117
z: -0.726064768505
w: 0.687447367477 covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06]
/odom
header:
seq: 5144
stamp:
secs: 1370526478
nsecs: 985132932
frame_id: odom
child_frame_id: base_footprint
pose:
pose:
position:
x: 0.122778090562
y: 0.0180749352772
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.754709580223
w: 0.656059028991
covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
/imu/data:
header:
seq: 5154
stamp:
secs: 1370526479
nsecs: 318391084
frame_id: gyro_link
orientation:
x: 0.0
y: 0.0
z: -0.641532116852
w: 0.767096175878
orientation_covariance: [1000000.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 1e-06]
and robot_pose_ekf/odom:
header:
seq: 3340
stamp:
secs: 1370526479
nsecs: 618319034
frame_id: odom
pose:
pose:
position:
x: 0.0979321075308
y: -0.0259189421082
z: -0.00496891294622
orientation:
x: 3.68249858201e-06
y: -4.82461122176e-05
z: 0.970055184809
w: -0.242884614753
covariance: [5.493486503712613e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.493486503712613e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0987278074026108e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.877878440724317, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.877878440724317, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.493333772345698e-10]
And how I construct the odometry msg:
void readAndSendVo()
{
tf::TransformListener lisVo;
tf::StampedTransform transformVo;
nav_msgs::Odometry vo;
std ...
just a note: 'ntpdate -q' does a query only, and does not set the time. you need to 'sudo ntpdate <master IP>' in order to sync
some troubleshooting: remove /imu and /vo as ekf inputs. ie see if only /odom topic going through ekf will affect the output. then add /vo. then add /imu. (i'm suspecting /imu is creating the error.. just a hunch)
I did your new sync with same result. I can see the msg vo, imu and odom sensor activated but often vo goes deactivated and return activated quickly. I have also the msg: "Robot pose ekf diagnostics discovered a potential problem"and "No devices connected.... waiting for devices to be connected"
Often imu orientation is very different from odom orientation, is that normal ? And if no, how can I fix it ?