From 801790426138eb9a746c06c10a49dd455b8e6451 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Tue, 9 Oct 2018 14:53:11 +0300
Subject: [PATCH] z_shift param in vl53 driver

---
 clever/launch/clever.launch | 1 +
 clever/src/vl53l1x.py       | 5 ++++-
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch
index 998ef361..102365ea 100644
--- a/clever/launch/clever.launch
+++ b/clever/launch/clever.launch
@@ -58,6 +58,7 @@
 
     <!-- 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>
 
diff --git a/clever/src/vl53l1x.py b/clever/src/vl53l1x.py
index 78abfb7c..346c382b 100755
--- a/clever/src/vl53l1x.py
+++ b/clever/src/vl53l1x.py
@@ -1,4 +1,6 @@
 #!/usr/bin/env python
+# TODO: rewrite, as Python version eats 20% CPU
+
 from __future__ import division
 
 import rospy
@@ -11,6 +13,7 @@ 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
@@ -28,7 +31,7 @@ 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
+    msg.range = tof.get_distance() / 1000 + z_shift
     range_pub.publish(msg)
     r.sleep()