From ef83edc1dc41557c5d91690e71840e96a63bbb28 Mon Sep 17 00:00:00 2001
From: Patrick Goebel <patrick@pirobot.org>
Date: Tue, 22 Nov 2016 07:32:25 -0800
Subject: [PATCH] Close the serial port when exiting arduino_node.py

---
 ros_arduino_python/nodes/arduino_node.py | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py
index 60b5620..e9be685 100755
--- a/ros_arduino_python/nodes/arduino_node.py
+++ b/ros_arduino_python/nodes/arduino_node.py
@@ -27,6 +27,7 @@ from ros_arduino_python.base_controller import BaseController
 from geometry_msgs.msg import Twist
 import os, time
 import thread
+from serial.serialutil import SerialException
 
 class ArduinoROS():
     def __init__(self):
@@ -201,6 +202,8 @@ class ArduinoROS():
         return AnalogReadResponse(value)
  
     def shutdown(self):
+        rospy.loginfo("Shutting down Arduino Node...")
+
         # Stop the robot
         try:
             rospy.loginfo("Stopping the robot...")
@@ -208,7 +211,19 @@ class ArduinoROS():
             rospy.sleep(2)
         except:
             pass
-        rospy.loginfo("Shutting down Arduino Node...")
         
+        # Close the serial port
+        try:
+            self.controller.close()
+        except:
+            pass
+        finally:
+            rospy.loginfo("Serial port closed.")
+            os._exit(0)
+
 if __name__ == '__main__':
-    myArduino = ArduinoROS()
+    try:
+        myArduino = ArduinoROS()
+    except SerialException:
+        rospy.logerr("Serial exception trying to open port.")
+        os._exit(0)