diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt
index 523d79ca..a32e4da9 100644
--- a/clever/CMakeLists.txt
+++ b/clever/CMakeLists.txt
@@ -17,6 +17,9 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   sensor_msgs
   geographic_msgs
+  tf
+  tf2
+  tf2_geometry_msgs
 )
 
 
@@ -142,6 +145,10 @@ add_library(fcu_horiz
   src/fcu_horiz.cpp
 )
 
+add_library(aruco_vpe
+  src/aruco_vpe.cpp
+)
+
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
@@ -168,6 +175,10 @@ target_link_libraries(fcu_horiz
   "/opt/ros/kinetic/lib/libtf2_ros.so"
 )
 
+target_link_libraries(aruco_vpe
+  ${catkin_LIBRARIES}
+)
+
 #############
 ## Install ##
 #############
diff --git a/clever/launch/aruco.launch b/clever/launch/aruco.launch
index b29a81af..ea7e450b 100644
--- a/clever/launch/aruco.launch
+++ b/clever/launch/aruco.launch
@@ -15,5 +15,8 @@
         <!--<rosparam param="marker_ids">[6, 5, 4, 3, 2, 1]</rosparam>-->
     </node>
 
-    <node pkg="clever" type="aruco_vpe.py" name="aruco_vpe" output="screen"/>
+    <node pkg="nodelet" type="nodelet" name="aruco_vpe" args="load clever/aruco_vpe nodelet_manager" clear_params="true">
+        <param name="aruco_orientation" value="local_origin"/>
+        <!--<param name="aruco_orientation" value="local_origin_upside_down"/>-->
+    </node>
 </launch>
diff --git a/clever/nodelet_plugins.xml b/clever/nodelet_plugins.xml
index 86ef931a..9e1c2c67 100644
--- a/clever/nodelet_plugins.xml
+++ b/clever/nodelet_plugins.xml
@@ -3,3 +3,8 @@
       <description/>
    </class>
 </library>
+<library path="lib/libaruco_vpe">
+   <class name="clever/aruco_vpe" type="ArucoVPE" base_class_type="nodelet::Nodelet">
+      <description/>
+   </class>
+</library>
diff --git a/clever/src/aruco_vpe.cpp b/clever/src/aruco_vpe.cpp
index e737970b..547be33b 100644
--- a/clever/src/aruco_vpe.cpp
+++ b/clever/src/aruco_vpe.cpp
@@ -15,6 +15,7 @@
 using namespace tf2_ros;
 using geometry_msgs::PoseStamped;
 using geometry_msgs::TransformStamped;
