Fix memory corruption due to publisher-multithreading

[jk-ethz]

For us, the publishing in a separate thread was causing memory corruption and crashes that occurred only sporadically and when ICP was running for an extended period of time (> 1 h). This is likely due to the lack of a mutex mechanism, resulting in reading data to publish while data might be changing from the other thread.

Also, since the publisher thread gets spawned and destroyed on every loop cycle, the time gain was not noticable for us. If it is still desired, a more optimal way would probably be to use a thread that gets spawned once and then gets fed data to publish by a mutexed queuing mechanism (which the ROS publishers have probably built-in already anyway).

See ethz-asl/moma#149

[jelavice]

Thank you for fixing this! Apologies for not reviewing earlier.