wrong diff_drive_controller pose calculation?

edit

Hallo,

I am using diff_drive_controller in my project. I wrote the hw interface which calculates the distance based on the ticks coming from the left and right wheels. I am sending absolut ticks (increased when moving forward, decreased when moving backwards). The ticks are coming from the robot (slave) left_wheel_ticks and right_wheel_ticks topics, while the hw interface is runing on a master pc and subscribed to these topics. Clocks are in sync. The calculated linear travel distance for each wheels seems to be ok and calculated well, but not the robot's travel distance (position in the odometry)

 void read(const ros::Duration &period) {
        double distance_left = (_wheel_ticks[0] * ((_wheel_diameter * M_PI) / _wheel_encoder_ticks));
        double distance_right = (_wheel_ticks[1] * ((_wheel_diameter * M_PI) / _wheel_encoder_ticks));

        pos[0] += linearToAngular(distance_left - last_dist_left);
        vel[0] += linearToAngular((distance_left - last_dist_left) / period.toSec());
        pos[1] += linearToAngular(distance_right - last_dist_right);
        vel[1] += linearToAngular((distance_right - last_dist_right) / period.toSec());

        last_dist_left = distance_left;
        last_dist_right = distance_right;
    }

double linearToAngular(const double &travel) const
  {
      return travel / _wheel_diameter;
  }

_wheel_encoder_ticks = 20 _wheel_diameter = 0.065

If I am pushing the robot by hand for 1 wheel rotation (20 ticks), I can see, that the left and right wheel (linear) distance is 20cm with my 6.5cm diameter wheels. My wheel raidus is 3.25cm, if I set the wheel_radius_multiplier to 2.0, it seems to be ok ... but I don't have 13cm wheels, but 6.5cm.

I would assume, that the position in the published odometry topic increases by 20cm / 1 wheel rotation, but it is around the half of it.

Why? What am I doing wrong?

Here is the params for the diff_drive_controller:

mobile_base_controller:
  type: "diff_drive_controller/DiffDriveController"
  left_wheel: ['base_lt_wheel_shaft_joint']
  right_wheel: ['base_rt_wheel_shaft_joint']
  publish_rate: 50
  #extra_joints:
  #  - name: <name_of_caster_wheel_joint>
  #    position: 0.01
  #    velocity: 0.0
  #    effort: 0.0
  #  - name: <name_of_caster_wheel_joint>
  #    position: 0.01
  #    velocity: 0.0
  #    effort: 0.0

  # Odometry covariances for the encoder output of the robot. These values should
  # be tuned to your robot's sample odometry data, but these values are a good place
  # to start
  pose_covariance_diagonal  : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
  twist_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
  estimate_velocity_from_position: false

  # pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
  # twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]

  cmd_vel_timeout: 0.25
  velocity_rolling_window_size: 2

  # Top level frame (link) of the robot description
  base_frame_id: base_footprint

  # Odometry fused with IMU is published by robot_localization, so
  # no need to publish a TF based on encoders alone.
  enable_odom_tf: true

  # Jetbot hardware does not provides wheel velocities
  # estimate_velocity_from_position: true

  # Wheel separation and radius multipliers
  wheel_separation: 0.12
  wheel_radius: 0.0325
  # wheel_separation_multiplier: 1.0 # default: 1.0
  # wheel_radius_multiplier    : 1.0 # default: 1.0

  # Velocity and acceleration limits for the robot
  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 0.1  # m/s
      has_acceleration_limits: true
      max_acceleration       : 0.05 # m/s^2
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 0.1  # rad/s
      has_acceleration_limits: true
      max_acceleration       : 0.6  # rad/s^2

Thanks!