Using the "write_item" service causes the driver to get stuck in an infinite loop of "package length not valid"

[Ben-Nivo]

Whenever we try to write any data item using the /write_item service of the tm_driver, the driver seems to get stuck in a infinite loop where it's constantly printing the message "package length not valid" to the terminal.

This seems to happen regardless of which item we try to write. Even the demo_write_item example script seems to trigger this behavior. However, the /set_io service does work as expected, so we are able to set I/O using that service even when sending the same command using the /write_item service fails. I haven't figured out yet why /write_item fails while /set_io works since as far as I'm aware both should essentially send the same TM Ethernet Slave commands. EDIT: Apparently /set_io uses the TmScript interface instead of the Ethernet Slave interface, so it's not using the same interface. So it's the Ethernet Slave interface that seems to have problems even for tags that can be written to using the TmScript interface.

This seems to prevent us from automatically starting the robot by writing to the "Stick_PlayPause". I've noticed that the driver also has a built-in way to send this automatically by setting the stick_play argument to true when instantiating the TmSrvRos2 class:

tm_ros2_composition_moveit.cpp line 99:

auto tm_svr = std::make_shared<TmSrvRos2(node, iface, is_fake);

replace this with:

auto tm_svr = std::make_shared<TmSrvRos2(node, iface, is_fake, true);//last argument is bool stick_play; setting it to true will trigger the Stick_PlayPause data item when the robot is connected

However, while this does write in the terminal that the data is sent, it still does not seem to actually cause the robot to start.

[Ben-Nivo]

I think this issue is related to thread synchronization. Using a mutex to lock the socket so that only a single thread can access it seems to already prevent the driver from getting stuck in that infinite loop, though some package errors still appear.