diff --git a/CMakeLists.txt b/CMakeLists.txt
index a5d807f..5dfc442 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(imu_3dm_gx4)
+
+find_package(Boost REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
   diagnostic_updater
   message_generation
@@ -11,20 +13,55 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(DIRECTORY msg)
 generate_messages(DEPENDENCIES geometry_msgs)
 
-catkin_package()
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}_ros
+  DEPENDS boost)
 
-# include boost
-find_package(Boost REQUIRED)
-link_directories(${Boost_LIBRARY_DIR})
-include_directories(${Boost_INCLUDE_DIR})
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIR})
+link_directories(
+  ${catkin_LIBRARY_DIRS}
+  ${Boost_LIBRARY_DIR})
 
-add_definitions("-std=c++0x -Wall -Werror")
 
-add_executable(${PROJECT_NAME} src/imu_3dm_gx4.cpp src/imu.cpp)
+#add_definitions("-std=c++0x -Wall -Werror")
+add_definitions("-std=c++0x -Wall -Wno-unused-variable -Wno-uninitialized")
+
+add_library(${PROJECT_NAME}
+  src/imu.cpp
+)
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
 )
 
+add_library(${PROJECT_NAME}_ros
+  src/imu_ros_base.cpp
+)
+target_link_libraries(${PROJECT_NAME}_ros
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_cont_node
+  src/imu_3dm_gx4_cont.cpp
+)
+target_link_libraries(${PROJECT_NAME}_cont_node
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_single_node
+  src/imu_3dm_gx4_single.cpp
+)
+target_link_libraries(${PROJECT_NAME}_single_node
+  ${PROJECT_NAME}
+  ${PROJECT_NAME}_ros
+  ${catkin_LIBRARIES}
+)
+
 add_dependencies(${PROJECT_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
   ${catkin_EXPORTED_TARGETS}
diff --git a/src/imu.hpp b/include/imu_3dm_gx4/imu.hpp
similarity index 94%
rename from src/imu.hpp
rename to include/imu_3dm_gx4/imu.hpp
index c69d40d..345de9c 100644
--- a/src/imu.hpp
+++ b/include/imu_3dm_gx4/imu.hpp
@@ -377,6 +377,32 @@ class Imu {
    */
   void enableFilterStream(bool enabled);
 
+  /**
+   * @brief enableIMUStream Poll IMU data once
+   * @param enabled If true, an ack package will be returned from the device
+   * before the actual data package. The default is set to false in order
+   * not to jam the bandwidth.
+   */
+  void pollIMUData(bool enable_ack = false);
+
+  /**
+   * @brief enableIMUStream Poll filter data once
+   * @param enabled If true, an ack package will be returned from the device
+   * before the actual data package. The default is set to false in order
+   * not to jam the bandwidth.
+   */
+  void pollFilterData(bool enable_ack = false);
+
+  /**
+   * @brief saveIMUFormat Save the IMU data format
+   */
+  void saveIMUFormat();
+
+  /**
+   * @brief saveFilterFormat Save the filter data format
+   */
+  void saveFilterFormat();
+
   /**
    * @brief Set the IMU data callback.
    * @note The IMU data callback is called every time new IMU data is read.
@@ -398,7 +424,7 @@ class Imu {
   int pollInput(unsigned int to);
 
   std::size_t handleByte(const uint8_t& byte, bool& found);
-  
+
   int handleRead(size_t);
 
   void processPacket();
diff --git a/include/imu_3dm_gx4/imu_ros_base.hpp b/include/imu_3dm_gx4/imu_ros_base.hpp
new file mode 100644
index 0000000..9092a50
--- /dev/null
+++ b/include/imu_3dm_gx4/imu_ros_base.hpp
@@ -0,0 +1,169 @@
+/*
+ * imu_ros.hpp
+ *
+ *  Copyright (c) 2014 Kumar Robotics. All rights reserved.
+ *
+ *  This file is part of galt.
+ *
+ *  Created on: 08/04/2014
+ *		  Author: Ke Sun
+ */
+
+
+#ifndef IMU_ROS_H
+#define IMU_ROS_H
+
+#include <ros/ros.h>
+#include <ros/node_handle.h>
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
+
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/MagneticField.h>
+#include <sensor_msgs/FluidPressure.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/QuaternionStamped.h>
+
+#include <imu_3dm_gx4/FilterOutput.h>
+#include <imu_3dm_gx4/imu.hpp>
+
+namespace imu_3dm_gx4 {
+
+  /**
+   * @brief ImuRosBase The class is a ros wrapper for the Imu class
+   * @author Ke Sun
+   */
+  class ImuRosBase {
+  public:
+    /**
+     * @brief Constructor of the class
+     */
+    ImuRosBase(const ros::NodeHandle& nh, const std::string& device, const bool verbose = false);
+    /**
+     * @brief Destructor of the class
+     * @note Disconnect the device automatically
+     */
+    ~ImuRosBase() {
+      disconnect();
+    }
+    /**
+     * @brief initialize Initialize IMU according to the settings
+     */
+    bool initialize();
+    /**
+     * @brief enableIMUStream Enable or disable IMU stream
+     * @param enabled If ture, the continuous stream is enabled
+     */
+    void enableIMUStream(bool enabled){
+      imu.enableIMUStream(enabled);
+      return;
+    }
+    /**
+     * @brief enableFilterStream Enable or disable filter stream
+     * @param enabled If ture, the continuous stream is enabled
+     */
+    void enableFilterStream(bool enabled){
+      if (enableFilter)
+        imu.enableFilterStream(enabled);
+      else
+        imu.enableFilterStream(false);
+
+      return;
+    }
+    /**
+     * @brief requestIMUOnce Request IMU data for a single time
+     * @param enable_ack If ture, an ack package will come before the data
+     */
+    void requestIMUOnce(bool enable_ack = false){
+      imu.pollIMUData(enable_ack);
+      return;
+    }
+    /**
+     * @brief requestFilterOnce Request filter data for a single time
+     * @param enable_ack If ture, an ack package will come before the data
+     */
+    void requestFilterOnce(bool enable_ack = false){
+      if (enableFilter)
+        imu.pollFilterData(enable_ack);
+      return;
+    }
+    /**
+     * @brief idle Set the device to idle mode
+     * @need_reply If ture, a ack package is expected
+     */
+    void idle(bool need_reply = true) {
+      imu.idle(need_reply);
+    }
+    /**
+     * @brief resume Resume data streaming
+     */
+    void resume() {
+      imu.resume();
+    }
+    /**
+     * @brief disconnect Disconnect the device
+     */
+    void disconnect() {
+      imu.disconnect();
+    }
+    /**
+     * @brief runOnce Wait for reading the data
+     */
+    void runOnce() {
+      imu.runOnce();
+      updater->update();
+    }
+
+    /**
+     * @Note Do not use these functions outside the class.
+     * They are defined in the public section due to the callback
+     * issue in the original imu_3dm_gx4_25 driver
+     * TODO: consider seal these functions
+     */
+    void publishIMUData(const Imu::IMUData& data);
+    void publishFilterData(const Imu::FilterData& data);
+
+    std::shared_ptr<diagnostic_updater::TopicDiagnostic> configTopicDiagnostic(
+        const std::string& name, double * target);
+    void updateDiagnosticInfo(diagnostic_updater::DiagnosticStatusWrapper& stat,
+        imu_3dm_gx4::Imu* imu);
+
+  private:
+    // Settings
+    int baudrate;
+    int requestedImuRate;
+    int requestedFilterRate;
+    bool enableFilter;
+    bool enableMagUpdate;
+    bool enableAccelUpdate;
+    std::string frameId;
+
+    ros::NodeHandle nh;
+    Imu imu;
+
+    // Publishers
+    ros::Publisher pubIMU;
+    ros::Publisher pubMag;
+    ros::Publisher pubPressure;
+    ros::Publisher pubFilter;
+
+    Imu::Info info;
+    Imu::DiagnosticFields fields;
+
+    // diagnostic_updater resources
+    std::shared_ptr<diagnostic_updater::Updater> updater;
+    std::shared_ptr<diagnostic_updater::TopicDiagnostic> imuDiag;
+    std::shared_ptr<diagnostic_updater::TopicDiagnostic> filterDiag;
+
+    // Disable copy constructor and assign operator
+    ImuRosBase(const ImuRosBase&);
+    ImuRosBase& operator=(const ImuRosBase&);
+
+    bool loadParameters();
+    void createPublishers();
+  };
+
+}
+
+#endif
+
diff --git a/launch/imu.launch b/launch/imu.launch
index f7b6c51..fe0280c 100644
--- a/launch/imu.launch
+++ b/launch/imu.launch
@@ -26,7 +26,7 @@
     <arg name="enable_accel_update" default="false"/>
     <arg name="enable_mag_update" default="false"/>
 
-    <node pkg="imu_3dm_gx4" name="$(arg imu)" type="imu_3dm_gx4" output="$(arg output)">
+    <node pkg="imu_3dm_gx4" name="$(arg imu)" type="imu_3dm_gx4_cont_node" output="$(arg output)">
         <param name="device" type="string" value="$(arg device)" />
         <param name="verbose" type="bool" value="$(arg verbose)"/>
         <param name="baudrate" type="int" value="$(arg baudrate)" />
diff --git a/launch/imu_single.launch b/launch/imu_single.launch
new file mode 100644
index 0000000..daf6a2f
--- /dev/null
+++ b/launch/imu_single.launch
@@ -0,0 +1,40 @@
+<launch>
+    <!-- Node Settings -->
+    <arg name="output" default="screen"/>
+    <arg name="imu" default="imu"/>
+
+    <!-- IMU Settings -->
+    <arg name="device" default="/dev/ttyACM0" />
+
+    <!-- Verbose logging -->
+    <arg name="verbose" default="false"/>
+
+    <!-- Frame ID for messages -->
+    <arg name="frame_id" default="$(arg imu)"/>
+
+    <!-- Baudrate of serial comms (see manual for allowed values) -->
+    <arg name="baudrate" default="115200"/>
+
+    <!-- Data rate in Hz -->
+    <arg name="imu_rate" default="100"/>
+    <arg name="filter_rate" default="100"/>
+
+    <!-- Enable/Disable the filter -->
+    <arg name="enable_filter" default="true"/>
+
+    <!-- Enable/Disable filter updates -->
+    <arg name="enable_accel_update" default="true"/>
+    <arg name="enable_mag_update" default="true"/>
+
+    <node pkg="imu_3dm_gx4" name="$(arg imu)" type="imu_3dm_gx4_single_node" output="$(arg output)">
+        <param name="device" type="string" value="$(arg device)" />
+        <param name="verbose" type="bool" value="$(arg verbose)"/>
+        <param name="baudrate" type="int" value="$(arg baudrate)" />
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="imu_rate" type="int" value="$(arg imu_rate)" />
+        <param name="filter_rate" type="int" value="$(arg filter_rate)"/>
+        <param name="enable_filter" type="bool" value="$(arg enable_filter)"/>
+        <param name="enable_accel_update" type="bool" value="$(arg enable_accel_update)"/>
+        <param name="enable_mag_update" type="bool" value="$(arg enable_mag_update)"/>
+    </node>
+</launch>
diff --git a/src/imu.cpp b/src/imu.cpp
index 442f59c..3334967 100644
--- a/src/imu.cpp
+++ b/src/imu.cpp
@@ -9,7 +9,7 @@
  *		  Author: gareth
  */
 
-#include "imu.hpp"
+#include <imu_3dm_gx4/imu.hpp>
 #include <chrono>
 #include <locale>
 #include <tuple>
@@ -51,7 +51,7 @@ extern "C" {
 //  base commands
 #define DEVICE_PING           u8(0x01)
 #define DEVICE_IDLE           u8(0x02)
-#define DEVICE_RESUME         u8(0x06) 
+#define DEVICE_RESUME         u8(0x06)
 
 //  3DM and FILTER commands
 #define COMMAND_GET_DEVICE_INFO       u8(0x03)
@@ -66,6 +66,11 @@ extern "C" {
 #define COMMAND_SET_SOFT_IRON         u8(0x3B)
 #define COMMAND_ENABLE_MEASUREMENTS   u8(0x41)
 #define COMMAND_DEVICE_STATUS         u8(0x64)
+#define COMMAND_POLL_IMU_DATA         u8(0x01)
+#define COMMAND_POLL_FILTER_DATA      u8(0x03)
+#define COMMAND_SAVE_IMU_FORMAT       u8(0x08)
+#define COMMAND_SAVE_FILTER_FORMAT    u8(0x0A)
+
 
 //  supported fields
 #define FIELD_QUATERNION        u8(0x03)
@@ -120,7 +125,7 @@ template <typename T> size_t encode(uint8_t *buffer, const T &t) {
   for (size_t i = 0; i < szT; i++) {
     buffer[i] = bytes[i];
   }
-  
+
   return szT;
 }
 
@@ -131,7 +136,7 @@ size_t encode(uint8_t *buffer, const T &t, const Ts &... ts) {
   return sz + encode(buffer + sizeof(T), ts...);
 }
 
-template <typename T> void decode(const uint8_t *buffer, size_t count, 
+template <typename T> void decode(const uint8_t *buffer, size_t count,
                                   T *output) {
   const size_t szT = sizeof(T);
   static_assert(std::is_fundamental<T>::value,
@@ -164,7 +169,7 @@ class PacketEncoder {
     //  no destruction without completion
     assert(!enc_);
   }
-  
+
   void beginField(uint8_t desc) {
     assert(!enc_);
     assert(p_.length < sizeof(p_.payload) - 2); //  2 bytes per field minimum
@@ -173,19 +178,19 @@ class PacketEncoder {
     p_.length += 2;
     enc_ = true;
   }
-  
+
   template <typename ...Ts>
   void append(const Ts& ...args) {
     assert(enc_); //  todo: check argument length here
     p_.length += encode(p_.payload+p_.length, args...);
   }
-  
+
   void endField() {
     assert(enc_);
     p_.payload[fs_] = p_.length - fs_;
     enc_ = false;
   }
-  
+
 private:
   Imu::Packet& p_;
   uint8_t fs_;
@@ -200,22 +205,22 @@ class PacketDecoder {
   PacketDecoder(const Imu::Packet& p) : p_(p), fs_(0), pos_(2) {
     assert(p.length > 0);
   }
-  
+
   int fieldDescriptor() const {
-    if (fs_ > sizeof(p_.payload)-2) { 
-      return -1; 
+    if (fs_ > sizeof(p_.payload)-2) {
+      return -1;
     }
-    if (p_.payload[fs_] == 0) { 
-      return -1;  //  no field 
+    if (p_.payload[fs_] == 0) {
+      return -1;  //  no field
     }
     return p_.payload[fs_ + 1]; //  descriptor after length
   }
-  
+
   int fieldLength() const {
     assert(fs_ < sizeof(p_.payload));
     return p_.payload[fs_];
   }
-  
+
   bool fieldIsAckOrNack() const {
     const int desc = fieldDescriptor();
     if (desc == static_cast<int>(FIELD_ACK_OR_NACK)) {
@@ -223,7 +228,7 @@ class PacketDecoder {
     }
     return false;
   }
-  
+
   bool advanceTo(uint8_t field) {
     for (int d; (d = fieldDescriptor()) > 0; advance()) {
       if (d == static_cast<int>(field)) {
@@ -232,12 +237,12 @@ class PacketDecoder {
     }
     return false;
   }
-  
+
   void advance() {
     fs_ += p_.payload[fs_];
     pos_=2; //  skip length and descriptor
   }
-  
+
   template <typename T>
   void extract(size_t count, T* output) {
     const size_t sz = sizeof(T)*count;
@@ -252,24 +257,24 @@ class PacketDecoder {
   uint8_t pos_;
 };
 
-bool Imu::Packet::isIMUData() const { 
+bool Imu::Packet::isIMUData() const {
   return descriptor == DATA_CLASS_IMU;
 }
 
-bool Imu::Packet::isFilterData() const { 
-  return descriptor == DATA_CLASS_FILTER; 
+bool Imu::Packet::isFilterData() const {
+  return descriptor == DATA_CLASS_FILTER;
 }
 
 int Imu::Packet::ackErrorCodeFor(const Packet &command) const {
   PacketDecoder decoder(*this);
   const uint8_t sentDesc = command.descriptor;
   const uint8_t sentField = command.payload[1];
-  
+
   if (sentDesc != this->descriptor) {
     //  not for this packet
     return -1;
   }
-  
+
   //  look for a matching ACK
   for (int d; (d = decoder.fieldDescriptor()) > 0; decoder.advance()) {
     if (decoder.fieldIsAckOrNack()) {
@@ -370,7 +375,7 @@ std::map <std::string, unsigned int> Imu::DiagnosticFields::toMap() const {
   return map;
 }
 
-Imu::command_error::command_error(const Packet& p, uint8_t code) : 
+Imu::command_error::command_error(const Packet& p, uint8_t code) :
   std::runtime_error(generateString(p, code)) {}
 
 std::string Imu::command_error::generateString(const Packet& p, uint8_t code) {
@@ -383,7 +388,7 @@ std::string Imu::command_error::generateString(const Packet& p, uint8_t code) {
 Imu::timeout_error::timeout_error(bool write, unsigned int to)
     : std::runtime_error(generateString(write,to)) {}
 
-std::string Imu::timeout_error::generateString(bool write, 
+std::string Imu::timeout_error::generateString(bool write,
                                                unsigned int to) {
   std::stringstream ss;
   ss << "Timed-out while " << ((write) ? "writing" : "reading") << ". ";
@@ -392,7 +397,7 @@ std::string Imu::timeout_error::generateString(bool write,
 }
 
 Imu::Imu(const std::string &device, bool verbose) : device_(device), verbose_(verbose),
-  fd_(0), 
+  fd_(0),
   rwTimeout_(kDefaultTimeout),
   srcIndex_(0), dstIndex_(0),
   state_(Idle) {
@@ -554,7 +559,7 @@ void Imu::selectBaudRate(unsigned int baud) {
     if (!termiosBaudRate(rates[i])) {
       throw io_error(strerror(errno));
     }
-    
+
     if (verbose_) {
       std::cout << "Switched baud rate to " << rates[i] << std::endl;
       std::cout << "Sending a ping packet.\n" << std::flush;
@@ -579,7 +584,7 @@ void Imu::selectBaudRate(unsigned int baud) {
     if (verbose_) {
       std::cout << "Found correct baudrate.\n" << std::flush;
     }
-    
+
     //  no error in receiveResponse, this is correct baud rate
     foundRate = true;
     break;
@@ -613,7 +618,7 @@ void Imu::selectBaudRate(unsigned int baud) {
     ss << e.what();
     throw std::runtime_error(ss.str());
   }
-  
+
   //  device has switched baud rate, now we should also
   if (!termiosBaudRate(baud)) {
     throw io_error(strerror(errno));
@@ -673,7 +678,7 @@ void Imu::getDeviceInfo(Imu::Info &info) {
     const bool advance = decoder.advanceTo(FIELD_DEVICE_INFO);
     assert(advance);
     char buffer[16];
-    
+
     decoder.extract(1, &info.firmwareVersion);
     //  decode all strings and trim left whitespace
     decoder.extract(sizeof(buffer), &buffer[0]);
@@ -739,19 +744,19 @@ void Imu::getDiagnosticInfo(Imu::DiagnosticFields &fields) {
   }
 }
 
-void Imu::setIMUDataRate(uint16_t decimation, 
+void Imu::setIMUDataRate(uint16_t decimation,
                         const std::bitset<4>& sources) {
   Imu::Packet p(COMMAND_CLASS_3DM);  //  was 0x04
   PacketEncoder encoder(p);
-  
+
   //  valid field descriptors: accel, gyro, mag, pressure
-  static const uint8_t fieldDescs[] = { FIELD_ACCELEROMETER, 
-                                        FIELD_GYROSCOPE, 
-                                        FIELD_MAGNETOMETER, 
+  static const uint8_t fieldDescs[] = { FIELD_ACCELEROMETER,
+                                        FIELD_GYROSCOPE,
+                                        FIELD_MAGNETOMETER,
                                         FIELD_BAROMETER };
   assert(sizeof(fieldDescs) == sources.size());
   std::vector<uint8_t> fields;
-  
+
   for (size_t i=0; i < sources.size(); i++) {
     if (sources[i]) {
       fields.push_back(fieldDescs[i]);
@@ -759,11 +764,11 @@ void Imu::setIMUDataRate(uint16_t decimation,
   }
   encoder.beginField(COMMAND_IMU_MESSAGE_FORMAT);
   encoder.append(FUNCTION_APPLY, u8(fields.size()));
-  
+
   for (const uint8_t& field : fields) {
     encoder.append(field, decimation);
   }
-  
+
   encoder.endField();
   p.calcChecksum();
   sendCommand(p);
@@ -772,23 +777,23 @@ void Imu::setIMUDataRate(uint16_t decimation,
 void Imu::setFilterDataRate(uint16_t decimation, const std::bitset<4>& sources) {
   Imu::Packet p(COMMAND_CLASS_3DM);  //  was 0x04
   PacketEncoder encoder(p);
- 
+
   static const uint8_t fieldDescs[] = { FIELD_QUATERNION,
                                         FIELD_GYRO_BIAS,
                                         FIELD_ANGLE_UNCERTAINTY,
                                         FIELD_BIAS_UNCERTAINTY };
   assert(sizeof(fieldDescs) == sources.size());
   std::vector<uint8_t> fields;
-  
+
   for (size_t i=0; i < sources.size(); i++) {
     if (sources[i]) {
       fields.push_back(fieldDescs[i]);
     }
   }
-  
+
   encoder.beginField(COMAMND_FILTER_MESSAGE_FORMAT);
   encoder.append(FUNCTION_APPLY, u8(fields.size()));
-  
+
   for (const uint8_t& field : fields) {
     encoder.append(field, decimation);
   }
@@ -821,7 +826,7 @@ void Imu::enableBiasEstimation(bool enabled) {
   uint16_t flag = 0xFFFE;
   if (enabled) {
     flag = 0xFFFF;
-  }  
+  }
   encoder.append(FUNCTION_APPLY, flag);
   encoder.endField();
   p.calcChecksum();
@@ -855,7 +860,7 @@ void Imu::setSoftIronMatrix(float matrix[9]) {
 
 void Imu::enableIMUStream(bool enabled) {
   Packet p(COMMAND_CLASS_3DM);
-  PacketEncoder encoder(p);  
+  PacketEncoder encoder(p);
   encoder.beginField(COMMAND_ENABLE_DATA_STREAM);
   encoder.append(FUNCTION_APPLY, SELECTOR_IMU);
   encoder.append(u8(enabled));
@@ -881,6 +886,47 @@ void Imu::enableFilterStream(bool enabled) {
   sendCommand(p);
 }
 
+void Imu::pollIMUData(bool enable_ack) {
+  Packet p(COMMAND_CLASS_3DM);
+  PacketEncoder encoder(p);
+  encoder.beginField(COMMAND_POLL_IMU_DATA);
+  //encoder.append(u8(!enable_ack), u8(0x00));
+  encoder.append(u8(0x00), u8(0x00));
+  encoder.endField();
+  p.calcChecksum();
+  sendCommand(p, false);
+}
+
+void Imu::pollFilterData(bool enable_ack) {
+  Packet p(COMMAND_CLASS_3DM);
+  PacketEncoder encoder(p);
+  encoder.beginField(COMMAND_POLL_FILTER_DATA);
+  encoder.append(u8(!enable_ack), u8(0x00));
+  encoder.endField();
+  p.calcChecksum();
+  sendCommand(p, false);
+}
+
+void Imu::saveIMUFormat() {
+  Packet p(COMMAND_CLASS_3DM);
+  PacketEncoder encoder(p);
+  encoder.beginField(COMMAND_SAVE_IMU_FORMAT);
+  encoder.append(u8(0x03), u8(0x00));
+  encoder.endField();
+  p.calcChecksum();
+  sendCommand(p);
+}
+
+void Imu::saveFilterFormat() {
+  Packet p(COMMAND_CLASS_3DM);
+  PacketEncoder encoder(p);
+  encoder.beginField(COMMAND_SAVE_FILTER_FORMAT);
+  encoder.append(u8(0x03), u8(0x00));
+  encoder.endField();
+  p.calcChecksum();
+  sendCommand(p);
+}
+
 void
 Imu::setIMUDataCallback(const std::function<void(const Imu::IMUData &)> &cb) {
   imuDataCallback_ = cb;
@@ -953,7 +999,7 @@ std::size_t Imu::handleByte(const uint8_t& byte, bool& found) {
     }
     else if (dstIndex_ == 2) {
       packet_.descriptor = byte;
-    } 
+    }
     else if (dstIndex_ == 3) {
       packet_.length = byte;
     }
@@ -976,7 +1022,7 @@ std::size_t Imu::handleByte(const uint8_t& byte, bool& found) {
         std::cout << "Warning: Dropped packet with mismatched checksum\n"
                   << std::flush;
         if (verbose_) {
-          std::cout << "Expected " << std::hex << 
+          std::cout << "Expected " << std::hex <<
                        static_cast<int>(packet_.checksum) << " but received " <<
                        static_cast<int>(sum) << std::endl;
           std::cout << "Packet content:\n" << packet_.toString() << std::endl;
@@ -995,7 +1041,7 @@ std::size_t Imu::handleByte(const uint8_t& byte, bool& found) {
       }
     }
   }
-  
+
   //  advance to next byte in packet
   dstIndex_++;
   return 0;
@@ -1014,7 +1060,7 @@ int Imu::handleRead(size_t bytes_transferred) {
   if (verbose_) {
     std::cout << ss.str() << std::flush;
   }
-  
+
   bool found = false;
   while (srcIndex_ < queue_.size() && !found) {
     const uint8_t head = queue_[srcIndex_];
@@ -1040,7 +1086,7 @@ void Imu::processPacket() {
   IMUData data;
   FilterData filterData;
   PacketDecoder decoder(packet_);
-  
+
   if (packet_.isIMUData()) {
     //  process all fields in the packet
     for (int d; (d = decoder.fieldDescriptor()) > 0; decoder.advance()) {
@@ -1116,7 +1162,7 @@ void Imu::processPacket() {
           //  error occurred
           std::cout << "Received NACK packet (class, command, code): ";
           std::cout << std::hex << static_cast<int>(packet_.descriptor) << ", ";
-          std::cout << static_cast<int>(cmd_code[0]) << ", "; 
+          std::cout << static_cast<int>(cmd_code[0]) << ", ";
           std::cout << static_cast<int>(cmd_code[1]) << "\n" << std::flush;
         }
       }
@@ -1184,7 +1230,7 @@ void Imu::receiveResponse(const Packet &command, unsigned int to) {
     if (resp > 0) {
       //  check if this is an ack
       const int ack = packet_.ackErrorCodeFor(command);
-      
+
       if (ack == 0) {
         return; //  success, exit
       } else if (ack > 0) {
@@ -1207,7 +1253,7 @@ void Imu::receiveResponse(const Packet &command, unsigned int to) {
     std::cout << command.toString() << std::endl << std::flush;
   }
   //  timed out
-  throw timeout_error(false, to);  
+  throw timeout_error(false, to);
 }
 
 void Imu::sendCommand(const Packet &p, bool readReply) {
diff --git a/src/imu_3dm_gx4.cpp b/src/imu_3dm_gx4_cont.cpp
similarity index 95%
rename from src/imu_3dm_gx4.cpp
rename to src/imu_3dm_gx4_cont.cpp
index 4ff5ade..98e410a 100644
--- a/src/imu_3dm_gx4.cpp
+++ b/src/imu_3dm_gx4_cont.cpp
@@ -10,7 +10,7 @@
 #include <geometry_msgs/QuaternionStamped.h>
 
 #include <imu_3dm_gx4/FilterOutput.h>
-#include "imu.hpp"
+#include <imu_3dm_gx4/imu.hpp>
 
 using namespace imu_3dm_gx4;
 
@@ -41,7 +41,7 @@ void publishData(const Imu::IMUData &data) {
   assert(data.fields & Imu::IMUData::Magnetometer);
   assert(data.fields & Imu::IMUData::Barometer);
   assert(data.fields & Imu::IMUData::Gyroscope);
-  
+
   //  timestamp identically
   imu.header.stamp = ros::Time::now();
   imu.header.frame_id = frameId;
@@ -80,7 +80,7 @@ void publishFilter(const Imu::FilterData &data) {
   assert(data.fields & Imu::FilterData::Bias);
   assert(data.fields & Imu::FilterData::AngleUnertainty);
   assert(data.fields & Imu::FilterData::BiasUncertainty);
-  
+
   imu_3dm_gx4::FilterOutput output;
   output.header.stamp = ros::Time::now();
   output.header.frame_id = frameId;
@@ -95,19 +95,19 @@ void publishFilter(const Imu::FilterData &data) {
   output.bias_covariance[0] = data.biasUncertainty[0]*data.biasUncertainty[0];
   output.bias_covariance[4] = data.biasUncertainty[1]*data.biasUncertainty[1];
   output.bias_covariance[8] = data.biasUncertainty[2]*data.biasUncertainty[2];
-  
+
   output.orientation_covariance[0] = data.angleUncertainty[0]*
       data.angleUncertainty[0];
   output.orientation_covariance[4] = data.angleUncertainty[1]*
       data.angleUncertainty[1];
   output.orientation_covariance[8] = data.angleUncertainty[2]*
       data.angleUncertainty[2];
-  
+
   output.quat_status = data.quaternionStatus;
   output.bias_status = data.biasStatus;
   output.orientation_covariance_status = data.angleUncertaintyStatus;
   output.bias_covariance_status = data.biasUncertaintyStatus;
-  
+
   pubFilter.publish(output);
   if (filterDiag) {
     filterDiag->tick(output.header.stamp);
@@ -118,11 +118,11 @@ std::shared_ptr<diagnostic_updater::TopicDiagnostic> configTopicDiagnostic(
     const std::string& name, double * target) {
   std::shared_ptr<diagnostic_updater::TopicDiagnostic> diag;
   const double period = 1.0 / *target;  //  for 1000Hz, period is 1e-3
-  
+
   diagnostic_updater::FrequencyStatusParam freqParam(target, target, 0.01, 10);
   diagnostic_updater::TimeStampStatusParam timeParam(0, period * 0.5);
-  diag.reset(new diagnostic_updater::TopicDiagnostic(name, 
-                                                     *updater, 
+  diag.reset(new diagnostic_updater::TopicDiagnostic(name,
+                                                     *updater,
                                                      freqParam,
                                                      timeParam));
   return diag;
@@ -135,11 +135,11 @@ void updateDiagnosticInfo(diagnostic_updater::DiagnosticStatusWrapper& stat,
   for (const std::pair<std::string,std::string>& p : map) {
     stat.add(p.first, p.second);
   }
-  
+
   try {
     //  try to read diagnostic info
     imu->getDiagnosticInfo(fields);
-    
+
     auto map = fields.toMap();
     for (const std::pair<std::string, unsigned int>& p : map) {
       stat.add(p.first, p.second);
@@ -162,7 +162,7 @@ int main(int argc, char **argv) {
   bool enableMagUpdate, enableAccelUpdate;
   int requestedImuRate, requestedFilterRate;
   bool verbose;
-  
+
   //  load parameters from launch file
   nh.param<std::string>("device", device, "/dev/ttyACM0");
   nh.param<int>("baudrate", baudrate, 115200);
@@ -173,12 +173,12 @@ int main(int argc, char **argv) {
   nh.param<bool>("enable_mag_update", enableMagUpdate, false);
   nh.param<bool>("enable_accel_update", enableAccelUpdate, true);
   nh.param<bool>("verbose", verbose, false);
-  
+
   if (requestedFilterRate < 0 || requestedImuRate < 0) {
     ROS_ERROR("imu_rate and filter_rate must be > 0");
     return -1;
   }
-  
+
   pubIMU = nh.advertise<sensor_msgs::Imu>("imu", 1);
   pubMag = nh.advertise<sensor_msgs::MagneticField>("magnetic_field", 1);
   pubPressure = nh.advertise<sensor_msgs::FluidPressure>("pressure", 1);
@@ -214,20 +214,20 @@ int main(int argc, char **argv) {
 
     //  calculate decimation rates
     if (static_cast<uint16_t>(requestedImuRate) > imuBaseRate) {
-      throw std::runtime_error("imu_rate cannot exceed " + 
+      throw std::runtime_error("imu_rate cannot exceed " +
                                std::to_string(imuBaseRate));
     }
     if (static_cast<uint16_t>(requestedFilterRate) > filterBaseRate) {
-      throw std::runtime_error("filter_rate cannot exceed " + 
+      throw std::runtime_error("filter_rate cannot exceed " +
                                std::to_string(filterBaseRate));
     }
-    
+
     const uint16_t imuDecimation = imuBaseRate / requestedImuRate;
     const uint16_t filterDecimation = filterBaseRate / requestedFilterRate;
-    
+
     ROS_INFO("Selecting IMU decimation: %u", imuDecimation);
     imu.setIMUDataRate(
-        imuDecimation, Imu::IMUData::Accelerometer | 
+        imuDecimation, Imu::IMUData::Accelerometer |
           Imu::IMUData::Gyroscope |
           Imu::IMUData::Magnetometer |
           Imu::IMUData::Barometer);
@@ -261,11 +261,11 @@ int main(int argc, char **argv) {
     if (!nh.hasParam("diagnostic_period")) {
       nh.setParam("diagnostic_period", 0.2);  //  5hz period
     }
-    
+
     updater.reset(new diagnostic_updater::Updater());
     const std::string hwId = info.modelName + "-" + info.modelNumber;
     updater->setHardwareID(hwId);
-    
+
     //  calculate the actual rates we will get
     double imuRate = imuBaseRate / (1.0 * imuDecimation);
     double filterRate = filterBaseRate / (1.0 * filterDecimation);
@@ -273,10 +273,10 @@ int main(int argc, char **argv) {
     if (enableFilter) {
       filterDiag = configTopicDiagnostic("filter",&filterRate);
     }
-    
-    updater->add("diagnostic_info", 
+
+    updater->add("diagnostic_info",
                  boost::bind(&updateDiagnosticInfo, _1, &imu));
-    
+
     ROS_INFO("Resuming the device");
     imu.resume();
 
diff --git a/src/imu_3dm_gx4_single.cpp b/src/imu_3dm_gx4_single.cpp
new file mode 100644
index 0000000..758c75d
--- /dev/null
+++ b/src/imu_3dm_gx4_single.cpp
@@ -0,0 +1,58 @@
+#include <ros/ros.h>
+#include <ros/node_handle.h>
+#include <boost/bind.hpp>
+#include <imu_3dm_gx4/imu_ros_base.hpp>
+
+using namespace imu_3dm_gx4;
+
+
+void imuTimerCallback(ImuRosBase* imu, const ros::TimerEvent& e) {
+  imu->requestIMUOnce();
+  return;
+}
+
+void filterTimerCallback(ImuRosBase* imu, const ros::TimerEvent& e) {
+  imu->requestFilterOnce();
+  return;
+}
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "imu_3dm_gx4");
+  ros::NodeHandle nh("~");
+
+  std::string device;
+  bool verbose;
+  int imu_rate, filter_rate;
+
+  // load parameters from launch file to
+  // create the device interface object
+  nh.param<std::string>("device", device, "/dev/ttyACM0");
+  nh.param<bool>("verbose", verbose, false);
+  nh.param<int>("imu_rate", imu_rate, 100);
+  nh.param<int>("filter_rate", filter_rate, 100);
+
+  // new instance of the IMU
+  ImuRosBase imu(nh, device, verbose);
+
+  // Initialize the device
+  if (!imu.initialize()) return -1;
+
+  // Create timers for triggering the device
+  ros::Timer imu_timer = nh.createTimer(
+      ros::Duration(1.0/static_cast<double>(imu_rate)),
+      boost::bind(&imuTimerCallback, &imu, _1));
+  ros::Timer filter_timer = nh.createTimer(
+      ros::Duration(1.0/static_cast<double>(filter_rate)),
+      boost::bind(&filterTimerCallback, &imu, _1));
+
+  // Read data
+  while (ros::ok()) {
+    ros::spinOnce();
+    imu.runOnce();
+  }
+
+  // Disconnect the device
+  imu.disconnect();
+
+  return 0;
+}
diff --git a/src/imu_ros_base.cpp b/src/imu_ros_base.cpp
new file mode 100644
index 0000000..d7d999e
--- /dev/null
+++ b/src/imu_ros_base.cpp
@@ -0,0 +1,307 @@
+/*
+ * imu_ros.cpp
+ *
+ *  Copyright (c) 2014 Kumar Robotics. All rights reserved.
+ *
+ *  This file is part of galt.
+ *
+ *  Created on: 08/04/2014
+ *		  Author: Ke
+ */
+
+#include <boost/bind.hpp>
+#include <imu_3dm_gx4/imu_ros_base.hpp>
+
+#define kEarthGravity (9.80665)
+using namespace imu_3dm_gx4;
+using namespace std;
+using namespace ros;
+
+ImuRosBase::ImuRosBase(const NodeHandle& n,
+    const string& device, const bool verbose):
+  nh(n),
+  imu(device, verbose) {
+  return;
+}
+
+/**
+ * @Note These three functions are wrappers for
+ * class member functions so that they can be used
+ * as callback functions.
+ */
+void publishIMUDataWrapper(ImuRosBase* imu_ros,
+    const Imu::IMUData& data) {
+  imu_ros->publishIMUData(data);
+  return;
+}
+
+void publishFilterDataWrapper(ImuRosBase* imu_ros,
+    const Imu::FilterData& data) {
+  imu_ros->publishFilterData(data);
+  return;
+}
+
+void updateDiagnosticInfoWrapper(ImuRosBase* imu_ros,
+    diagnostic_updater::DiagnosticStatusWrapper& stat,
+    imu_3dm_gx4::Imu* imu) {
+  imu_ros->updateDiagnosticInfo(stat, imu);
+  return;
+}
+
+bool ImuRosBase::initialize() {
+  // load parameters
+  if(!loadParameters()) return false;
+
+  // Create publishers
+  createPublishers();
+
+  // Try to connect IMU
+  try {
+    ROS_INFO("Connect to device");
+    imu.connect();
+
+    ROS_INFO("Selecting baud rate %u", baudrate);
+    imu.selectBaudRate(baudrate);
+
+    ROS_INFO("Fetching device info.");
+    imu.getDeviceInfo(info);
+    map<string, string> map = info.toMap();
+    for (const pair<string, string>& p : map) {
+      ROS_INFO("\t%s: %s", p.first.c_str(), p.second.c_str());
+    }
+
+    ROS_INFO("Idling the device");
+    imu.idle();
+
+    //  read back data rates
+    uint16_t imuBaseRate, filterBaseRate;
+    imu.getIMUDataBaseRate(imuBaseRate);
+    ROS_INFO("IMU data base rate: %u Hz", imuBaseRate);
+    imu.getFilterDataBaseRate(filterBaseRate);
+    ROS_INFO("Filter data base rate: %u Hz", filterBaseRate);
+
+    //  calculate decimation rates
+    if (static_cast<uint16_t>(requestedImuRate) > imuBaseRate) {
+      throw std::runtime_error("imu_rate cannot exceed " +
+          to_string(imuBaseRate));
+    }
+    if (static_cast<uint16_t>(requestedFilterRate) > filterBaseRate) {
+      throw std::runtime_error("filter_rate cannot exceed " +
+          to_string(filterBaseRate));
+    }
+
+    const uint16_t imuDecimation = imuBaseRate / requestedImuRate;
+    const uint16_t filterDecimation = filterBaseRate / requestedFilterRate;
+
+    ROS_INFO("Selecting IMU decimation: %u", imuDecimation);
+    imu.setIMUDataRate(
+        imuDecimation, Imu::IMUData::Accelerometer |
+          Imu::IMUData::Gyroscope |
+          Imu::IMUData::Magnetometer |
+          Imu::IMUData::Barometer);
+
+    ROS_INFO("Selecting filter decimation: %u", filterDecimation);
+    imu.setFilterDataRate(filterDecimation, Imu::FilterData::Quaternion |
+          Imu::FilterData::Bias |
+          Imu::FilterData::AngleUnertainty |
+          Imu::FilterData::BiasUncertainty);
+
+    // Set callback functions triggered by receiving new package
+    imu.setIMUDataCallback(boost::bind(&publishIMUDataWrapper, this, _1));
+    imu.setFilterDataCallback(boost::bind(&publishFilterDataWrapper, this, _1));
+
+    // Config the filter output if enabled
+    if (enableFilter) {
+      imu.enableMeasurements(enableAccelUpdate, enableMagUpdate);
+      imu.enableBiasEstimation(true);
+    }
+
+    // Save the IMU and filter data format
+    imu.saveIMUFormat();
+    imu.saveFilterFormat();
+
+    // configure diagnostic updater
+    if (!nh.hasParam("diagnostic_period")) {
+      nh.setParam("diagnostic_period", 0.2);  //  5hz period
+    }
+
+    updater.reset(new diagnostic_updater::Updater());
+    const string hwId = info.modelName + "-" + info.modelNumber;
+    updater->setHardwareID(hwId);
+
+    // calculate the actual rates we will get
+    double imuRate = imuBaseRate / (1.0 * imuDecimation);
+    double filterRate = filterBaseRate / (1.0 * filterDecimation);
+    imuDiag = configTopicDiagnostic("imu",&imuRate);
+    if (enableFilter) {
+      filterDiag = configTopicDiagnostic("filter",&filterRate);
+    }
+
+    updater->add("diagnostic_info",
+        boost::bind(&updateDiagnosticInfoWrapper, this, _1, &imu));
+
+  }
+  catch (Imu::io_error &e) {
+    ROS_ERROR("IO error: %s\n", e.what());
+    return false;
+  }
+  catch (Imu::timeout_error &e) {
+    ROS_ERROR("Timeout: %s\n", e.what());
+    return false;
+  }
+  catch (std::exception &e) {
+    ROS_ERROR("Exception: %s\n", e.what());
+    return false;
+  }
+  return true;
+}
+
+bool ImuRosBase::loadParameters() {
+
+  nh.param<int>("baudrate", baudrate, 115200);
+  nh.param<string>("frameId", frameId, std::string("imu"));
+  nh.param<int>("imu_rate", requestedImuRate, 100);
+  nh.param<int>("filter_rate", requestedFilterRate, 100);
+  nh.param<bool>("enable_filter", enableFilter, false);
+  nh.param<bool>("enable_mag_update", enableMagUpdate, false);
+  nh.param<bool>("enable_accel_update", enableAccelUpdate, true);
+
+  if (requestedFilterRate < 0 || requestedImuRate < 0) {
+    ROS_ERROR("imu_rate and filter_rate must be > 0");
+    return false;
+  }
+  return true;
+}
+
+void ImuRosBase::createPublishers(){
+  pubIMU = nh.advertise<sensor_msgs::Imu>("imu", 1);
+  pubMag = nh.advertise<sensor_msgs::MagneticField>("magnetic_field", 1);
+  pubPressure = nh.advertise<sensor_msgs::FluidPressure>("pressure", 1);
+
+  if (enableFilter) {
+    pubFilter = nh.advertise<imu_3dm_gx4::FilterOutput>("filter", 1);
+  }
+  return;
+}
+
+void ImuRosBase::publishIMUData(const Imu::IMUData& data){
+  sensor_msgs::Imu imu;
+  sensor_msgs::MagneticField field;
+  sensor_msgs::FluidPressure pressure;
+
+  //  assume we have all of these since they were requested
+  /// @todo: Replace this with a mode graceful failure...
+  assert(data.fields & Imu::IMUData::Accelerometer);
+  assert(data.fields & Imu::IMUData::Magnetometer);
+  assert(data.fields & Imu::IMUData::Barometer);
+  assert(data.fields & Imu::IMUData::Gyroscope);
+
+  //  timestamp identically
+  imu.header.stamp = ros::Time::now();
+  imu.header.frame_id = frameId;
+  field.header.stamp = imu.header.stamp;
+  field.header.frame_id = frameId;
+  pressure.header.stamp = imu.header.stamp;
+  pressure.header.frame_id = frameId;
+
+  imu.orientation_covariance[0] =
+      -1; //  orientation data is on a separate topic
+
+  imu.linear_acceleration.x = data.accel[0] * kEarthGravity;
+  imu.linear_acceleration.y = data.accel[1] * kEarthGravity;
+  imu.linear_acceleration.z = data.accel[2] * kEarthGravity;
+  imu.angular_velocity.x = data.gyro[0];
+  imu.angular_velocity.y = data.gyro[1];
+  imu.angular_velocity.z = data.gyro[2];
+
+  field.magnetic_field.x = data.mag[0];
+  field.magnetic_field.y = data.mag[1];
+  field.magnetic_field.z = data.mag[2];
+
+  pressure.fluid_pressure = data.pressure;
+
+  //  publish
+  pubIMU.publish(imu);
+  pubMag.publish(field);
+  pubPressure.publish(pressure);
+  if (imuDiag) {
+    imuDiag->tick(imu.header.stamp);
+  }
+  return;
+}
+
+void ImuRosBase::publishFilterData(const Imu::FilterData& data){
+  assert(data.fields & Imu::FilterData::Quaternion);
+  assert(data.fields & Imu::FilterData::Bias);
+  assert(data.fields & Imu::FilterData::AngleUnertainty);
+  assert(data.fields & Imu::FilterData::BiasUncertainty);
+
+  imu_3dm_gx4::FilterOutput output;
+  output.header.stamp = ros::Time::now();
+  output.header.frame_id = frameId;
+  output.orientation.w = data.quaternion[0];
+  output.orientation.x = data.quaternion[1];
+  output.orientation.y = data.quaternion[2];
+  output.orientation.z = data.quaternion[3];
+  output.bias.x = data.bias[0];
+  output.bias.y = data.bias[1];
+  output.bias.z = data.bias[2];
+
+  output.bias_covariance[0] = data.biasUncertainty[0]*data.biasUncertainty[0];
+  output.bias_covariance[4] = data.biasUncertainty[1]*data.biasUncertainty[1];
+  output.bias_covariance[8] = data.biasUncertainty[2]*data.biasUncertainty[2];
+
+  output.orientation_covariance[0] = data.angleUncertainty[0]*
+      data.angleUncertainty[0];
+  output.orientation_covariance[4] = data.angleUncertainty[1]*
+      data.angleUncertainty[1];
+  output.orientation_covariance[8] = data.angleUncertainty[2]*
+      data.angleUncertainty[2];
+
+  output.quat_status = data.quaternionStatus;
+  output.bias_status = data.biasStatus;
+  output.orientation_covariance_status = data.angleUncertaintyStatus;
+  output.bias_covariance_status = data.biasUncertaintyStatus;
+
+  pubFilter.publish(output);
+  if (filterDiag) {
+    filterDiag->tick(output.header.stamp);
+  }
+  return;
+}
+
+std::shared_ptr<diagnostic_updater::TopicDiagnostic> ImuRosBase::configTopicDiagnostic(
+    const std::string& name, double * target) {
+  std::shared_ptr<diagnostic_updater::TopicDiagnostic> diag;
+  const double period = 1.0 / *target;  //  for 1000Hz, period is 1e-3
+
+  diagnostic_updater::FrequencyStatusParam freqParam(target, target, 0.01, 10);
+  diagnostic_updater::TimeStampStatusParam timeParam(0, period * 0.5);
+  diag.reset(new diagnostic_updater::TopicDiagnostic(name,
+        *updater, freqParam, timeParam));
+  return diag;
+}
+
+void ImuRosBase::updateDiagnosticInfo(diagnostic_updater::DiagnosticStatusWrapper& stat,
+                          imu_3dm_gx4::Imu* imu) {
+  //  add base device info
+  std::map<std::string,std::string> map = info.toMap();
+  for (const std::pair<std::string,std::string>& p : map) {
+    stat.add(p.first, p.second);
+  }
+
+  try {
+    //  try to read diagnostic info
+    imu->getDiagnosticInfo(fields);
+
+    auto map = fields.toMap();
+    for (const std::pair<std::string, unsigned int>& p : map) {
+      stat.add(p.first, p.second);
+    }
+    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Read diagnostic info.");
+  }
+  catch (std::exception& e) {
+    const std::string message = std::string("Failed: ") + e.what();
+    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, message);
+  }
+}