From ee8a150ae781c052a052fcf5a7bc9706e6e2b6aa Mon Sep 17 00:00:00 2001
From: Ryuichi Ueda <ryuichiueda@gmail.com>
Date: Mon, 30 May 2016 01:53:02 +0000
Subject: [PATCH] Added error handling

---
 scripts/rtbuzzer.py       |  8 +++++---
 scripts/rtlightsensors.py | 22 +++++++++++++---------
 scripts/rtmotor.py        | 31 +++++++++++++++++++------------
 scripts/rtswitches.py     | 15 +++++++++------
 4 files changed, 46 insertions(+), 30 deletions(-)

diff --git a/scripts/rtbuzzer.py b/scripts/rtbuzzer.py
index 9e413e3..5c7d0f1 100755
--- a/scripts/rtbuzzer.py
+++ b/scripts/rtbuzzer.py
@@ -4,9 +4,11 @@
 
 def callback(message):
     devfile = '/dev/rtbuzzer0'
-    #rospy.loginfo("Buzzer %d", message.data)
-    with open(devfile,'w') as f:
-        print >> f, message.data
+    try:
+        with open(devfile,'w') as f:
+            print >> f, message.data
+    except:
+        rospy.logerr("cannot open " + devfile)
         
 
 def listner():
diff --git a/scripts/rtlightsensors.py b/scripts/rtlightsensors.py
index e012481..9df0a78 100755
--- a/scripts/rtlightsensors.py
+++ b/scripts/rtlightsensors.py
@@ -3,20 +3,24 @@
 from raspimouse_ros.msg import LightSensorValues
 
 def talker():
+    devfile = '/dev/rtlightsensor0'
     rospy.init_node('lightsensors')
     pub = rospy.Publisher('lightsensors', LightSensorValues, queue_size=1)
     rate = rospy.Rate(10)
 
     while not rospy.is_shutdown():
-        with open('/dev/rtlightsensor0','r') as f:
-            data = f.readline().split()
-            d = LightSensorValues()
-            d.right_forward = int(data[0])
-            d.right_side = int(data[1])
-            d.left_side = int(data[2])
-            d.left_forward = int(data[3])
-            pub.publish(d)
-            rate.sleep()
+        try:
+            with open(devfile,'r') as f:
+                data = f.readline().split()
+                d = LightSensorValues()
+                d.right_forward = int(data[0])
+                d.right_side = int(data[1])
+                d.left_side = int(data[2])
+                d.left_forward = int(data[3])
+                pub.publish(d)
+                rate.sleep()
+        except:
+            rospy.logerr("cannot open " + devfile)
 
 if __name__ == '__main__':
     try:
diff --git a/scripts/rtmotor.py b/scripts/rtmotor.py
index eb52f25..a83f7fb 100755
--- a/scripts/rtmotor.py
+++ b/scripts/rtmotor.py
@@ -9,9 +9,13 @@
 def callback_motor_sw(message):
     enfile = '/dev/rtmotoren0'
 
-    with open(enfile,'w') as f:
-        if message.on: print >> f, '1'
-        else:          print >> f, '0'
+    try: 
+        with open(enfile,'w') as f:
+            if message.on: print >> f, '1'
+            else:          print >> f, '0'
+    except:
+        rospy.logerr("cannot write to " + enfile)
+        return False
 
     return True
 
@@ -19,24 +23,27 @@ def callback_motor_raw(message):
     lfile = '/dev/rtmotor_raw_l0'
     rfile = '/dev/rtmotor_raw_r0'
 
+    print message
     try:
         lf = open(lfile,'w')
         rf = open(rfile,'w')
         print >> lf, str(message.left)
         print >> rf, str(message.right)
     except:
-        print >> sys.stderr, "cannot write to rtmotor_raw_*" 
-        sys.exit(1)
-    else:
-        lf.close()
-        rf.close()
+        rospy.logerr("cannot write to rtmotor_raw_*")
+
+    lf.close()
+    rf.close()
 
 def callback_put_freqs(message):
     devfile = '/dev/rtmotor0'
-    putstr = "%s %s %s" % (message.left, message.right, message.duration)
-    print putstr
-    with open(devfile,'w') as f:
-        print >> f, putstr
+
+    try:
+        with open(devfile,'w') as f:
+            print >> f, "%s %s %s" % (message.left, message.right, message.duration)
+    except:
+        rospy.logerr("cannot write to " + devfile)
+        return False
 
     return True
         
diff --git a/scripts/rtswitches.py b/scripts/rtswitches.py
index 24b4532..c12408f 100755
--- a/scripts/rtswitches.py
+++ b/scripts/rtswitches.py
@@ -12,12 +12,15 @@ def talker():
 
     d = Switches()
     while not rospy.is_shutdown():
-        with open(devfile + '0','r') as f:
-            d.front = True if '0' in f.readline() else False
-        with open(devfile + '1','r') as f:
-            d.center = True if '0' in f.readline() else False
-        with open(devfile + '2','r') as f:
-            d.rear = True if '0' in f.readline() else False
+        try:
+            with open(devfile + '0','r') as f:
+                d.front = True if '0' in f.readline() else False
+            with open(devfile + '1','r') as f:
+                d.center = True if '0' in f.readline() else False
+            with open(devfile + '2','r') as f:
+                d.rear = True if '0' in f.readline() else False
+        except:
+            rospy.logerr("cannot open " + devfile + "[0,1,2]")
 
         pub.publish(d)
         rate.sleep()