Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
L
lrs_pyutil
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Iterations
Wiki
Requirements
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Locked files
Build
Pipelines
Jobs
Pipeline schedules
Test cases
Artifacts
Deploy
Releases
Package 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
Terms and privacy
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
lrs
lrs_pyutil
Commits
910e9349
Commit
910e9349
authored
2 years ago
by
Piotr Rudol
Browse files
Options
Downloads
Patches
Plain Diff
added decoder script which produces the necessary data for IP based on QR code info
parent
47ac8c56
No related branches found
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/add_qr.py
+26
-24
26 additions, 24 deletions
src/add_qr.py
src/decode_qr_inference.py
+154
-0
154 additions, 0 deletions
src/decode_qr_inference.py
with
180 additions
and
24 deletions
src/add_qr.py
+
26
−
24
View file @
910e9349
...
@@ -56,10 +56,10 @@ def json_to_transform(jobj):
...
@@ -56,10 +56,10 @@ def json_to_transform(jobj):
res
.
translation
.
x
=
jobj
[
"
tra
"
][
"
x
"
]
res
.
translation
.
x
=
jobj
[
"
tra
"
][
"
x
"
]
res
.
translation
.
y
=
jobj
[
"
tra
"
][
"
y
"
]
res
.
translation
.
y
=
jobj
[
"
tra
"
][
"
y
"
]
res
.
translation
.
z
=
jobj
[
"
tra
"
][
"
z
"
]
res
.
translation
.
z
=
jobj
[
"
tra
"
][
"
z
"
]
res
.
rotation
.
x
=
jobj
[
"
rot
"
][
"
x
"
]
res
.
rotation
.
x
=
jobj
[
"
rot
"
][
"
x
"
]
res
.
rotation
.
y
=
jobj
[
"
rot
"
][
"
y
"
]
res
.
rotation
.
y
=
jobj
[
"
rot
"
][
"
y
"
]
res
.
rotation
.
z
=
jobj
[
"
rot
"
][
"
z
"
]
res
.
rotation
.
z
=
jobj
[
"
rot
"
][
"
z
"
]
res
.
rotation
.
w
=
jobj
[
"
rot
"
][
"
w
"
]
res
.
rotation
.
w
=
jobj
[
"
rot
"
][
"
w
"
]
return
res
return
res
def
header_to_json
(
data
):
def
header_to_json
(
data
):
...
@@ -82,7 +82,7 @@ def image_callback(data):
...
@@ -82,7 +82,7 @@ def image_callback(data):
global
fifo
,
remembered_points
,
have_points
global
fifo
,
remembered_points
,
have_points
try
:
try
:
# print("image_callback_main")
# print("image_callback_main")
if
options
.
compressed
:
if
options
.
compressed
:
img
=
bridge
.
compressed_imgmsg_to_cv2
(
data
,
desired_encoding
=
'
bgr8
'
)
img
=
bridge
.
compressed_imgmsg_to_cv2
(
data
,
desired_encoding
=
'
bgr8
'
)
else
:
else
:
img
=
bridge
.
imgmsg_to_cv2
(
data
,
desired_encoding
=
'
bgr8
'
)
img
=
bridge
.
imgmsg_to_cv2
(
data
,
desired_encoding
=
'
bgr8
'
)
...
@@ -94,7 +94,7 @@ def image_callback(data):
...
@@ -94,7 +94,7 @@ def image_callback(data):
look_up_time
=
rospy
.
Time
(
0
)
look_up_time
=
rospy
.
Time
(
0
)
# look_up_time = data.header.stamp
# look_up_time = data.header.stamp
# print("TRANS-ROT:", trans, rot)
# print("TRANS-ROT:", trans, rot)
if
options
.
qr_transform
:
if
options
.
qr_transform
:
...
@@ -117,14 +117,14 @@ def image_callback(data):
...
@@ -117,14 +117,14 @@ def image_callback(data):
jobj
=
transform_to_json
(
transform
)
jobj
=
transform_to_json
(
transform
)
# print("DUMPS:", json.dumps(jobj))
# print("DUMPS:", json.dumps(jobj))
scaledimg
=
cv2
.
resize
(
img
,
(
1024
,
600
))
scaledimg
=
cv2
.
resize
(
img
,
(
1024
,
600
))
if
options
.
qr_transform
or
options
.
qr_header
or
options
.
qr_time
:
if
options
.
qr_transform
or
options
.
qr_header
or
options
.
qr_time
:
# print("SCALEDIMG:", scaledimg.shape, scaledimg.dtype)
# print("SCALEDIMG:", scaledimg.shape, scaledimg.dtype)
frame_pil
=
PilImage
.
fromarray
(
scaledimg
)
frame_pil
=
PilImage
.
fromarray
(
scaledimg
)
qr
=
qrcode
.
QRCode
(
box_size
=
2
)
qr
=
qrcode
.
QRCode
(
box_size
=
2
)
if
options
.
qr_time
:
if
options
.
qr_time
:
timestamp
=
time
.
time
()
timestamp
=
time
.
time
()
...
@@ -136,48 +136,50 @@ def image_callback(data):
...
@@ -136,48 +136,50 @@ def image_callback(data):
pos
=
(
frame_pil
.
size
[
0
]
-
img_qr
.
size
[
0
]
-
1
,
1
)
#better result when 1 pixel in, thus -1 and +1
pos
=
(
frame_pil
.
size
[
0
]
-
img_qr
.
size
[
0
]
-
1
,
1
)
#better result when 1 pixel in, thus -1 and +1
# print("POS QR:", pos)
# print("POS QR:", pos)
frame_pil
.
paste
(
img_qr
,
pos
)
frame_pil
.
paste
(
img_qr
,
pos
)
final_image
=
np
.
asarray
(
frame_pil
)
final_image
=
np
.
asarray
(
frame_pil
)
else
:
else
:
final_image
=
scaledimg
final_image
=
scaledimg
# print("FINALIMAGE:", final_image.shape, final_image.dtype)
# print("FINALIMAGE:", final_image.shape, final_image.dtype)
#gray = numpy.array(img_qr).astype(numpy.float32)
#gray = numpy.array(img_qr).astype(numpy.float32)
#print("GRAY:", gray.shape, gray.dtype)
#print("GRAY:", gray.shape, gray.dtype)
#cv_img_qr = cv2.cvtColor(gray, cv2.COLOR_GRAY2RGB)
#cv_img_qr = cv2.cvtColor(gray, cv2.COLOR_GRAY2RGB)
#print("CV_IMG_QR:", cv_img_qr.shape, cv_img_qr.dtype)
#print("CV_IMG_QR:", cv_img_qr.shape, cv_img_qr.dtype)
# gray = cv2. cvtColor(image, cv2. COLOR_BGR2GRAY)
# gray = cv2. cvtColor(image, cv2. COLOR_BGR2GRAY)
# x_offset=y_offset=50
# x_offset=y_offset=50
# scaledimg[y_offset:y_offset+cv_img_qr.shape[0], x_offset:x_offset+cv_img_qr.shape[1]] = cv_img_qr
# scaledimg[y_offset:y_offset+cv_img_qr.shape[0], x_offset:x_offset+cv_img_qr.shape[1]] = cv_img_qr
if
options
.
test
:
if
options
.
test
:
cv2
.
imshow
(
"
image
"
,
final_image
)
cv2
.
imshow
(
"
image
"
,
final_image
)
cv2
.
waitKey
(
1
)
cv2
.
waitKey
(
1
)
if
options
.
compressed
:
if
options
.
compressed
:
msg
=
bridge
.
cv2_to_compressed_imgmsg
(
final_image
)
msg
=
bridge
.
cv2_to_compressed_imgmsg
(
final_image
)
msg
.
header
=
data
.
header
image_pub
.
publish
(
msg
)
image_pub
.
publish
(
msg
)
else
:
else
:
msg
=
bridge
.
cv2_to_imgmsg
(
final_image
)
msg
=
bridge
.
cv2_to_imgmsg
(
final_image
)
msg
.
header
=
data
.
header
image_pub
.
publish
(
msg
)
image_pub
.
publish
(
msg
)
if
options
.
decode
:
if
options
.
decode
:
retval
,
points
=
detector
.
detect
(
final_image
)
retval
,
points
=
detector
.
detect
(
final_image
)
#
print(retval, points)
print
(
retval
,
points
)
if
retval
:
if
retval
:
remembered_points
=
points
remembered_points
=
points
have_points
=
True
have_points
=
True
else
:
else
:
print
(
"
FAILED TO FIND CODE, trying to use saved points
"
)
print
(
"
FAILED TO FIND CODE, trying to use saved points
"
)
if
have_points
:
if
have_points
:
decoded_data
,
_
=
detector
.
decode
(
final_image
,
remembered_points
)
decoded_data
,
_
=
detector
.
decode
(
final_image
,
remembered_points
)
if
decoded_data
:
if
decoded_data
:
print
(
"
DECODED DATA:
"
,
decoded_data
)
print
(
"
DECODED DATA:
"
,
decoded_data
)
if
options
.
qr_transform
:
if
options
.
qr_transform
:
jobj
=
json
.
loads
(
decoded_data
)
#
jobj = json.loads(decoded_data)
transform
=
json_to_transform
(
jobj
)
#
transform = json_to_transform(jobj)
print
(
"
ROS TRANSFORM:
"
,
transform
)
print
(
"
ROS TRANSFORM:
"
,
transform
)
else
:
else
:
print
(
"
DECODING FAILED!!!!!!!!!
"
)
print
(
"
DECODING FAILED!!!!!!!!!
"
)
...
@@ -186,13 +188,13 @@ def image_callback(data):
...
@@ -186,13 +188,13 @@ def image_callback(data):
#print("DETECT:", retval, points)
#print("DETECT:", retval, points)
#qr_timestamp, _ , _ = detector.detectAndDecode(final_image)
#qr_timestamp, _ , _ = detector.detectAndDecode(final_image)
#print("QR_TIMESTAMP:", qr_timestamp)
#print("QR_TIMESTAMP:", qr_timestamp)
except
Exception
as
exc
:
except
Exception
as
exc
:
print
(
"
image_callback exception:
"
,
exc
)
print
(
"
image_callback exception:
"
,
exc
)
#fifo.close()
#fifo.close()
#fifo = open(options.fifo, 'wb')
#fifo = open(options.fifo, 'wb')
#print(f'FIFO RE-OPENED: {options.fifo}')
#print(f'FIFO RE-OPENED: {options.fifo}')
if
__name__
==
"
__main__
"
:
if
__name__
==
"
__main__
"
:
rospy
.
init_node
(
"
add_qr
"
)
rospy
.
init_node
(
"
add_qr
"
)
...
@@ -206,7 +208,7 @@ if __name__ == "__main__":
...
@@ -206,7 +208,7 @@ if __name__ == "__main__":
# os.mkfifo(options.fifo)
# os.mkfifo(options.fifo)
#except Exception as exc:
#except Exception as exc:
# print(f'{options.fifo} exist:', exc)
# print(f'{options.fifo} exist:', exc)
#if not options.test:
#if not options.test:
# fifo = open(options.fifo, 'wb')
# fifo = open(options.fifo, 'wb')
# print(f'FIFO OPENED: {options.fifo}')
# print(f'FIFO OPENED: {options.fifo}')
...
@@ -229,9 +231,9 @@ if __name__ == "__main__":
...
@@ -229,9 +231,9 @@ if __name__ == "__main__":
image_pub
=
rospy
.
Publisher
(
out_topic
,
CompressedImage
,
queue_size
=
10
)
image_pub
=
rospy
.
Publisher
(
out_topic
,
CompressedImage
,
queue_size
=
10
)
else
:
else
:
image_sub
=
rospy
.
Subscriber
(
in_topic
,
Image
,
image_callback
)
image_sub
=
rospy
.
Subscriber
(
in_topic
,
Image
,
image_callback
)
image_pub
=
rospy
.
Publisher
(
out_topic
,
Image
,
queue_size
=
10
)
image_pub
=
rospy
.
Publisher
(
out_topic
,
Image
,
queue_size
=
10
)
rospy
.
spin
()
rospy
.
spin
()
This diff is collapsed.
Click to expand it.
src/decode_qr_inference.py
0 → 100755
+
154
−
0
View file @
910e9349
#!/usr/bin/env python3
import
rospy
import
json
import
time
import
os
import
qrcode
import
numpy
from
PIL
import
Image
as
PilImage
from
optparse
import
OptionParser
from
lrs_msgs_common.msg
import
InferenceVisionDetection
import
cv2
from
sensor_msgs.msg
import
Image
,
CompressedImage
from
cv_bridge
import
CvBridge
,
CvBridgeError
import
numpy
as
np
import
tf
from
camera_info_manager
import
CameraInfoManager
,
CameraInfoError
,
CameraInfoMissingError
,
resolveURL
,
parseURL
,
\
loadCalibrationFile
,
getPackageFileName
,
URL_empty
,
URL_file
,
URL_package
remembered_points
=
None
have_points
=
False
camera_info
=
None
parser
=
OptionParser
()
parser
.
add_option
(
""
,
"
--image-topic
"
,
action
=
"
store
"
,
type
=
"
string
"
,
dest
=
"
image_topic
"
,
help
=
"
Topic to take image from
"
,
default
=
"
/dji21/camera0/image_raw_orig
"
)
parser
.
add_option
(
""
,
"
--camera-info-file
"
,
action
=
"
store
"
,
type
=
"
string
"
,
dest
=
"
camera_info_file
"
,
help
=
"
Full path of the camera calibration YAML file
"
,
default
=
"
camera_gazebo.yml
"
)
parser
.
add_option
(
""
,
"
--camera-name
"
,
action
=
"
store
"
,
type
=
"
string
"
,
dest
=
"
camera_name
"
,
help
=
"
Camera name
"
,
default
=
"
camera_gazebo
"
)
parser
.
add_option
(
""
,
"
--topic-out
"
,
action
=
"
store
"
,
type
=
"
string
"
,
dest
=
"
topic_out
"
,
help
=
"
Info decoded from the image + camera info
"
,
default
=
"
inference_image_info
"
)
parser
.
add_option
(
""
,
"
--compressed
"
,
action
=
"
store_true
"
,
dest
=
"
compressed
"
,
help
=
"
If true image in is compressed.
"
)
parser
.
add_option
(
""
,
"
--test
"
,
action
=
"
store_true
"
,
dest
=
"
test
"
,
help
=
"
Test mode.
"
)
parser
.
add_option
(
""
,
"
--debug
"
,
action
=
"
store_true
"
,
dest
=
"
debug
"
,
help
=
"
Extra debug print
"
)
(
options
,
args
)
=
parser
.
parse_args
()
from
geometry_msgs.msg
import
TransformStamped
,
Transform
def
json_to_transform
(
jobj
):
res
=
Transform
()
res
.
translation
.
x
=
jobj
[
"
tra
"
][
"
x
"
]
res
.
translation
.
y
=
jobj
[
"
tra
"
][
"
y
"
]
res
.
translation
.
z
=
jobj
[
"
tra
"
][
"
z
"
]
res
.
rotation
.
x
=
jobj
[
"
rot
"
][
"
x
"
]
res
.
rotation
.
y
=
jobj
[
"
rot
"
][
"
y
"
]
res
.
rotation
.
z
=
jobj
[
"
rot
"
][
"
z
"
]
res
.
rotation
.
w
=
jobj
[
"
rot
"
][
"
w
"
]
return
res
def
image_callback
(
data
):
global
fifo
,
remembered_points
,
have_points
,
camera_info
try
:
# print("image_callback_main")
if
options
.
compressed
:
img
=
bridge
.
compressed_imgmsg_to_cv2
(
data
,
desired_encoding
=
'
bgr8
'
)
else
:
#img = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
# FIXME - encoding gives an error
img
=
bridge
.
imgmsg_to_cv2
(
data
)
scaledimg
=
cv2
.
resize
(
img
,
(
1024
,
600
))
final_image
=
scaledimg
retval
,
points
=
detector
.
detect
(
final_image
)
# print(retval, points)
if
retval
:
remembered_points
=
points
have_points
=
True
else
:
print
(
"
FAILED TO FIND CODE, trying to use saved points
"
)
if
have_points
:
decoded_data
,
_
=
detector
.
decode
(
final_image
,
remembered_points
)
if
decoded_data
:
print
(
"
DECODED DATA:
"
,
decoded_data
)
jobj
=
json
.
loads
(
decoded_data
)
c2w_transform
=
json_to_transform
(
jobj
)
if
options
.
debug
:
print
(
"
ROS TRANSFORM:
"
,
c2w_transform
)
else
:
print
(
"
DECODING FAILED!!!!!!!!!
"
)
# make and publish the message
#
msg
=
InferenceVisionDetection
()
msg
.
header
=
data
.
header
msg
.
camera_info
=
camera_info
msg
.
c2w_transform
=
c2w_transform
data_pub
.
publish
(
msg
)
if
options
.
debug
:
print
(
msg
)
except
Exception
as
exc
:
print
(
"
image_callback exception:
"
,
exc
)
if
__name__
==
"
__main__
"
:
rospy
.
init_node
(
"
decode_qr_inference
"
)
print
(
"
OpenCV version:
"
,
cv2
.
__version__
)
listener
=
tf
.
TransformListener
()
detector
=
cv2
.
QRCodeDetector
()
print
(
"
Create cv bridge
"
)
bridge
=
CvBridge
()
_camera_name
=
options
.
camera_name
_camera_info_file
=
options
.
camera_info_file
resolved_url
=
resolveURL
(
_camera_info_file
,
_camera_name
)
print
(
"
Camera info URL:
"
,
resolved_url
)
# the exception is not working - wrong version?
try
:
camera_info
=
loadCalibrationFile
(
_camera_info_file
,
_camera_name
)
except
IOError
as
exc
:
print
(
"
Camera info load exception:
"
,
exc
)
exit
()
# extra check because the above exception is not working
if
camera_info
.
height
!=
0
and
camera_info
.
width
!=
0
:
print
(
"
Loaded camera info:
"
,
camera_info
)
else
:
print
(
"
Loaded camera info width and height are zeros. Calibration camera info URL correct?
"
)
exit
()
if
options
.
compressed
:
in_topic
=
f
'
{
options
.
image_topic
}
/compressed
'
else
:
in_topic
=
f
'
{
options
.
image_topic
}
'
print
(
"
in_topic:
"
,
in_topic
)
print
(
"
out_topic:
"
,
options
.
topic_out
)
if
options
.
compressed
:
image_sub
=
rospy
.
Subscriber
(
in_topic
,
CompressedImage
,
image_callback
)
else
:
image_sub
=
rospy
.
Subscriber
(
in_topic
,
Image
,
image_callback
)
data_pub
=
rospy
.
Publisher
(
options
.
topic_out
,
InferenceVisionDetection
,
queue_size
=
10
)
rospy
.
spin
()
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