+using std::string;
 
 class ArucoVPE : public nodelet::Nodelet
 {
@@ -29,12 +30,18 @@ private:
     ros::Duration lookup_timeout_;
     ros::Publisher vision_position_pub_;
     ros::Timer dummy_vision_timer_;
+    string aruco_orientation_;
 
     void onInit()
     {
         ros::NodeHandle& nh = getNodeHandle();
+        ros::NodeHandle& nh_priv = getPrivateNodeHandle();
+
         static ros::Subscriber pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this);
         vision_position_pub_ = nh.advertise<PoseStamped>("mavros/vision_pose/pose", 1);
+        nh_priv.param<std::string>("aruco_orientation", aruco_orientation_, "local_origin");
+
+        ROS_INFO("aruco orientation frame: %s", aruco_orientation_.c_str());
 
         dummy_vision_timer_ = nh.createTimer(ros::Duration(0.5), &ArucoVPE::publishDummy, this);
 
@@ -66,7 +73,7 @@ private:
         {
             // Refine aruco map pose
             // Reference in local origin
-            t = tf_buffer.lookupTransform("local_origin", "aruco_map_reference", stamp, lookup_timeout_);
+            t = tf_buffer.lookupTransform(aruco_orientation_, "aruco_map_reference", stamp, lookup_timeout_);
             quaternionToEuler(t.transform.rotation, roll, pitch, yaw);
             eulerToQuaternion(t.transform.rotation, 0, 0, yaw);
             t.child_frame_id = "aruco_map_reference_horiz";
diff --git a/clever/src/aruco_vpe.py b/clever/src/aruco_vpe.py
deleted file mode 100755
index aaadbd6a..00000000
--- a/clever/src/aruco_vpe.py
+++ /dev/null
@@ -1,106 +0,0 @@
-#!/usr/bin/env python
-
-import rospy
-from geometry_msgs.msg import PoseStamped, PointStamped, Quaternion
-import tf2_ros
-from tf2_geometry_msgs import do_transform_pose
-import tf.transformations
-
-from util import orientation_from_euler, euler_from_orientation
-
-
-rospy.init_node('aruco_vpe')
-
-
-LOOKUP_TIMEOUT = rospy.Duration(.05)
-CAMERA_FRAME_ID = rospy.get_param('~camera_frame_id', 'bottom_camera_optical')
-
-
-# TF2 stuff
-tf_broadcaster = tf2_ros.TransformBroadcaster()
-static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
-
-tf_buffer = tf2_ros.Buffer()
-tf_listener = tf2_ros.TransformListener(tf_buffer)
-
-
-vision_position_pub = rospy.Publisher('mavros/vision_pose/pose', PoseStamped, queue_size=1)
-_vision_position_pub = rospy.Publisher('fake_vision_pose', PoseStamped, queue_size=1)
-last_published = None
-
-
-q = Quaternion()
-q.w = 1
-ps = PoseStamped()
-ps.pose.orientation = q
-
-
-def send_transform(transform, child_frame_id):
-    transform.child_frame_id = child_frame_id
-    tf_broadcaster.sendTransform(transform)
-
-
-vpe_posted = False
-
-
-def publish_vpe(pose):
-    stamp = pose.header.stamp
-
-    global last_published, vpe_posted
-    vpe_posted = True
-
-    def lookup_transform(target_frame, source_frame):
-        return tf_buffer.lookup_transform(target_frame, source_frame, stamp, LOOKUP_TIMEOUT)
-
-    # Refine aruco_map
-    reference_in_local_origin = lookup_transform('local_origin', 'aruco_map_reference')
-    roll, pitch, yaw = euler_from_orientation(reference_in_local_origin.transform.rotation)
-    reference_in_local_origin.transform.rotation = orientation_from_euler(0, 0, yaw)
-    send_transform(reference_in_local_origin, 'aruco_map_reference_horiz')
-
-    aruco_map_in_reference = lookup_transform('aruco_map_reference', 'aruco_map_raw')
-    aruco_map_in_reference.header.frame_id = 'aruco_map_reference_horiz'
-    send_transform(aruco_map_in_reference, 'aruco_map_vision')
-
-    # Reset VPE
-    if last_published is None or stamp - last_published > rospy.Duration(2):
-        rospy.loginfo('Reset VPE')
-        aruco_map_in_local_origin = lookup_transform('local_origin', 'aruco_map_vision')
-        aruco_map_in_local_origin.child_frame_id = 'aruco_map'
-        static_tf_broadcaster.sendTransform(aruco_map_in_local_origin)
-
-    # Calculate VPE
-    ps.header.frame_id = 'fcu_horiz'
-    ps.header.stamp = stamp
-    vpe_raw = tf_buffer.transform(ps, 'aruco_map_vision', LOOKUP_TIMEOUT)
-    vpe_raw.header.frame_id = 'aruco_map'
-    vpe = tf_buffer.transform(vpe_raw, 'local_origin', LOOKUP_TIMEOUT)
-    _vision_position_pub.publish(vpe_raw)
-    vision_position_pub.publish(vpe)
-    last_published = stamp
-
-
-rospy.Subscriber('aruco_pose/pose', PoseStamped, publish_vpe, queue_size=1)
-
-
-local_pose = None
-
-
-def handle_pose(data):
-    global local_pose
-    local_pose = data
-
-
-rospy.Subscriber('mavros/local_position/pose', PoseStamped, handle_pose, queue_size=1)
-
-
-rospy.loginfo('aruco_vpe inited')
-r = rospy.Rate(5)
-
-
-while not rospy.is_shutdown():
-    if not vpe_posted:
-        ps.header.stamp = rospy.get_rostime()
-        vision_position_pub.publish(ps)
-
-    r.sleep()