diff_drive_controller odometry issues

edit

Hello everyone!

I have following issue with diff_drive_controller I really don't know how to solve. I have an sdf defined robot, which uses odometry published by the controller defined as follows:

<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
     <robotNamespace />
     <alwaysOn>true</alwaysOn>
     <updateRate>10</updateRate>
     <legacyMode>true</legacyMode>
     <leftJoint>right_wheel_joint</leftJoint>
     <rightJoint>left_wheel_joint</rightJoint>
     <wheelSeparation>0.5380</wheelSeparation>
     <wheelDiameter>0.2410</wheelDiameter>
     <torque>20</torque>
     <commandTopic>cmd_vel</commandTopic>
     <odometryTopic>odom</odometryTopic>
     <odometryFrame>odom</odometryFrame>
     <robotBaseFrame>chassis</robotBaseFrame>
     <publishWheelTF>false</publishWheelTF>
     <publishWheelJointState>false</publishWheelJointState>
     <rosDebugLevel>na</rosDebugLevel>
     <wheelAcceleration>0</wheelAcceleration>
     <wheelTorque>5</wheelTorque>
     <odometrySource>world</odometrySource>
     <publishTf>1</publishTf>
  </plugin>

The problem is that when robot starts navigating from the origin of the world, everything is fine, but when I slightly shift it away, the robot's odometry jumps and shows incorrect values. Video of shifted and properly placed robots is attached below: https://www.youtube.com/watch?v=vvPnN...

I really need the robot to be placed somewhere not in the origin. I would really appreciate any help and suggestions.

Here's the full .sdf configuration of my robot, if necessary:

<?xml version="1.0" encoding="UTF-8"?>

<sdf version="1.4"> <model name="amazon_warehouse_robot"> <static>false</static>

  <link name="chassis">
     <pose>3 0 .05 0 0 0</pose>
     <inertial>
        <inertia>
           <ixx>0</ixx>
           <iyy>0</iyy>
           <izz>0</izz>
           <ixy>0</ixy>
           <ixz>0</ixz>
           <iyz>0</iyz>
        </inertia>
        <mass>4.5</mass>
     </inertial>
     <collision name="collision">
        <geometry>
           <box>
              <size>.9 .675 .15</size>
           </box>
        </geometry>
     </collision>
     <visual name="visual">
        <geometry>
           <box>
              <size>.9 .675 .15</size>
           </box>
        </geometry>
        <material>
           <ambient>1 0.5 0 1</ambient>
           <diffuse>1 0.5 0 1</diffuse>
           <specular>0.1 0.1 0.1 1</specular>
           <emissive>0 0 0 0</emissive>
        </material>
     </visual>
     <collision name="caster_collision_front">
        <pose>0.40 0 -0.05 0 0 0</pose>
        <geometry>
           <sphere>
              <radius>.05</radius>
           </sphere>
        </geometry>
        <surface>
           <friction>
              <ode>
                 <mu>0.0</mu>
                 <mu2>0.0</mu2>
                 <slip1>0.0</slip1>
                 <slip2>0.0</slip2>
              </ode>
           </friction>
        </surface>
     </collision>
     <visual name="caster_visual_front">
        <pose>0.40 0 -0.05 0 0 0</pose>
        <geometry>
           <sphere>
              <radius>.05</radius>
           </sphere>
        </geometry>
     </visual>
     <collision name="caster_collision_back">
        <pose>-0.40 0 -0.05 0 0 0</pose>
        <geometry>
           <sphere>
              <radius>.05</radius>
           </sphere>
        </geometry>
        <surface>
           <friction>
              <ode>
                 <mu>0.0</mu>
                 <mu2>0.0</mu2>
                 <slip1>0.0</slip1>
                 <slip2>0.0</slip2>
              </ode>
           </friction>
        </surface>
     </collision>
     <visual name="caster_visual_back">
        <pose>-0.40 0 -0.05 0 0 0</pose>
        <geometry>
           <sphere>
              <radius>.05</radius>
           </sphere>
        </geometry>
     </visual>
  </link>

  <link name="left_wheel">
     <pose>3 0.30 0.05 0 1.5707 1.5707</pose>
     <collision name="collision">
        <geometry>
           <cylinder>
              <radius>.1</radius>
              <length>.05</length>
           </cylinder>
        </geometry>
     </collision>
     <visual name="visual">
        <geometry>
           <cylinder>
              <radius>.1</radius>
              <length>.05</length>
           </cylinder>
        </geometry>
     </visual>
  </link>

  <link name="right_wheel">
     <pose>3 -0.30 0.05 0 1.5707 1.5707</pose>
     <collision name="collision">
        <geometry>
           <cylinder>
              <radius>.1</radius>
              <length>.05</length> ...