Skip to content
Snippets Groups Projects
Commit 910e9349 authored by Piotr Rudol's avatar Piotr Rudol
Browse files

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
...@@ -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()
#!/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()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment