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