diff --git a/uwrt_mars_rover_navigation/vectornav/CMakeLists.txt b/uwrt_mars_rover_navigation/vectornav/CMakeLists.txt
index c4643754..8d3a63c1 100644
--- a/uwrt_mars_rover_navigation/vectornav/CMakeLists.txt
+++ b/uwrt_mars_rover_navigation/vectornav/CMakeLists.txt
@@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -41,6 +43,22 @@ target_link_libraries( ${PROJECT_NAME}
 rclcpp_components_register_nodes(${PROJECT_NAME} "vectornav::VectornavNode")
 set(node_plugs "${node_plugins}vectornav::VectornavNode;$<TARGET_FILE:${PROJECT_NAME}>\n")
 
+# testing
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(${PROJECT_NAME}_test test/test.cpp)
+  target_include_directories(${PROJECT_NAME}_test PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test
+    rclcpp
+    geometry_msgs 
+  )
+  target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME} vncxx)
+endif()
+
+
 # install nodes
 install(
   TARGETS ${PROJECT_NAME}
diff --git a/uwrt_mars_rover_navigation/vectornav/include/vectornav/vectornav_node.hpp b/uwrt_mars_rover_navigation/vectornav/include/vectornav/vectornav_node.hpp
index 675744f1..18ecd65c 100644
--- a/uwrt_mars_rover_navigation/vectornav/include/vectornav/vectornav_node.hpp
+++ b/uwrt_mars_rover_navigation/vectornav/include/vectornav/vectornav_node.hpp
@@ -18,14 +18,12 @@ namespace vectornav {
 class VectornavNode : public rclcpp::Node {
  public:
   VectornavNode(const rclcpp::NodeOptions& options);
-  ~VectornavNode();
+  ~VectornavNode() override;
  private:
   // function
   void publish();
 
   rclcpp::Time getTimeStamp(vn::sensors::CompositeData& data);
-  geometry_msgs::msg::Quaternion toMsg(const vn::math::vec4f &rhs);
-  geometry_msgs::msg::Vector3 toMsg(const vn::math::vec3f & rhs);
 
   void publishImu(vn::sensors::CompositeData &data);
   void publishGps(vn::sensors::CompositeData &data);
@@ -45,4 +43,7 @@ class VectornavNode : public rclcpp::Node {
   std::chrono::milliseconds delay{static_cast<long int>(1000 / rate)};
 };
 
+geometry_msgs::msg::Quaternion toMsg(const vn::math::vec4f &rhs);
+geometry_msgs::msg::Vector3 toMsg(const vn::math::vec3f & rhs);
+
 } // namespace vectornav
diff --git a/uwrt_mars_rover_navigation/vectornav/src/vectornav_node.cpp b/uwrt_mars_rover_navigation/vectornav/src/vectornav_node.cpp
index 0538f0f2..0f7074c9 100644
--- a/uwrt_mars_rover_navigation/vectornav/src/vectornav_node.cpp
+++ b/uwrt_mars_rover_navigation/vectornav/src/vectornav_node.cpp
@@ -47,7 +47,7 @@ namespace vectornav {
   }
 
   // Convert from vn::math::vec4f to geometry_msgs::msgs::Quaternion
-  geometry_msgs::msg::Quaternion VectornavNode::toMsg(const vn::math::vec4f & rhs) {
+  geometry_msgs::msg::Quaternion toMsg(const vn::math::vec4f & rhs) {
     geometry_msgs::msg::Quaternion lhs;
     lhs.x = rhs[0];
     lhs.y = rhs[1];
@@ -57,7 +57,7 @@ namespace vectornav {
   }
 
   // Convert from vn::math::vec3f to geometry_msgs::msgs::Vector3
-  geometry_msgs::msg::Vector3 VectornavNode::toMsg(const vn::math::vec3f & rhs) {
+  geometry_msgs::msg::Vector3 toMsg(const vn::math::vec3f & rhs) {
     geometry_msgs::msg::Vector3 lhs;
     lhs.x = rhs[0];
     lhs.y = rhs[1];
@@ -79,17 +79,17 @@ namespace vectornav {
 
     // QUATERNION
     if (data.hasQuaternion()) {
-      message->orientation = this->toMsg(data.quaternion());
+      message->orientation = toMsg(data.quaternion());
     }
 
     // ANGULAR RATE
     if (data.hasAngularRate()) {
-      message->angular_velocity = this->toMsg(data.angularRate());
+      message->angular_velocity = toMsg(data.angularRate());
     }
 
     // ACCELERATION
     if (data.hasAcceleration()) {
-      message->linear_acceleration = this->toMsg(data.acceleration());
+      message->linear_acceleration = toMsg(data.acceleration());
     }
     this->publisher_imu_->publish(std::move(message));
   }
@@ -123,7 +123,7 @@ namespace vectornav {
     }
 
     if (data.hasPositionUncertaintyGpsNed()) {
-      geometry_msgs::msg::Vector3 uncertainty = this->toMsg(data.positionUncertaintyGpsNed());
+      geometry_msgs::msg::Vector3 uncertainty = toMsg(data.positionUncertaintyGpsNed());
       
       message->position_covariance = {uncertainty.y, 0.0000, 0.0000, 0.0000, uncertainty.x, 0.0000, 0.0000, 0.0000, uncertainty.z};
       message->position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
diff --git a/uwrt_mars_rover_navigation/vectornav/test/test.cpp b/uwrt_mars_rover_navigation/vectornav/test/test.cpp
new file mode 100644
index 00000000..dc4fa11e
--- /dev/null
+++ b/uwrt_mars_rover_navigation/vectornav/test/test.cpp
@@ -0,0 +1,43 @@
+#include <string>
+#include "vectornav/vectornav_node.hpp"
+#include <gtest/gtest.h>
+#include "rclcpp/rclcpp.hpp"
+
+TEST(vectornav, to_msg_quaternion) {
+
+  rclcpp::NodeOptions options;
+
+  rclcpp::init(0, nullptr);
+  vectornav::VectornavNode vnode{options};
+  rclcpp::shutdown();
+
+  geometry_msgs::msg::Quaternion quat;
+  quat.x = 1;
+  quat.y = 2;
+  quat.z = 3;
+  quat.w = 4;
+  vn::math::vec4f vec4 {1, 2, 3, 4};
+
+  EXPECT_EQ(quat, vectornav::toMsg(vec4));
+}
+
+TEST(vectornav, to_msg_vector3) {
+  rclcpp::NodeOptions options;
+
+  rclcpp::init(0, nullptr);
+  vectornav::VectornavNode vnode{options};
+  rclcpp::shutdown();
+
+  geometry_msgs::msg::Vector3 tri;
+  tri.x = 1;
+  tri.y = 2;
+  tri.z = 3;
+  vn::math::vec3f vec3 {1, 2, 3};
+
+  EXPECT_EQ(tri, vectornav::toMsg(vec3));
+}
+
+int main(int argc, char **argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}