From c8d74b9b2b06ccd3faaba1090443ffba1e0689ab Mon Sep 17 00:00:00 2001
From: Jacob Panikulam <JPanikulam@ufl.edu>
Date: Mon, 3 Nov 2014 18:15:43 -0500
Subject: [PATCH] XMega: Add threading protection to prevent race conditions

---
 .../src/communication.py                      | 23 ++++++++++++++++++-
 1 file changed, 22 insertions(+), 1 deletion(-)

diff --git a/ros/ieee2015_xmega_driver/src/communication.py b/ros/ieee2015_xmega_driver/src/communication.py
index fadde2b..94ce5f4 100755
--- a/ros/ieee2015_xmega_driver/src/communication.py
+++ b/ros/ieee2015_xmega_driver/src/communication.py
@@ -13,6 +13,7 @@
 import rospy
 import tf.transformations as tf_trans
 import tf
+import threading
 
 # Ros Msgs
 from std_msgs.msg import Header, Float32, Float64, String
@@ -20,6 +21,24 @@
 from sensor_msgs.msg import Imu
 from ieee2015_xmega_driver.msg import XMega_Message
 
+lock = threading.Lock()
+
+def thread_lock(fcn):
+    '''thread_lock decorator
+
+    If you use this as a decorator for a function, it will apply a threading lock during the execution of that function,
+    which guarantees that no ROS callbacks can change the state of data while it is executing.
+
+    This is critical to make sure that a new message being sent doesn't cause a weird serial interruption
+
+    '''
+    def locked_function(*args, **kwargs):
+        lock.acquire()
+        result = fcn(*args, **kwargs)
+        lock.release()
+        return result
+    return locked_function
+
 class Communicator(object):
     def __init__(self, port, baud_rate, msg_sub_topic='robot/send_xmega_msg', verbose=True):
         '''Superclass for XMega communication
@@ -75,6 +94,7 @@ def err_log(self, *args):
         if self.verbose:
             print args
 
+    @thread_lock
     def got_poll_msg(self, msg):
         '''Only supports 2 byte and empty messages right now!'''
         self.err_log("Got poll message of type ", msg.type.data)
@@ -83,8 +103,9 @@ def got_poll_msg(self, msg):
         else:
             self.write_packet(msg.type.data, msg.data.data)
 
+    @thread_lock
     def got_data_msg(self, msg):
-        print "Data message:", msg.type, "Contents:", msg.data
+        self.err_log("Data message: ", msg.type, "Contents: ", msg.data)
         self.write_packet(msg.type, msg.data)
 
     def read_packets(self):