Revision history [back]

After few hours of starring at my code I've found a problem in the motor controller node. The bug was in velocity in odom topic, it was computed as follows:

float vx = (odom_x - odom_last_x) / dt;
float vy = (odom_y - odom_last_y) / dt;
float vyaw = (odom_yaw - odom_last_yaw) / dt;

And should be:

float linear = (linear_right + linear_left) / 2.0;
float angular = (linear_right - linear_left) / track_width;

float vx = linear;
float vy = 0;
float vyaw = angular;