Revision history [back]

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());

probably don't need linearToAngular. Before doing this operation, the unit is set to meters. (This is the distance the robot has traveled.)

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

Also, since vel is supposed to be finding the velocity at that point, there is no need to add it.

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

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

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());

probably don't need linearToAngular. Before doing this operation, the unit is set to meters. (This is the distance the robot has traveled.)

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

Also, since vel is supposed to be finding the velocity at that point, there is no need to add it.

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

Update

Apparently, my understanding of linear velocity is wrong. The correct term seems to be angular velocity. I did some research and came across this. https://answers.gazebosim.org/question/21825/what-is-the-velocity-assigned-to-the-gazebo-ros-conrtol-controller/?answer=22000#post-id-22000

From what I learned there, I think the following should be changed.

// Divide the linear velocity by the radius, not the diameter.
double linearToAngular(const double &travel) const
{
  return travel / ( _wheel_diameter / 2 );
}

I think it is consistent with the case where setting wheel_radius_multiplier to 2.0 works correctly.

OLD

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());

probably don't need linearToAngular. Before doing this operation, the unit is set to meters. (This is the distance the robot has traveled.)

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

Also, since vel is supposed to be finding the velocity at that point, there is no need to add it.

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