diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch
index 8610bcb4..9b3ee831 100644
--- a/clever/launch/clever.launch
+++ b/clever/launch/clever.launch
@@ -8,8 +8,8 @@
     <arg name="optical_flow" default="false"/>
     <arg name="aruco" default="false"/>
     <arg name="rc" default="true"/>
+    <arg name="rangefinder_vl53l1x" default="false"/>
     <arg name="arduino" default="false"/>
-    <arg name="vl53l1x" default="false"/>
 
     <!-- mavros -->
     <include file="$(find clever)/launch/mavros.launch">
@@ -54,9 +54,10 @@
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
 
     <!-- vl53l1x ToF rangefinder -->
-    <node name="vl53l1x" pkg="clever" type="vl53l1x.py" output="screen" if="$(arg vl53l1x)">
-        <param name="z_shift" value="-0.05"/>
-        <!-- <remap from="~range" to="mavros/distance_sensor/rangefinder_3_sub"/> -->
+    <node name="vl53l1x" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
+        <param name="frame_id" value="rangefinder"/>
+        <param name="offset" value="-0.05"/>
+        <remap from="~range" to="mavros/distance_sensor/rangefinder_3_sub"/> <!-- redirect data to FCU -->
     </node>
 
     <!-- rc backend -->
diff --git a/clever/src/vl53l1x.py b/clever/src/vl53l1x.py
deleted file mode 100755
index 346c382b..00000000
--- a/clever/src/vl53l1x.py
+++ /dev/null
@@ -1,38 +0,0 @@
-#!/usr/bin/env python
-# TODO: rewrite, as Python version eats 20% CPU
-
-from __future__ import division
-
-import rospy
-import VL53L1X
-from sensor_msgs.msg import Range
-
-rospy.init_node('vl53l1x')
-
-
-# range_pub = rospy.Publisher('~range', Range, queue_size=5)
-# TODO: why remmaping is not working?
-range_pub = rospy.Publisher('mavros/distance_sensor/rangefinder_3_sub', Range, queue_size=10)
-z_shift = rospy.get_param("z_shift", 0)  # TODO: move to mavros (use frame)
-
-msg = Range()
-msg.radiation_type = Range.INFRARED
-msg.field_of_view = 0.471239
-msg.min_range = 0
-msg.max_range = 4
-msg.header.frame_id = 'rangefinder'
-
-tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
-tof.open() # Initialise the i2c bus and configure the sensor
-tof.start_ranging(3) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
-
-rospy.loginfo('vl53l1x: start ranging')
-
-r = rospy.Rate(14)
-while not rospy.is_shutdown():
-    msg.header.stamp = rospy.get_rostime()
-    msg.range = tof.get_distance() / 1000 + z_shift
-    range_pub.publish(msg)
-    r.sleep()
-
-tof.stop_ranging() # Stop ranging