diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..b5870a3
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "ros_ws/src/arudino/razor-9dof-ahrs"]
+	path = ros_ws/src/arudino/razor-9dof-ahrs
+	url = https://github.com/MST-Robotics/razor-9dof-ahrs.git
diff --git a/ros_ws/src/arudino/imu/imu.ino b/ros_ws/src/arudino/imu/imu.ino
deleted file mode 100644
index 85389a3..0000000
--- a/ros_ws/src/arudino/imu/imu.ino
+++ /dev/null
@@ -1,179 +0,0 @@
-#include <Wire.h>
-
-#define GYROADDR 0x68
-#define COMPASSADDR 0x1e
-#define ACCELADDR 0x53
-
-union XYZBuffer {
-  struct {
-    short x,y,z;
-  } value;
-  byte buff[6];
-};
-
-void changeEndian(union XYZBuffer *xyz) {
-  for (int i=0;i<6;i+=2) {
-    byte t=xyz->buff[i];
-    xyz->buff[i]=xyz->buff[i+1];
-    xyz->buff[i+1]=t;
-  }
-}
-
-// Generically useful reading into a union type
-void readXYZ(int device,union XYZBuffer *xyz) {     
-  Wire.requestFrom(device, 6);      
-  long start=millis();
-  while (!Wire.available() && (millis()-start)<100);
-  if (millis()-start<100) {
-    for (int i=0;i<6;i++)
-      xyz->buff[i]=Wire.read();
-  }
-}
-
-void setupAccel(int device) {
-  // Check ID to see if we are communicating
-  Wire.beginTransmission(device);
-  Wire.write(0x00); // One Reading
-  Wire.endTransmission(); 
-  Wire.requestFrom(device,1);
-  while (!Wire.available());  
-  byte ch=Wire.read();
-  Serial.print("Accel id is 0x");
-  Serial.println(ch,HEX);
-  // Should output E5
-
-  // https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf
-  // Page 16
-  Wire.beginTransmission(device);
-  Wire.write(0x2d);
-  Wire.write(0x08);
-  Wire.endTransmission();
-  Wire.beginTransmission(device);
-  Wire.write(0x38);
-  Wire.write(0x84);
-  Wire.endTransmission();
-
-}
-void readAccel(int device,union XYZBuffer *xyz) {
-  Wire.beginTransmission(device);
-  Wire.write(0x32); // One Reading
-  Wire.endTransmission(); 
-  readXYZ(device,xyz);
-}
-
-void setupCompass(int device) {
-  // Check ID to see if we are communicating
-  Serial.print("Compass id is ");
-  Wire.beginTransmission(device);
-  Wire.write(10); // One Reading
-  Wire.endTransmission(); 
-  Wire.requestFrom(device,2); 
-  while (!Wire.available());
-  char ch=Wire.read();
-  Serial.print(ch);   
-  ch=Wire.read();
-  Serial.println(ch);
-  // Should output H4  
-
-// Page 18
-// at http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Magneto/HMC5883L-FDS.pdf
-  Wire.beginTransmission(device);
-  Wire.write(0x00); Wire.write(0x70);
-  Wire.endTransmission();
-  Wire.beginTransmission(device);
-  Wire.write(0x01); Wire.write(0xA0);
-  Wire.endTransmission();
-  Wire.beginTransmission(device);
-  Wire.write(0x02); Wire.write(0x00); //  Reading
-  Wire.endTransmission(); 
-  delay(6);
-}
-void readCompass(int device,union XYZBuffer *xyz) {
-  readXYZ(device,xyz);
-  changeEndian(xyz);
-  Wire.beginTransmission(device);
-  Wire.write(0x03);
-  Wire.endTransmission(); 
-}
-
-void setupGyro(int device) {
-  // Check ID to see if we are communicating
-  Wire.beginTransmission(device);
-  Wire.write(0x00); // One Reading
-  Wire.endTransmission(); 
-  Wire.requestFrom(device,1);
-  while (!Wire.available());  
-  byte ch=Wire.read();
-  Serial.print("Gyro id is 0x");
-  Serial.println(ch,HEX);  
-  // Should output 69
-}
-void readGyro(int device,union XYZBuffer *xyz) {
-  // https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
-  // page 20
-  Wire.beginTransmission(device);
-  Wire.write(0x1d);
-  Wire.endTransmission(); 
-  readXYZ(device,xyz);
-  changeEndian(xyz);  
-}
-
-void pad(int width,int number) {
-  int n=abs(number);
-  int w=width;
-  if (number<0) w--;
-  while (n>0) {
-    w--;
-    n/=10;
-  }
-  if (number==0) w--;
-  for (int i=0;i<w;i++) Serial.print(' ');
-}
-
-void output(union XYZBuffer xyz) {
-//  pad(6,xyz.value.x);
-  Serial.print(xyz.value.x);
-  Serial.print(',');
-//  pad(6,xyz.value.y);
-  Serial.print(xyz.value.y);
-  Serial.print(',');
-//  pad(6,xyz.value.z);
-  Serial.print(xyz.value.z);
-}
-
-void setup()
-{
-  Serial.begin(9600);  // start serial for output
-  Wire.begin();        // join i2c bus (address optional for master)
-  setupCompass(COMPASSADDR);
-  setupAccel(ACCELADDR);
-  setupGyro(GYROADDR);
-}
-
-void loop()
-{
-  union XYZBuffer compass,gyro,accel;
-  int l1,l2,l3,l4;
-  l1=analogRead(0);
-  l2=analogRead(1);
-  l3=analogRead(2);
-  l4=analogRead(3);
-  readAccel(ACCELADDR,&accel);
-  readCompass(COMPASSADDR,&compass);
-  readGyro(GYROADDR,&gyro);
-  Serial.print("A,");
-  output(accel);
-  Serial.print(",C,");
-  output(compass);
-  Serial.print(",G,");  
-  output(gyro);
-  Serial.print(",L,");
-  Serial.print(l1);
-  Serial.print(",");
-  Serial.print(l2);
-  Serial.print(",");
-  Serial.print(l3);
-  Serial.print(",");
-  Serial.print(l4);
-  Serial.println();
-}
diff --git a/ros_ws/src/arudino/razor-9dof-ahrs b/ros_ws/src/arudino/razor-9dof-ahrs
new file mode 160000
index 0000000..f20eea9
--- /dev/null
+++ b/ros_ws/src/arudino/razor-9dof-ahrs
@@ -0,0 +1 @@
+Subproject commit f20eea9d70f92035607a6953e8a6d61cf4d6b790
diff --git a/ros_ws/src/localization/CMakeLists.txt b/ros_ws/src/localization/CMakeLists.txt
new file mode 100644
index 0000000..eb4e72d
--- /dev/null
+++ b/ros_ws/src/localization/CMakeLists.txt
@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(localization)
+
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    tf
+    message_generation
+    std_msgs
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+add_message_files(FILES rpy.msg)
+generate_messages(DEPENDENCIES std_msgs)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+        INCLUDE_DIRS include
+        LIBRARIES localization
+        CATKIN_DEPENDS roscpp tf message_generation std_msgs
+        DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+include_directories(
+        ${catkin_INCLUDE_DIRS}
+        include/localization/
+)
+
+add_executable(orientation_node src/localization/orientation_node.cpp include/localization/orientation.h
+               src/localization/orientation.cpp)
+add_dependencies(orientation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(orientation_node ${catkin_LIBRARIES})
diff --git a/ros_ws/src/localization/include/localization/orientation.h b/ros_ws/src/localization/include/localization/orientation.h
new file mode 100644
index 0000000..37f209d
--- /dev/null
+++ b/ros_ws/src/localization/include/localization/orientation.h
@@ -0,0 +1,54 @@
+/**
+ * @file orientation.h
+ * @class Orientation
+ * @brief The class definition file for the Orientation class
+ */
+
+#ifndef PROJECT_ORIENTATION_H
+#define PROJECT_ORIENTATION_H
+
+#include <ros/ros.h>
+#include <localization/rpy.h>
+#include <sensor_msgs/Imu.h>
+#include <tf/transform_datatypes.h>
+
+class Orientation
+{
+    private:
+        /**
+         * @brief The ros nodehandle
+         */
+        ros::NodeHandle nh;
+
+        /**
+         * @brief The subscriber to the message from the imu, expects an rpy message
+         */
+        ros::Subscriber imu_sub;
+
+        /**
+         * @brief The publisher of the imu message
+         */
+        ros::Publisher orientation_pub;
+
+        /**
+         * @brief The orientation of the robot
+         */
+        sensor_msgs::Imu orientation;
+
+        /**
+         * @brief The callback function for the rpy message
+         *
+         * This function will accept rpy messages and will publish this information to an imu message
+         *
+         * @param msg The message received from the rpy publisher
+         */
+        void rpyCallback(const localization::rpy::ConstPtr& msg);
+
+    public:
+        Orientation();
+
+        void update();
+};
+
+
+#endif //PROJECT_ORIENTATION_H
diff --git a/ros_ws/src/localization/msg/rpy.msg b/ros_ws/src/localization/msg/rpy.msg
new file mode 100644
index 0000000..84885f7
--- /dev/null
+++ b/ros_ws/src/localization/msg/rpy.msg
@@ -0,0 +1,3 @@
+float32 roll
+float32 pitch
+float32 yaw
\ No newline at end of file
diff --git a/ros_ws/src/localization/package.xml b/ros_ws/src/localization/package.xml
new file mode 100644
index 0000000..2f24f5a
--- /dev/null
+++ b/ros_ws/src/localization/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package>
+  <name>localization</name>
+  <version>0.0.0</version>
+  <description>The localization package</description>
+
+  <maintainer email="mia2n4@mst.edu">Matt Anderson</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://robotics.mst.edu</url>
+  <url type="repository">https://github.com/MST-Robotics/IGVC</url>
+
+  <author email="mia2n4@mst.edu">Matt Anderson</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>tf</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>message_generation</run_depend>
+  <run_depend>tf</run_depend>
+
+</package>
\ No newline at end of file
diff --git a/ros_ws/src/localization/src/localization/orientation.cpp b/ros_ws/src/localization/src/localization/orientation.cpp
new file mode 100644
index 0000000..f9f5696
--- /dev/null
+++ b/ros_ws/src/localization/src/localization/orientation.cpp
@@ -0,0 +1,26 @@
+/**
+ * @file orientation.cpp
+ * @class Orientation
+ * @brief The class implementation file for the orientation class
+ */
+#include "orientation.h"
+
+Orientation::Orientation()
+{
+    // Initialize the imu subscriber
+    imu_sub = nh.subscribe<localization::rpy>("imu", 1, &Orientation::rpyCallback, this);
+    orientation_pub = nh.advertise<sensor_msgs::Imu>("orientation", 1);
+}
+
+void Orientation::rpyCallback(const localization::rpy::ConstPtr &msg)
+{
+    orientation.header.stamp = ros::Time::now();
+    orientation.header.frame_id = "camera_link";
+    orientation.orientation = tf::createQuaternionMsgFromRollPitchYaw(msg->roll, msg->pitch, msg->yaw);
+}
+
+void Orientation::update()
+{
+    orientation_pub.publish(orientation);
+}
+
diff --git a/ros_ws/src/localization/src/localization/orientation_node.cpp b/ros_ws/src/localization/src/localization/orientation_node.cpp
new file mode 100644
index 0000000..dc9b5ed
--- /dev/null
+++ b/ros_ws/src/localization/src/localization/orientation_node.cpp
@@ -0,0 +1,25 @@
+/**
+ * @file orientation_node.cpp
+ * @brief The orientation node source file
+ */
+
+#include <ros/ros.h>
+#include "orientation.h"
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "orientation_node");
+
+    Orientation o;
+
+    // Spin at 30hz
+    ros::Rate loop_rate(30);
+
+    // Spin forever as long as ros is okay
+    while (ros::ok())
+    {
+        ros::spinOnce();
+        o.update();
+        loop_rate.sleep();
+    }
+}
\ No newline at end of file