Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
P
PAT
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Requirements
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Locked files
Build
Pipelines
Jobs
Pipeline schedules
Test cases
Artifacts
Deploy
Releases
Package registry
Container Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Code review analytics
Issue analytics
Insights
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Tobias Glaser
PAT
Commits
86403c8c
Commit
86403c8c
authored
2 years ago
by
Tobias Glaser
Browse files
Options
Downloads
Patches
Plain Diff
executable with rosrun
parent
bb6cbd28
Branches
master
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
VL53L1XtoDistAngle.py
+133
-133
133 additions, 133 deletions
VL53L1XtoDistAngle.py
with
133 additions
and
133 deletions
VL53L1XtoDistAngle.py
100644 → 100755
+
133
−
133
View file @
86403c8c
#!/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...
'
)
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment