diff --git a/VL53L1XtoDistAngle.py b/VL53L1XtoDistAngle.py
old mode 100644
new mode 100755
index 8e13bffb92d0123d2f88ce308030e61dc64f4054..7a54ca52c9202f5dc2b601f0c2e9e040e7b619fe
--- a/VL53L1XtoDistAngle.py
+++ b/VL53L1XtoDistAngle.py
@@ -1,133 +1,133 @@
-#!/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...')
+
+
+
+
+
+
+