Skip to content
Snippets Groups Projects
Commit 86403c8c authored by Tobias Glaser's avatar Tobias Glaser
Browse files

executable with rosrun

parent bb6cbd28
Branches master
No related tags found
No related merge requests found
#!/usr/bin/env python3 #!/usr/bin/env python3
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
# Beispiel-Skript für den STM TOF-Sensor VL53L1X # Beispiel-Skript für den STM TOF-Sensor VL53L1X
# Grundgeruest stammt aus VL53L1X library for Arduino von Pololu # Grundgeruest stammt aus VL53L1X library for Arduino von Pololu
# github.com/pololu/vl53l1x-arduino fuer Arduino. # github.com/pololu/vl53l1x-arduino fuer Arduino.
# Version mit ROI-Zentrum in Richtung lange Seite des Sensors mit nur 4 Winkeln scannen. # 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) # 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. # Positionen verschoben werden. Ohne Überlappung nur um 4 Position = 4 Scanwinkel.
# Vom Sensor wird nur die Reichweite abgefragt, damit höhere Messrate. # Vom Sensor wird nur die Reichweite abgefragt, damit höhere Messrate.
# Modul VL53L1XRegAddr.py mit den Registeradressen sowie Modul tofSens.py # Modul VL53L1XRegAddr.py mit den Registeradressen sowie Modul tofSens.py
# müssen im selben Verzeichnis sein. # müssen im selben Verzeichnis sein.
# Objektorientierte Variante des Skripts, Ausgabe Werte als Floats. # Objektorientierte Variante des Skripts, Ausgabe Werte als Floats.
# S. Mack, 14.11.22 # S. Mack, 14.11.22
import rospy import rospy
from geometry_msgs.msg import Pose2D from geometry_msgs.msg import Pose2D
import smbus import smbus
import sys import sys
import math import math
import numpy as np import numpy as np
from time import sleep from time import sleep
import VL53L1XRegAddr as REG import VL53L1XRegAddr as REG
from tofSens import TofSens, countRateFixedToFloat, writeReg, writeReg16Bit, writeReg32Bit, readReg, readReg16Bit from tofSens import TofSens, countRateFixedToFloat, writeReg, writeReg16Bit, writeReg32Bit, readReg, readReg16Bit
DEBUG_PRINT = False DEBUG_PRINT = False
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# Variablen ------------------------------------------------------------------ # Variablen ------------------------------------------------------------------
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
calibrated = False; calibrated = False;
def talker(): def talker():
pub = rospy.Publisher('pose', Pose2D, queue_size=10) pub = rospy.Publisher('pose', Pose2D, queue_size=10)
rospy.init_node('TOFxPose', anonymous=False) rospy.init_node('TOFxPose', anonymous=False)
rate = rospy.Rate(10) # maximum rate 10 Hz due to sensor response time rate = rospy.Rate(10) # maximum rate 10 Hz due to sensor response time
try: try:
my_tof = TofSens(i2c, ic2_addr=0x29, timeout_ms=500) my_tof = TofSens(i2c, ic2_addr=0x29, timeout_ms=500)
if not my_tof.initSensor(True): if not my_tof.initSensor(True):
print('Failed to detect and initialize sensor!') print('Failed to detect and initialize sensor!')
else: else:
print('Sensor Initialisierung i.O.') print('Sensor Initialisierung i.O.')
# The minimum timing budget is 20 ms for short distance mode and 33 ms for # 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 # medium and long distance modes. See the VL53L1X datasheet for more
# information on range and timing limits. # information on range and timing limits.
my_tof.setDistanceMode(0) # 0=short, 1=medium, 2=long my_tof.setDistanceMode(0) # 0=short, 1=medium, 2=long
my_tof.setMeasurementTimingBudget(20000) # µs nicht! ms eingeben! my_tof.setMeasurementTimingBudget(20000) # µs nicht! ms eingeben!
# Groesse des ROI festlegen, x=horizontal Laengsrichtung Sensorchip # Groesse des ROI festlegen, x=horizontal Laengsrichtung Sensorchip
my_tof.setROISize(4,16) # x min 4, y (Hoehe) min 4 max 16 my_tof.setROISize(4,16) # x min 4, y (Hoehe) min 4 max 16
while not rospy.is_shutdown(): while not rospy.is_shutdown():
ranges = [] ranges = []
# 13 Scanwinkel 0...12 = range(13), 4 Scanwinkel 0,4,8,12 = range(0,13,4) # 13 Scanwinkel 0...12 = range(13), 4 Scanwinkel 0,4,8,12 = range(0,13,4)
for angle in range(0,13,4): for angle in range(0,13,4):
roi_center = 8*angle + 151; roi_center = 8*angle + 151;
my_tof.setROICenter(roi_center) # ROI setzen my_tof.setROICenter(roi_center) # ROI setzen
my_tof.sensorReadSingle(blocking=True) # Abstandswert auslesen (blocking und nur Reichweite) my_tof.sensorReadSingle(blocking=True) # Abstandswert auslesen (blocking und nur Reichweite)
ranges.append(round(my_tof.ranging_data['range_mm'],0)) ranges.append(round(my_tof.ranging_data['range_mm'],0))
ranges[0] += 9 ranges[0] += 9
ranges[1] += 5 ranges[1] += 5
position_x = [0,0,0,0] position_x = [0,0,0,0]
position_y = [0,0,0,0] position_y = [0,0,0,0]
angles = [-10.8, -3.6, 3.6, 10.8] angles = [-10.8, -3.6, 3.6, 10.8]
steigung = 0 steigung = 0
mean_x1 = 0 mean_x1 = 0
mean_y1 = 0 mean_y1 = 0
parameter_zaehler = 0 parameter_zaehler = 0
parameter_nenner = 0 parameter_nenner = 0
for i in range(0,4): for i in range(0,4):
position_y[i] = round(ranges[i]*math.cos(math.radians(angles[i])),2) position_y[i] = round(ranges[i]*math.cos(math.radians(angles[i])),2)
position_x[i] = round(ranges[i]*math.sin(math.radians(angles[i])),2) position_x[i] = round(ranges[i]*math.sin(math.radians(angles[i])),2)
mean_x1 += position_x[i] mean_x1 += position_x[i]
mean_y1 += position_y[i] mean_y1 += position_y[i]
if (i==3): if (i==3):
mean_x = mean_x1/4 mean_x = mean_x1/4
mean_y = mean_y1/4 mean_y = mean_y1/4
for i in range(0,4): for i in range(0,4):
parameter_zaehler += (position_x[i] - mean_x) * (position_y[i] - mean_y) parameter_zaehler += (position_x[i] - mean_x) * (position_y[i] - mean_y)
parameter_nenner += (position_x[i] - mean_x)**2 parameter_nenner += (position_x[i] - mean_x)**2
parameter_b = parameter_zaehler/parameter_nenner parameter_b = parameter_zaehler/parameter_nenner
steigung = mean_y - parameter_b * mean_x steigung = mean_y - parameter_b * mean_x
line = np.polyfit(position_x, position_y,1) line = np.polyfit(position_x, position_y,1)
p = np.poly1d(line) p = np.poly1d(line)
winkel = (180/math.pi)*np.arctan(parameter_b) winkel = (180/math.pi)*np.arctan(parameter_b)
#winkel_2 = (180/math.pi)*math.atan(p[1]) #winkel_2 = (180/math.pi)*math.atan(p[1])
winkel_test = math.atan(p[1]) winkel_test = math.atan(p[1])
winkel_2 = math.degrees(winkel_test) winkel_2 = math.degrees(winkel_test)
#print('Range (mm): ',end='') #print('Range (mm): ',end='')
#print(*ranges, sep=', ') #print(*ranges, sep=', ')
#print(parameter_b) #print(parameter_b)
#print(ranges) #print(ranges)
#print(p, 'polyfit') #print(p, 'polyfit')
#print(p[1],'polyfit steigung') #print(p[1],'polyfit steigung')
#print(parameter_b,'steigung berechnet') #print(parameter_b,'steigung berechnet')
#print(winkel,'winkel berechnet') #print(winkel,'winkel berechnet')
print('Abstand',p[0]) print('Abstand',p[0])
print('winkel ',winkel_2) print('winkel ',winkel_2)
#print('x', position_x) #print('x', position_x)
#print('y', position_y) #print('y', position_y)
#print('range', ranges) #print('range', ranges)
#print('Angle (°): -10.8, -3.6, 3.6, 10.8') #print('Angle (°): -10.8, -3.6, 3.6, 10.8')
pub.publish(Pose2D(x=p[0], y=0, theta=winkel_2)) pub.publish(Pose2D(x=p[0], y=0, theta=winkel_2))
rate.sleep() rate.sleep()
except KeyboardInterrupt: except KeyboardInterrupt:
print('KeyboardInterrupt') print('KeyboardInterrupt')
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# main()---------------------------------------------------------------------- # main()----------------------------------------------------------------------
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if __name__ == '__main__': if __name__ == '__main__':
try: try:
print('TOFx4 startet...') print('TOFx4 startet...')
i2c = smbus.SMBus(2) # I2C-Kommunikation aktivieren i2c = smbus.SMBus(2) # I2C-Kommunikation aktivieren
talker() talker()
except rospy.ROSInterruptException: except rospy.ROSInterruptException:
print('ROSInterruptException') print('ROSInterruptException')
pass pass
finally: finally:
i2c.close() i2c.close()
print('...i2c-bus closed.') print('...i2c-bus closed.')
print('Byebye...') print('Byebye...')
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment