Awesome, thank you so much! I will test it on the Teensy 4.0 sometime this week and let you know how it goes.
Yes, the 4.0 uses exactly the same MCU as the 4.1 so it should work just fine. There are a few minor differences in the Teensyduino code, for example fewer serial ports are enabled on the 4.0.
To integrate changes into the official Teensyduino, you might want to post a message about it on the Teensy Forum. Paul, the creator of Teensy, is very active on there. The Teensy community seems reluctant to use any RTOS, though, preferring bare-metal programming instead. Also most people on there are probably not familiar with ROS either, so you might want to provide a little background on why this may be interesting to them.
USB serial is a good way for the client to talk to the agent since the firmware gets loaded over USB and leaving it connected allows firmware updates. I have a project right now, though, that uses 16 Teensy boards in a distributed system and I am finding that many laptops and desktops cannot connect to that many boards simultaneously over USB, although it does work with a Raspberry Pi. I always thought hundreds of USB devices could be connected to a host computer, but that is not true in many cases.
Someday it might be interesting to allow clients to send messages to each other directly over something like CAN bus so more boards can be connected with more deterministic communication timing without the USB and agent delays.