From 07d33798a047eeb670d083daa65e2961ace9a667 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Thu, 27 Sep 2018 06:11:52 +0300
Subject: [PATCH] Add Python version of vl53l1x driver

---
 clever/launch/clever.launch |  6 ++++++
 clever/requirements.txt     |  2 ++
 clever/src/vl53l1x.py       | 35 +++++++++++++++++++++++++++++++++++
 3 files changed, 43 insertions(+)
 create mode 100755 clever/src/vl53l1x.py

diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch
index f64ace1c..95f4e2a7 100644
--- a/clever/launch/clever.launch
+++ b/clever/launch/clever.launch
@@ -10,6 +10,7 @@
     <arg name="aruco" default="false"/>
     <arg name="rc" value="true"/>
     <arg name="arduino" default="false"/>
+    <arg name="vl53l1x" default="false"/>
 
     <!-- mavros -->
     <include file="$(find clever)/launch/mavros.launch">
@@ -54,6 +55,11 @@
     <!-- rosbridge -->
     <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)">
+        <!-- <remap from="~range" to="mavros/distance_sensor/rangefinder_3_sub"/> -->
+    </node>
+
     <!-- rc backend -->
     <node name="rc" pkg="clever" type="rc" output="screen" if="$(arg rc)"/>
 
diff --git a/clever/requirements.txt b/clever/requirements.txt
index 579c0031..b067ca96 100644
--- a/clever/requirements.txt
+++ b/clever/requirements.txt
@@ -1,3 +1,5 @@
 flask==0.12.2
 geopy==1.11.0
 pymavlink==2.2.10
+smbus2==0.2.1
+VL53L1X==0.0.2
diff --git a/clever/src/vl53l1x.py b/clever/src/vl53l1x.py
new file mode 100755
index 00000000..78abfb7c
--- /dev/null
+++ b/clever/src/vl53l1x.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+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)
+
+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
+    range_pub.publish(msg)
+    r.sleep()
+
+tof.stop_ranging() # Stop ranging