CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Replace Python vl53l1x node to C++ vl53l1x_ros (from package)

Browse Source
This commit is contained in:
Oleg Kalachev
2019-01-03 13:36:33 +03:00
parent 77e172e623
commit d07eba683d
2 changed files with 5 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
clever/launch/clever.launch
View File

@@ -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 -->

38
clever/src/vl53l1x.py
View File

@@ -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