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

executable with rosrun

parent bb6cbd28
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 133 additions and 133 deletions
VL53L1XtoDistAngle.py 100644 → 100755
+ 133
− 133
View file @ 86403c8c
#!/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 geometry_msgs.msg import Pose2D
import smbus
import sys
import math
import numpy as np
from time import sleep
import VL53L1XRegAddr as REG
from tofSens import TofSens, countRateFixedToFloat, writeReg, writeReg16Bit, writeReg32Bit, readReg, readReg16Bit
DEBUG_PRINT = False
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# Variablen ------------------------------------------------------------------
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
calibrated = False;
def talker():
pub = rospy.Publisher('pose', Pose2D, queue_size=10)
rospy.init_node('TOFxPose', 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))
ranges[0] += 9
ranges[1] += 5
position_x = [0,0,0,0]
position_y = [0,0,0,0]
angles = [-10.8, -3.6, 3.6, 10.8]
steigung = 0
mean_x1 = 0
mean_y1 = 0
parameter_zaehler = 0
parameter_nenner = 0
for i in range(0,4):
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)
mean_x1 += position_x[i]
mean_y1 += position_y[i]
if (i==3):
mean_x = mean_x1/4
mean_y = mean_y1/4
for i in range(0,4):
parameter_zaehler += (position_x[i] - mean_x) * (position_y[i] - mean_y)
parameter_nenner += (position_x[i] - mean_x)**2
parameter_b = parameter_zaehler/parameter_nenner
steigung = mean_y - parameter_b * mean_x
line = np.polyfit(position_x, position_y,1)
p = np.poly1d(line)
winkel = (180/math.pi)*np.arctan(parameter_b)
#winkel_2 = (180/math.pi)*math.atan(p[1])
winkel_test = math.atan(p[1])
winkel_2 = math.degrees(winkel_test)
#print('Range (mm): ',end='')
#print(*ranges, sep=', ')
#print(parameter_b)
#print(ranges)
#print(p, 'polyfit')
#print(p[1],'polyfit steigung')
#print(parameter_b,'steigung berechnet')
#print(winkel,'winkel berechnet')
print('Abstand',p[0])
print('winkel ',winkel_2)
#print('x', position_x)
#print('y', position_y)
#print('range', ranges)
#print('Angle (°): -10.8, -3.6, 3.6, 10.8')
pub.publish(Pose2D(x=p[0], y=0, theta=winkel_2))
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...')
#!/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 geometry_msgs.msg import Pose2D
import smbus
import sys
import math
import numpy as np
from time import sleep
import VL53L1XRegAddr as REG
from tofSens import TofSens, countRateFixedToFloat, writeReg, writeReg16Bit, writeReg32Bit, readReg, readReg16Bit
DEBUG_PRINT = False
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# Variablen ------------------------------------------------------------------
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
calibrated = False;
def talker():
pub = rospy.Publisher('pose', Pose2D, queue_size=10)
rospy.init_node('TOFxPose', 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))
ranges[0] += 9
ranges[1] += 5
position_x = [0,0,0,0]
position_y = [0,0,0,0]
angles = [-10.8, -3.6, 3.6, 10.8]
steigung = 0
mean_x1 = 0
mean_y1 = 0
parameter_zaehler = 0
parameter_nenner = 0
for i in range(0,4):
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)
mean_x1 += position_x[i]
mean_y1 += position_y[i]
if (i==3):
mean_x = mean_x1/4
mean_y = mean_y1/4
for i in range(0,4):
parameter_zaehler += (position_x[i] - mean_x) * (position_y[i] - mean_y)
parameter_nenner += (position_x[i] - mean_x)**2
parameter_b = parameter_zaehler/parameter_nenner
steigung = mean_y - parameter_b * mean_x
line = np.polyfit(position_x, position_y,1)
p = np.poly1d(line)
winkel = (180/math.pi)*np.arctan(parameter_b)
#winkel_2 = (180/math.pi)*math.atan(p[1])
winkel_test = math.atan(p[1])
winkel_2 = math.degrees(winkel_test)
#print('Range (mm): ',end='')
#print(*ranges, sep=', ')
#print(parameter_b)
#print(ranges)
#print(p, 'polyfit')
#print(p[1],'polyfit steigung')
#print(parameter_b,'steigung berechnet')
#print(winkel,'winkel berechnet')
print('Abstand',p[0])
print('winkel ',winkel_2)
#print('x', position_x)
#print('y', position_y)
#print('range', ranges)
#print('Angle (°): -10.8, -3.6, 3.6, 10.8')
pub.publish(Pose2D(x=p[0], y=0, theta=winkel_2))
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...')
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment