From 278d694dc201dd58b479c5870f28307a6a4ab529 Mon Sep 17 00:00:00 2001
From: Matthijs van der Burgh <MatthijsBurgh@outlook.com>
Date: Wed, 1 May 2024 10:11:04 +0200
Subject: [PATCH] (hmi) improve QR reader

---
 hmi/scripts/qr_code_decoder | 58 ++++++++++++++++++-------------------
 1 file changed, 29 insertions(+), 29 deletions(-)

diff --git a/hmi/scripts/qr_code_decoder b/hmi/scripts/qr_code_decoder
index 7f3d575..cb903f6 100755
--- a/hmi/scripts/qr_code_decoder
+++ b/hmi/scripts/qr_code_decoder
@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 
-from typing import Callable
+from typing import Callable, List, Optional
 
 from numpy import ndarray
 
@@ -9,7 +9,7 @@ from hmi import AbstractHMIServer, HMIResult
 from hmi.common import parse_sentence
 from cv_bridge import CvBridge
 from sensor_msgs.msg import CompressedImage
-from pyzbar.pyzbar import decode as qr_decode
+from pyzbar.pyzbar import Decoded, decode as qr_decode
 
 
 def validate_image(qr_image: ndarray, qr_code_rect: tuple, threshold_percentage: int = 20) -> bool:
@@ -45,48 +45,46 @@ class QRCodeDecode(AbstractHMIServer):
     QRCodeDecode class for decoding QR codes.
     """
 
-    def __init__(self, *args, **kwargs) -> None:
+    def __init__(self, max_tries: int = 5, loop_rate: float = 1.0, *args, **kwargs) -> None:
         super(self.__class__, self).__init__(*args, **kwargs)
 
         # Image
-        self._image_topic = "~image"
-        self._image = None
-        self._image_sub = None
+        self._image_sub: Optional[rospy.Subscriber] = None
 
         self._cv_bridge = CvBridge()
 
-        self._tries = 5
-        self._results = []
-        self._qr_code_data = None
+        self._max_tries: int = max_tries
+        self._tries: int = 0
+        self._results: List[str] = []
+        self._qr_code_data: Optional[str] = None
+
+        self._loop_rate: float = loop_rate
 
     def _image_callback(self, msg: CompressedImage) -> None:
         # Will be called every time a new image is received
-        self._image = self._cv_bridge.compressed_imgmsg_to_cv2(msg)
-        result = self.decode_qr_code()
+        image = self._cv_bridge.compressed_imgmsg_to_cv2(msg)
+        result = self.decode_qr_code(image)
 
         # If we see a QR code
         if result:
             self._results.append(result)
-            self._tries -= 1
+            self._tries += 1
 
     def _determine_answer(
-        self,
-        description: str,
-        grammar: str,
-        target: str,
-        is_preempt_requested: Callable,
+        self, description: str, grammar: str, target: str, is_preempt_requested: Callable
     ) -> HMIResult:
 
-        self._image_sub = rospy.Subscriber(self._image_topic, CompressedImage, self._image_callback, queue_size=1)
-        rospy.loginfo("QRCodeDecode: subscribed to %s", self._image_sub.name)
+        self._image_sub = rospy.Subscriber("~image", CompressedImage, self._image_callback, queue_size=1)
+        rospy.logdebug(f"QRCodeDecode: subscribed to {self._image_sub.name}")
 
         # Initialize
         result = HMIResult("", {})
 
         # Waits for _image_callback to do the tries
-        while self._tries > 0:
-            rospy.loginfo("Waiting for QR code data...")
-            rospy.sleep(1)
+        r = rospy.Rate(self._loop_rate)
+        while not rospy.is_shutdown and not self._tries <= self._max_tries:
+            rospy.logdebug("Waiting for QR code data...")
+            r.sleep()
             if is_preempt_requested():
                 return result
 
@@ -96,25 +94,27 @@ class QRCodeDecode(AbstractHMIServer):
 
         if self._qr_code_data:
             self._image_sub.unregister()
-            rospy.loginfo("QR code data: %s", self._qr_code_data)
+            rospy.loginfo(f"QR code data: {self._qr_code_data}")
             semantics = parse_sentence(self._qr_code_data, grammar, target)
-            rospy.loginfo("Parsed semantics: %s", semantics)
+            rospy.loginfo(f"Parsed semantics: {semantics}")
             result = HMIResult(self._qr_code_data, semantics)
 
         self.reset_server()
         return result
 
-    def decode_qr_code(self) -> str:
+    def decode_qr_code(self, image: ndarray) -> str:
         """
         Decode the QR code from the image.
+
+        :param image: image to run qr decoding on
         """
 
-        qr_list = qr_decode(self._image)  # Gets all QR codes in the image
-        qr = None
+        qr_list: List[Decoded] = qr_decode(image)  # Gets all QR codes in the image
+        qr: Optional[Decoded] = None
 
         # Stores all QRs that occupy the required percentage of the screen
         for qr_i in qr_list:
-            if validate_image(self._image, qr_i.rect):
+            if validate_image(image, qr_i.rect):
                 qr = qr_i
                 break
 
@@ -129,7 +129,7 @@ class QRCodeDecode(AbstractHMIServer):
         """
         Re-initialize the server.
         """
-        self._tries = 5
+        self._tries = 0
         self._results = []
         self._qr_code_data = None