* Diagnostics loop for Husky, not realtime safe */

* Add a Message to the Message queue. * Checks Message validity, and drops invalid messages. * Trims queue down to size if it gets too big. * @param m

* Instantiates the Message subclass corresponding to the * type field in raw message data. * @param input The raw message data to instantiate from

* Read data until an ack message is found. * Any data messages received by this function will be queued. * @return The next ack message, if one is re

* Copies message payload into a provided buffer. * @param buf The buffer to fill * @param buf_size Maximum length of buf * @return number of by

* Register diagnostic tasks with updater class */

Custom Callback Queue /////////////////////////////////////////////////////////////////////////// Custom callback queue thread

* Scale left and right speed outputs to maintain ros_control's desired trajectory without saturating the outputs */

* Husky reports travel in metres, need radians for ros_control RobotHW */

* Sets SOH, STX, length, and checksum so that this message becomes valid. */

* Opens a serial port with the default configuration * (115200 bps, 8-N-1), using the device specified in the constructor */

* Public function which makes sure buffered messages are still being read into * the internal buffer. A compromise between forcing a thread-based impl

* Removes the oldest Message from the Message queue and returns it. * All data waiting in the input buffer will be read and queued. * @return The old

* Prints a nice list of counter values */

* Register interfaces with the RobotHW interface manager, allowing ros_control operation */

* Wipes out counters */

* Get current encoder travel offsets from MCU and bias future encoder readings against them */

* Non-blocking message receive function. * !!! Absolutely not reentrant !!! * !!! Keeps internal static state !!! * @return A pointer to a dynamicall