ROS2 serial packets sent to teensy getting corrupted

edit

I've got a project where I have a Raspberry Pi running ROS2 Foxy communicating with a Teensy 4.1 over serial to control a quadruped. I'm encountering an issue where when sending serial packets (using Python + PySerial) to the teensy where the packets start to overlap and entangle with each other resulting in bad packets that get thrown out. Looking at the raw serial input on the teensy, it reads like one of two things are happening:

1. Serial packets are being sent asynchronously despite the codebase being synchronous in design
2. Each packet is being sent multiple times

I suspect that I've coded or configured a node incorrectly or some other part of the ROS system was incorrectly setup, but I don't have the experience to know where these issues could arise in a potentially misconfigured ROS project. The following is pseudocode for the node that manages the system:

wait for list of setpoints
if (list of setpoints received):
    for setpoint in list of setpoints:
        send setpoint
        wait for ack
        if ack reports failure:
            resend setpoint until ack reports success
        else continue

        request current position data
        wait for data
        if current position not at setpoint:
            request data again until setpoint reached
        else continue

Sending setpoints and requesting data used to use services, but I swapped them out with two pubsubs each thinking the issue had to do with the async service calls being used synchronously (using the method found here), but using pubsubs didn't resolve the issue.

One thing I tried that succeeded in avoiding the packet overlapping problem was to introduce a 100ms delay between sending a setpoint and requesting data. This gets past the issue, but makes the motion very choppy and slow which is unacceptable. The delay getting past the issue though made me think that there was a race condition somewhere where the serial port is being fought over by the two processes, so I added locks to the serial port so that only one process could use the port at a time. This also didn't resolve the issue.

The actual code can be viewed here:

Arduino controller

ROS node to interface to serial port

ROS node that governs/tracks the system

ROS node that generates list of setpoints to be executed

ROS interfaces

I realize there's a lot of information here to sort through, but any help or insight as to what might be causing these problems would be greatly appreciated.