diff --git a/examples/serial_example.cc b/examples/serial_example.cc
index af7b666..27857ff 100644
--- a/examples/serial_example.cc
+++ b/examples/serial_example.cc
@@ -1,5 +1,6 @@
 #include <string>
 #include <iostream>
+#include <stdio.h>
 
 #include <boost/thread.hpp>
 
@@ -27,7 +28,7 @@ int run(int argc, char **argv)
     
     int count = 0;
     while (count >= 0) {
-        int bytes_wrote = serial.write("Testing.");
+        size_t bytes_wrote = serial.write("Testing.");
         std::string result = serial.read(8);
         std::cout << ">" << count << ">" << bytes_wrote << ">";
         std::cout << result.length() << "<" << result << std::endl;
diff --git a/serial_ros.cmake b/serial_ros.cmake
index 0c69171..029b83a 100644
--- a/serial_ros.cmake
+++ b/serial_ros.cmake
@@ -42,4 +42,10 @@ rosbuild_add_executable(serial_listener_example
   examples/serial_listener_example.cc)
 target_link_libraries(serial_listener_example ${PROJECT_NAME})
 
+# Create unit tests
+rosbuild_add_gtest(serial_tests tests/serial_tests.cc)
+target_link_libraries(serial_tests ${PROJECT_NAME})
+rosbuild_add_gtest(serial_listener_tests tests/serial_listener_tests.cc)
+target_link_libraries(serial_listener_tests ${PROJECT_NAME})
+
 endmacro(build_serial)
diff --git a/tests/proof_of_concepts/python_serial_test.py b/tests/proof_of_concepts/python_serial_test.py
new file mode 100644
index 0000000..6f92b84
--- /dev/null
+++ b/tests/proof_of_concepts/python_serial_test.py
@@ -0,0 +1,15 @@
+#!/usr/bin/env python
+
+import serial, sys
+
+if len(sys.argv) != 2:
+    print "python: Usage_serial_test <port name like: /dev/ttyUSB0>"
+    sys.exit(1)
+
+sio = serial.Serial(sys.argv[1], 115200)
+sio.timeout = 250
+
+while True:
+    sio.write("Testing.")
+    print sio.read(8)
+
diff --git a/tests/serial_listener_tests.cc b/tests/serial_listener_tests.cc
index 64d41b5..ee84c9c 100644
--- a/tests/serial_listener_tests.cc
+++ b/tests/serial_listener_tests.cc
@@ -17,7 +17,7 @@
  
  */
 
-#define SERIAL_PORT_NAME "/dev/tty.usbserial"
+#define SERIAL_PORT_NAME "/dev/tty.usbserial-A900cfJA"
 
 #include "gtest/gtest.h"
 
@@ -39,11 +39,19 @@ void default_handler(std::string line) {
 
 namespace {
 
+
+void my_sleep(long milliseconds) {
+  boost::this_thread::sleep(boost::posix_time::milliseconds(milliseconds));
+}
+
 class SerialListenerTests : public ::testing::Test {
 protected:
   virtual void SetUp() {
     port1 = new Serial(SERIAL_PORT_NAME, 115200, 250);
 
+    // Need to wait a bit for the Arduino to come up
+    my_sleep(1000);
+
     listener.setDefaultHandler(default_handler);
     listener.startListening((*port1));
   }
@@ -59,10 +67,6 @@ protected:
 
 };
 
-void my_sleep(long milliseconds) {
-  boost::this_thread::sleep(boost::posix_time::milliseconds(milliseconds));
-}
-
 TEST_F(SerialListenerTests, handlesPartialMessage) {
   global_count = 0;
   std::string input_str = "?$1E\r$1E=Robo";
diff --git a/tests/serial_tests.cc b/tests/serial_tests.cc
index 42983a1..6a5a0c5 100644
--- a/tests/serial_tests.cc
+++ b/tests/serial_tests.cc
@@ -1,35 +1,64 @@
-#include <string>
-#include <iostream>
+/* To run these tests you need to change the define below to the serial port 
+ * with a loop back device attached.
+ * 
+ * Alternatively you could use an Arduino:
+ 
+ void setup()
+ {
+   Serial.begin(115200);
+ }
+
+ void loop()
+ {
+   while (Serial.available() > 0) {
+     Serial.write(Serial.read());
+   }
+ }
+ 
+ */
+
+#define SERIAL_PORT_NAME "/dev/tty.usbserial"
+
+#include "gtest/gtest.h"
+
+#include <boost/bind.hpp>
+
+// OMG this is so nasty...
+#define private public
+#define protected public
 
 #include "serial/serial.h"
+using namespace serial;
 
-using std::string;
-using std::cout;
-using std::endl;
-using serial::Serial;
-using serial::SerialExecption;
+namespace {
+
+class SerialTests : public ::testing::Test {
+protected:
+  virtual void SetUp() {
+    port1 = new Serial(SERIAL_PORT_NAME, 115200, 250);
+  }
+
+  virtual void TearDown() {
+    port1->close();
+    delete port1;
+  }
+
+  Serial * port1;
+
+};
+
+// TEST_F(SerialTests, throwsOnInvalidPort) {
+//   
+// }
+
+}  // namespace
 
 int main(int argc, char **argv) {
   try {
-    Serial s("/dev/tty.usbserial-A900adHq", 115200, 100);
-    s.flush();
-    long long count = 0;
-    while (1) {
-      // size_t available = s.available();
-      // cout << "avialable: " << available << endl;
-      string line = s.readline();
-      if (line.empty())
-        cout << "Nothing\n";
-      cout << count << ": " << line << line.length() << endl;
-      count++;
-    }
-  }
-  catch (SerialExecption e)
-  {
-    cout << "Caught SerialException: " << e.what() << endl;
-  }
-  catch (...)
-  {
-    cout << "Caught an error." << endl;
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+  } catch (std::exception &e) {
+    std::cerr << "Unhandled Exception: " << e.what() << std::endl;
   }
+  return 1;
 }