diff --git a/driver_mpu9250/CMakeLists.txt b/CMakeLists.txt
similarity index 95%
rename from driver_mpu9250/CMakeLists.txt
rename to CMakeLists.txt
index 0102e6432ebf7a69868003b3973041bcaaa3dcdd..448f52d8d90d9e3107985d3a6169768a08fb0df1 100644
--- a/driver_mpu9250/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,13 +1,9 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(driver_mpu9250)
 
-# Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
-
 # Find catkin and package dependencies
 find_package(catkin REQUIRED COMPONENTS
   roscpp
-  driver_mpu9250_msgs
   sensor_msgs_ext
 )
 
diff --git a/driver_mpu9250/doxyfile b/doxyfile
similarity index 100%
rename from driver_mpu9250/doxyfile
rename to doxyfile
diff --git a/driver_mpu9250_msgs/CMakeLists.txt b/driver_mpu9250_msgs/CMakeLists.txt
deleted file mode 100644
index 7279065f2f4b43d945d82210c3c124b7c673154e..0000000000000000000000000000000000000000
--- a/driver_mpu9250_msgs/CMakeLists.txt
+++ /dev/null
@@ -1,37 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(driver_mpu9250_msgs)
-
-# Find catkin and package dependencies.
-find_package(catkin REQUIRED COMPONENTS
-    message_generation)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-# Generate services in the 'srv' folder
-add_service_files(
-    FILES
-    calibrate_gyroscope.srv
-)
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-)
-
-# Generage catkin package.
-catkin_package(
-#  CATKIN_DEPENDS other_catkin_pkg
-)
\ No newline at end of file
diff --git a/driver_mpu9250_msgs/package.xml b/driver_mpu9250_msgs/package.xml
deleted file mode 100644
index ac64dbc754d75bd8d5884ecf03baa55e7ecf747f..0000000000000000000000000000000000000000
--- a/driver_mpu9250_msgs/package.xml
+++ /dev/null
@@ -1,16 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-
-  <name>driver_mpu9250_msgs</name>
-  <version>1.0.0</version>
-  <description>Provides messages and services for interacting with the driver_mpu9250 node.</description>
-
-  <author email="pcdangio@gmail.com">Paul D'Angio</author>
-  <maintainer email="pcdangio@gmail.com">Paul D'Angio</maintainer>
-
-  <license>MIT</license>
-
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>message_generation</build_depend>
-
-</package>
\ No newline at end of file
diff --git a/driver_mpu9250_msgs/srv/calibrate_gyroscope.srv b/driver_mpu9250_msgs/srv/calibrate_gyroscope.srv
deleted file mode 100644
index f04afd6035b18f5e6e7ded5333b1d19b3240d449..0000000000000000000000000000000000000000
--- a/driver_mpu9250_msgs/srv/calibrate_gyroscope.srv
+++ /dev/null
@@ -1,6 +0,0 @@
-# Requests for the IMU to perform a zero-velocity calibration of the gyroscope.
-
-uint32 averaging_period     # The number of milliseconds to average the bias calculation over.
----
-bool success                # TRUE if the calibration was successful, otherwise FALSE.
-string message              # If unsuccesful, reports the reason.
\ No newline at end of file
diff --git a/driver_mpu9250/package.xml b/package.xml
similarity index 84%
rename from driver_mpu9250/package.xml
rename to package.xml
index 96d61597b8b35234a19df2517b45d8f4e0950b74..2753c6ad4c16505c9747a8f07a700761715975a4 100644
--- a/driver_mpu9250/package.xml
+++ b/package.xml
@@ -11,8 +11,6 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   <depend>roscpp</depend>
-  <build_depend>libeigen3-dev</build_depend>
-  <depend>driver_mpu9250_msgs</depend>
   <depend>sensor_msgs_ext</depend>
 
 </package>
diff --git a/driver_mpu9250/src/calibration.cpp b/src/calibration.cpp
similarity index 100%
rename from driver_mpu9250/src/calibration.cpp
rename to src/calibration.cpp
diff --git a/driver_mpu9250/src/calibration.h b/src/calibration.h
similarity index 100%
rename from driver_mpu9250/src/calibration.h
rename to src/calibration.h
diff --git a/driver_mpu9250/src/driver.cpp b/src/driver.cpp
similarity index 100%
rename from driver_mpu9250/src/driver.cpp
rename to src/driver.cpp
diff --git a/driver_mpu9250/src/driver.h b/src/driver.h
similarity index 100%
rename from driver_mpu9250/src/driver.h
rename to src/driver.h
diff --git a/driver_mpu9250/src/main_rpi.cpp b/src/main_rpi.cpp
similarity index 100%
rename from driver_mpu9250/src/main_rpi.cpp
rename to src/main_rpi.cpp
diff --git a/driver_mpu9250/src/ros_node.cpp b/src/ros_node.cpp
similarity index 95%
rename from driver_mpu9250/src/ros_node.cpp
rename to src/ros_node.cpp
index 706a6491d6ec5dcc0c91e8f0743f4aac17d59e65..3f4d956c9545ce9f606bf69cb6d64c5c1159554c 100644
--- a/driver_mpu9250/src/ros_node.cpp
+++ b/src/ros_node.cpp
@@ -1,7 +1,7 @@
 #include "ros_node.h"
 
