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

publishing Pose2D messaged to /pose

parent f06d270f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 95 additions and 72 deletions
VL53L1XtoDistAngle.py
+ 95
− 72
View file @ bb6cbd28
...@@ -12,7 +12,8 @@ ...@@ -12,7 +12,8 @@
# 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
from geometry_msgs.msg import Pose2D
import smbus import smbus
import sys import sys
import math import math
...@@ -32,79 +33,101 @@ DEBUG_PRINT = False ...@@ -32,79 +33,101 @@ DEBUG_PRINT = False
calibrated = 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()---------------------------------------------------------------------- # main()----------------------------------------------------------------------
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
try: if __name__ == '__main__':
print("Python-Interpreter: {}\n".format(sys.version)) try:
i2c = smbus.SMBus(2) # I2C-Kommunikation aktivieren print('TOFx4 startet...')
my_tof = TofSens(i2c, ic2_addr=0x29, timeout_ms=500) i2c = smbus.SMBus(2) # I2C-Kommunikation aktivieren
if not my_tof.initSensor(True): talker()
print('Failed to detect and initialize sensor!') except rospy.ROSInterruptException:
else: print('ROSInterruptException')
print('Sensor Initialisierung i.O.') 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')
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