Now, we can command the wheels of our robot using a joystick, publishing manually /cmd_vel topics or using navigation algorithms. Also, we can calculate the odometry of the vehicle and estimate its position; thanks to the encoder data. But have you checked whether your /sensor_velocity data is close to /cmd_vel command?

If you check our /cmd_vel command and the data from /sensor_velocity, you will see that they are not equal. You can try to set different gains in your /cmd_vel command functions to adjust the velocity command with your real velocity, but it is not the best way to do that. This solution will depend on your power supply, the weight of the vehicle, or the floor surface where you are driving the
robot among others.

It is time to go deeper and explore control algorithms. As we have a desired velocity for our robot, /cmd_vel and feedback from our encoder sensors, we can implement close-loop control algorithms to reach our /cmd_vel in our robot platform. For example, PID algorithms are commonly applied in these situations. So, try to do your own platform control algorithm in Arduino. If you have arrived here, you will be able to do it.

In the code repository, you will find an example of close-loop control algorithm with PID, just in case you want to try it without developing your own control algorithm.