-#include <sensor_msgs_ext/acceleration.h>
-#include <sensor_msgs_ext/angular_velocity.h>
+#include <sensor_msgs_ext/accelerometer.h>
+#include <sensor_msgs_ext/gyroscope.h>
 #include <sensor_msgs_ext/magnetic_field.h>
 #include <sensor_msgs_ext/temperature.h>
 #include <sensor_msgs_ext/covariance.h>
@@ -39,8 +39,8 @@ ros_node::ros_node(std::shared_ptr<driver> driver, int argc, char **argv)
     ros_node::m_calibration_magnetometer.load(private_node, "calibration_magnetometer");
 
     // Set up data publishers.
-    ros_node::m_publisher_accelerometer = ros_node::m_node->advertise<sensor_msgs_ext::acceleration>("imu/accelerometer", 1);
-    ros_node::m_publisher_gyroscope = ros_node::m_node->advertise<sensor_msgs_ext::angular_velocity>("imu/gyroscope", 1);
+    ros_node::m_publisher_accelerometer = ros_node::m_node->advertise<sensor_msgs_ext::accelerometer>("imu/accelerometer", 1);
+    ros_node::m_publisher_gyroscope = ros_node::m_node->advertise<sensor_msgs_ext::gyroscope>("imu/gyroscope", 1);
     ros_node::m_publisher_magnetometer = ros_node::m_node->advertise<sensor_msgs_ext::magnetic_field>("imu/magnetometer", 1);
     ros_node::m_publisher_temperature = ros_node::m_node->advertise<sensor_msgs_ext::temperature>("imu/temperature", 1);
     
@@ -86,7 +86,7 @@ void ros_node::spin()
 }
 
 // SERVICES
-bool ros_node::service_calibrate_gyroscope(driver_mpu9250_msgs::calibrate_gyroscopeRequest& request, driver_mpu9250_msgs::calibrate_gyroscopeResponse& response)
+bool ros_node::service_calibrate_gyroscope(sensor_msgs_ext::calibrate_gyroscopeRequest& request, sensor_msgs_ext::calibrate_gyroscopeResponse& response)
 {
     response.success = ros_node::calibrate_gyroscope(request.averaging_period);
     if(!response.success)
@@ -202,7 +202,7 @@ void ros_node::deinitialize_driver()
 void ros_node::data_callback(driver::data data)
 {
     // Create accelerometer message.
-    sensor_msgs_ext::acceleration message_accel;
+    sensor_msgs_ext::accelerometer message_accel;
     // Set accelerations (convert from g's to m/s^2)
     message_accel.x = static_cast<double>(data.accel_x) * 9.80665;
     message_accel.y = static_cast<double>(data.accel_y) * 9.80665;
@@ -213,7 +213,7 @@ void ros_node::data_callback(driver::data data)
     ros_node::m_publisher_accelerometer.publish(message_accel);
 
     // Create gyroscope message.
-    sensor_msgs_ext::angular_velocity message_gyro;
+    sensor_msgs_ext::gyroscope message_gyro;
     // Set rotation rates (convert from deg/sec to rad/sec)
     message_gyro.x = static_cast<double>(data.gyro_x) * M_PI / 180.0;
     message_gyro.y = static_cast<double>(data.gyro_y) * M_PI / 180.0;
diff --git a/driver_mpu9250/src/ros_node.h b/src/ros_node.h
similarity index 93%
rename from driver_mpu9250/src/ros_node.h
rename to src/ros_node.h
index 6a8952e8753169b54544eb46469fadcf356755eb..2794ceab8aeb56abff389fbc56371c07ecd96100 100644
--- a/driver_mpu9250/src/ros_node.h
+++ b/src/ros_node.h
@@ -7,7 +7,7 @@
 #include "calibration.h"
 
 #include <ros/ros.h>
-#include <driver_mpu9250_msgs/calibrate_gyroscope.h>
+#include <sensor_msgs_ext/calibrate_gyroscope.h>
 
 #include <atomic>
 #include <deque>
@@ -79,7 +79,7 @@ private:
     /// \param request The service request.
     /// \param response The service response.
     /// \returns TRUE if the service completed successfully, otherwise FALSE.
-    bool service_calibrate_gyroscope(driver_mpu9250_msgs::calibrate_gyroscopeRequest& request, driver_mpu9250_msgs::calibrate_gyroscopeResponse& response);
+    bool service_calibrate_gyroscope(sensor_msgs_ext::calibrate_gyroscopeRequest& request, sensor_msgs_ext::calibrate_gyroscopeResponse& response);
 
     // METHODS
     /// \brief Publishes covariance matrices.
diff --git a/driver_mpu9250/src/rpi_driver.cpp b/src/rpi_driver.cpp
similarity index 100%
rename from driver_mpu9250/src/rpi_driver.cpp
rename to src/rpi_driver.cpp
diff --git a/driver_mpu9250/src/rpi_driver.h b/src/rpi_driver.h
similarity index 100%
rename from driver_mpu9250/src/rpi_driver.h
rename to src/rpi_driver.h