diff --git a/VL53L1XtoDistAngle.py b/VL53L1XtoDistAngle.py
index 011ee8238227873119e79005e306bc7eba68a745..8e13bffb92d0123d2f88ce308030e61dc64f4054 100644
--- a/VL53L1XtoDistAngle.py
+++ b/VL53L1XtoDistAngle.py
@@ -12,7 +12,8 @@
# 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
@@ -32,79 +33,101 @@ DEBUG_PRINT = False
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()----------------------------------------------------------------------
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-try:
- print("Python-Interpreter: {}\n".format(sys.version))
- i2c = smbus.SMBus(2) # I2C-Kommunikation aktivieren
- 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.')
+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...')
+
+
+
+
+
+
- # 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(1):
- 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')
-except KeyboardInterrupt:
- print(' ')
- i2c.close()
- print('...i2c-bus closed.')
- print('ByeBye')