From 41bd8f004afa08b7b2de6e5d3a6ec6d3ff8a1076 Mon Sep 17 00:00:00 2001 From: Tommy Persson <tommy.persson@liu.se> Date: Wed, 24 Aug 2022 15:19:52 +0200 Subject: [PATCH] Work on qr code. --- src/add_qr.py | 69 +++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 62 insertions(+), 7 deletions(-) diff --git a/src/add_qr.py b/src/add_qr.py index 0d327b9..ffa485f 100755 --- a/src/add_qr.py +++ b/src/add_qr.py @@ -15,15 +15,38 @@ import cv2 from sensor_msgs.msg import Image, CompressedImage from cv_bridge import CvBridge, CvBridgeError import numpy as np +import tf 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 ("", "--compressed", action="store_true", dest="compressed", help="If true image in is compressed.") +parser.add_option ("", "--decode", action="store_true", dest="decode", help="If true test the decoding.") #parser.add_option ("", "--fifo", action="store", type="string", dest="fifo", help="Fifo to write raw images to", default="/tmp/fifo_dji21") parser.add_option ("", "--test", action="store_true", dest="test", help="Test mode.") -parser.add_option ("", "--qr", action="store_true", dest="qr", help="Time encoding qr code.") +parser.add_option ("", "--qr-transform", action="store_true", dest="qr_transform", help="Time encoding qr code.") +parser.add_option ("", "--qr-header", action="store_true", dest="qr_header", help="Time encoding qr code.") +parser.add_option ("", "--qr-time", action="store_true", dest="qr_time", help="Time encoding qr code.") (options, args) = parser.parse_args() +from geometry_msgs.msg import TransformStamped, Transform + + +def transform_to_json(tr): + jobj = { + "translation": { + "x": tr.translation.x, + "y": tr.translation.y, + "z": tr.translation.z + }, + "rotation": { + "x": tr.rotation.x, + "y": tr.rotation.y, + "z": tr.rotation.z, + "w": tr.rotation.w + } + } + return jobj + def header_to_json(data): jobj = { "seq": data.seq, @@ -49,13 +72,40 @@ def image_callback(data): else: img = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8') - # print(data.header) - jobj = header_to_json(data.header) - print(json.dumps(jobj)) + if options.qr_header: + # print(data.header) + jobj = header_to_json(data.header) + # print(json.dumps(jobj)) + + look_up_time = rospy.Time(0) + # look_up_time = data.header.stamp + + # print("TRANS-ROT:", trans, rot) + + if options.qr_transform: + try: + (trans,rot) = listener.lookupTransform('/world', '/dji0/camera0/image_frame', look_up_time) + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): + print("Exception lookup trans:", look_up_time) + return + + transform = Transform() + transform.translation.x = round(trans[0], 3) + transform.translation.y = round(trans[1], 3) + transform.translation.z = round(trans[2], 3) + transform.rotation.x = round(rot[0], 5) + transform.rotation.y = round(rot[1], 5) + transform.rotation.z = round(rot[2], 5) + transform.rotation.w = round(rot[3], 5) + + # print(transform) + + jobj = transform_to_json(transform) + # print("DUMPS:", json.dumps(jobj)) scaledimg = cv2.resize(img, (1024, 600)) - if options.qr: + if options.qr_transform or options.qr_header or options.qr_time: # print("SCALEDIMG:", scaledimg.shape, scaledimg.dtype) frame_pil = PilImage.fromarray(scaledimg) @@ -92,9 +142,13 @@ def image_callback(data): if options.compressed: msg = bridge.cv2_to_compressed_imgmsg(final_image) image_pub.publish(msg) + else: + msg = bridge.cv2_to_imgmsg(final_image) + image_pub.publish(msg) - decoded_data, _ , _ = detector.detectAndDecode(final_image) - print("DECODED DATA:", decoded_data) + if options.decode: + decoded_data, _ , _ = detector.detectAndDecode(final_image) + print("DECODED DATA:", decoded_data) #retval, points = detector.detect(final_image) @@ -112,6 +166,7 @@ if __name__ == "__main__": rospy.init_node ("add_qr") + listener = tf.TransformListener() detector = cv2.QRCodeDetector() #try: -- GitLab