Skip to content
Snippets Groups Projects
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
TOFx4.py 3.17 KiB
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# Beispiel-Skript für den STM TOF-Sensor VL53L1X
# Grundgeruest stammt aus VL53L1X library for Arduino von Pololu
# github.com/pololu/vl53l1x-arduino fuer Arduino.
# Version mit ROI-Zentrum in Richtung lange Seite des Sensors mit nur 4 Winkeln scannen.
# Da der ROI minimal 4 Pixel breit sein muss, kann dieser um bis zu 13 (überlappenden)
# Positionen verschoben werden. Ohne Überlappung nur um 4 Position = 4 Scanwinkel.
# Vom Sensor wird nur die Reichweite abgefragt, damit höhere Messrate.
# Modul VL53L1XRegAddr.py mit den Registeradressen sowie Modul tofSens.py
# müssen im selben Verzeichnis sein.
# Objektorientierte Variante des Skripts, Ausgabe Werte als Floats.
# S. Mack, 14.11.22


import rospy
from std_msgs.msg import UInt16MultiArray
import smbus
import sys
from time import sleep
import VL53L1XRegAddr as REG
from tofSens import TofSens, countRateFixedToFloat, writeReg, writeReg16Bit, writeReg32Bit, readReg, readReg16Bit

def talker():
    pub = rospy.Publisher('range_val', UInt16MultiArray, queue_size=10)
    rospy.init_node('TOFx4', anonymous=False)
    rate = rospy.Rate(10) # maximum rate 10 Hz due to sensor response time
    
    try:
        my_tof = TofSens(i2c, ic2_addr=0x29, timeout_ms=500)
        if not my_tof.initSensor(True): 
            print('Failed to detect and initialize sensor!')
        else:
            print('Sensor Initialisierung i.O.')
        
        # The minimum timing budget is 20 ms for short distance mode and 33 ms for
        # medium and long distance modes. See the VL53L1X datasheet for more
        # information on range and timing limits.
        my_tof.setDistanceMode(0) # 0=short, 1=medium, 2=long
        my_tof.setMeasurementTimingBudget(20000) # µs nicht! ms eingeben!
        # Groesse des ROI festlegen, x=horizontal Laengsrichtung Sensorchip
        my_tof.setROISize(4,16) # x min 4, y (Hoehe) min 4 max 16
        
        while not rospy.is_shutdown():
            ranges = []
            # 13 Scanwinkel 0...12 = range(13), 4 Scanwinkel 0,4,8,12 = range(0,13,4)
            for angle in range(0,13,4):
                roi_center = 8*angle + 151
                my_tof.setROICenter(roi_center) # ROI setzen
                my_tof.sensorReadSingle(blocking=True) # Abstandswert auslesen (blocking und nur Reichweite)       
                ranges.append(round(my_tof.ranging_data['range_mm'],0))
            print('Range (mm): ',end='')
            print(*ranges, sep=', ')
            
            pub.publish(UInt16MultiArray(data=ranges))
            rate.sleep()
    
    except KeyboardInterrupt:
        print('KeyboardInterrupt')

# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# main()----------------------------------------------------------------------
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

if __name__ == '__main__':
    try:
        print('TOFx4 startet...')
        i2c = smbus.SMBus(2) # I2C-Kommunikation aktivieren
        talker()
    except rospy.ROSInterruptException:
        print('ROSInterruptException')
        pass
    finally:
        i2c.close()
        print('...i2c-bus closed.')
        print('Byebye...')