problem while building map

edit

hi

i am trying to build map , but when i move the robot in real life some it's take some time to realize where it is position what i need is when i teleop to the robot to move forward the robot model or the axis in the map move forward not after some time realize i order it to move forward.

information: i am using indigo and odoemtery is published in good shape i use arduino mega to connect motor and encoder and imu and every thing publishing correctly i calibriate the imu to publish when the robot don't move it's publish gyro 0 and xand y accel 0 and z 16520 . and when i publish the encoder data i use the following equation :

rpm_act1 = double((count1-countAnt1)601000)/double(time1000/386.8);

rpm_act2 = double((count2-countAnt2)601000)/double(time1000/386.8);

countAnt1 = count1;

countAnt2 = count2;

Encoder resolution: 1000 state changes per 3 wheel rotations.thats why time *1000/3 and gear ratio is 86.8:1

and i subscribe to this data using bas_controller node in this package with some modification. https://github.com/sungjik/my_persona...

and there is the odometery data when i move forward

frame_id: /odom

child_frame_id: /base_footprint

pose:

pose:

position: 

  x: 0.00118784888106

  y: -0.000174585926668

  z: 0.0

orientation: 

  x: 0.0

  y: 0.0

  z: -0.187005357211

  w: 0.982358893875

covariance: [1e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 1e-09, 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, 1e-09]

twist:

twist:

linear: 

  x: 0.0

  y: 0.0

  z: 0.0

angular: 

  x: 0.0

  y: 0.0

  z: 0.0

covariance: [1e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 1e-09, 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, 1e-09]

header:

seq: 348

stamp:

secs: 1466690997

nsecs: 215294884

frame_id: /odom

child_frame_id: /base_footprint

pose:

pose:

position: 

  x: 0.00118784888106

  y: -0.000174585926668

  z: 0.0

orientation: 

  x: 0.0

  y: 0.0

  z: -0.187005357211

  w: 0.982358893875

covariance: [1e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 1e-09, 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 ... (more)