Data transfer to and from Arduino to Raspberry Pi
Ok, without rewriting all your code, one thing I'm seeing here is a lot of sloooow string functions.

Pythons print function is extremely slow, for example. You should insert some timers in your code and see how long everything's taking. I think you will be surprised.

For a real-time application, which it sounds like this is, you need to be very careful and use timeouts and non-blocking code to make sure that everything doesn't grind to a halt unexpectedly.

You really need to re-write all this code, but I'll point out a few specific problems.

delay(20); 
if (Serial.available()&gt;0){
   ...
}


This is sloppy. If your python code does not process and return data before the 20ms elapses everything is going to go out of sync.

str_data = Serial.readString();


A look at the documentation reveals that this will only return on timeout, so this is guaranteed to block for 1s (I think that's the default), so there's another show-stopper problem. See this question for more info.

cmd = str_data.substring(0, firstCommaIndex);


This is also not sufficient. There is no synchronization between the pi and the arduino, so you have no guarantee that you won't get half of a previous transmission tacked onto the front of your data string (or worse). You need to make your own buffer and parse for the datastring. Or do some careful buffer clearing and use generous timeouts.

So TLDR; here's what you want to do (assuming you want to block while this exchange happens)


Clear the arduino receive buffer.
Send data packet from arduino.
Start receiving. Add received bytes to a char buffer.
Continuously search that buffer for your complete data packet.
Timeout after x ms. Handle the lack of returned data somehow.
If you do find the complete data packet, then process it.


On the Pi:


Poll for serial data. Add it to a buffer as it comes in.
Start deleting the oldest characters from the buffer if it's longer than 2x the longest data packet
Continuously search the buffer for your data packet. If you find it then continue below.
delete the data packet from the receive buffer.
process the data.
Send out return data packet to arduino.