From 1f8c5005dda2250d65c9f7e9e1e14a2db2c3fc5f Mon Sep 17 00:00:00 2001
From: Mohamed Fazil <mfazil@hello-robot.com>
Date: Wed, 22 Feb 2023 19:48:28 -0800
Subject: [PATCH] Add version check for detect_aruco_marker node

---
 hello_helpers/src/hello_helpers/hello_misc.py | 20 +++++++++++++++++++
 stretch_core/nodes/detect_aruco_markers       |  8 ++++++++
 2 files changed, 28 insertions(+)

diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py
index 50f79c8..d916b09 100644
--- a/hello_helpers/src/hello_helpers/hello_misc.py
+++ b/hello_helpers/src/hello_helpers/hello_misc.py
@@ -272,3 +272,23 @@ def bound_ros_command(bounds, ros_pos, fail_out_of_range_goal, clip_ros_toleranc
             return bounds[1]
 
     return ros_pos
+
+def compare_versions(v1, v2):
+    """
+    Compare two strings of versions.
+    Returns 1  if v1>v2
+            -1 if v1<v2
+            0  if v1==v2
+    """
+    v1 = [int(v) for v in v1.split('.')]
+    v2 = [int(v) for v in v2.split('.')]
+    while len(v1) < len(v2):
+        v1.append(0)
+    while len(v2) < len(v1):
+        v2.append(0)
+    for i in range(len(v1)):
+        if v1[i] < v2[i]:
+            return -1
+        elif v1[i] > v2[i]:
+            return 1
+    return 0
\ No newline at end of file
diff --git a/stretch_core/nodes/detect_aruco_markers b/stretch_core/nodes/detect_aruco_markers
index d7cc399..bef696b 100755
--- a/stretch_core/nodes/detect_aruco_markers
+++ b/stretch_core/nodes/detect_aruco_markers
@@ -5,6 +5,7 @@ import rospy
 import cv2
 import numpy as np
 import math
+import click
 
 import message_filters
 from std_msgs.msg import Header
@@ -22,8 +23,10 @@ import tf
 from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_from_matrix
 
 import struct
+import cv2
 import cv2.aruco as aruco
 import hello_helpers.fit_plane as fp
+from hello_helpers.hello_misc import compare_versions
 import threading
 from collections import deque
 
@@ -715,6 +718,11 @@ class DetectArucoNode:
 
 
 if __name__ == '__main__':
+    if compare_versions(cv2.__version__,'4.7') == -1:
+        txt = f"[ERROR] Found unsupported cv2 version({cv2.__version__}), Requires opencv-contrib-python>=4.7.0 " \
+              f"\n\t\t\tShutting down node detect_aruco_markers"
+        rospy.logerr(click.style(txt,fg='red'))
+        sys.exit()
     node = DetectArucoNode()
     node.main()
     try: