Change HSV tuner to use the function of the CameraVision class

grp_astro/CMakeLists.txt

@@ -51,7 +51,7 @@ install(DIRECTORY
 # Python scripts
 install( PROGRAMS 
   src/cam_driver.py
-  src/cam_vision
+  src/cam_vision.py
   src/reactive_move
   src/scan2point_cloud
   src/bottle_mapping

grp_astro/launch/tbot_launch.yaml

@@ -23,7 +23,7 @@ launch:
 # Publish if there is a bottle in the field of view
 - node:
     pkg: "grp_astro"
-    exec: "cam_vision"
+    exec: "cam_vision.py"
     name: "cam_vision"
 
 # Launch a SLAM algorithm

grp_astro/src/cam_driver.py

@@ -11,11 +11,12 @@ from cv_bridge import CvBridge
 
 class CameraDriver(Node):
 
-    def __init__(self, name="cam_driver", timerFreq = 1/60.0):
-        super().__init__(name) # Create the node
+    def __init__(self, name="cam_driver", timerFreq = 1/60.0, isRos = True):
+        if isRos:
+            super().__init__(name) # Create the node
 
-        # Initialize publishers:
-        self.init_publishers(timerFreq)
+            # Initialize publishers:
+            self.init_publishers(timerFreq)
 
         # Initialize camera
         self._init_camera()

grp_astro/src/cam_vision → grp_astro/src/cam_vision.py

@@ -15,37 +15,40 @@ import pyrealsense2 as rs
 class CameraVision(CameraDriver):
     
 
-    def __init__(self, name="cam_vision", timerFreq = 1/60.0):
-        super().__init__(name, timerFreq) # Create the node
+    def __init__(self, name="cam_vision", timerFreq = 1/60.0, isRos = True):
+        super().__init__(name, timerFreq, isRos) # Create the node
 
-        # Load parameters
-        directory = pathlib.PurePath(__file__).parent
-        try:
-            # Load HSV threshold params from file
-            file = open(str(directory / "config" / "hsv_params" ), "r")
 
-            # Parse string
-            text = file.read()
-            low = text.split(",")[0].strip('][ ')
-            high = text.split(",")[1].strip('][ ')
+        if isRos:
+            # Load parameters
+            directory = pathlib.PurePath(__file__).parent
+            try:
+                # Load HSV threshold params from file
+                file = open(str(directory / "config" / "hsv_params" ), "r")
+
+                # Parse string
+                text = file.read()
+                low = text.split(",")[0].strip('][ ')
+                high = text.split(",")[1].strip('][ ')
 
-            self._low = np.array([int(i) for i in low.split(" ") if i])
-            self._high = np.array([int(i) for i in high.split(" ") if i])
+                self._low = np.array([int(i) for i in low.split(" ") if i])
+                self._high = np.array([int(i) for i in high.split(" ") if i])
 
-            print(f"Successfully loaded HSV params :\n - low : {self._low}\n - high {self._high}")
-        except:
-            # Initialize default variables if no file exists
-            self._low = np.array([60-25, 50, 50])
-            self._high = np.array([65+25, 255, 255])
+                print(f"Successfully loaded HSV params :\n - low : {self._low}\n - high {self._high}")
+            except:
+                # Initialize default variables if no file exists
+                self._low = np.array([60-25, 50, 50])
+                self._high = np.array([65+25, 255, 255])
 
-            print("Set HSV params to default values :\n - low : {self._low}\n - high {self._high}")
+                print("Set HSV params to default values :\n - low : {self._low}\n - high {self._high}")
 
-        # Node parameters
-        self.declare_parameters(
-            namespace='',
-            parameters=[
-                ('DEPTH_CHECK_RADIUS', 10)
-            ])
+
+            # Node parameters
+            self.declare_parameters(
+                namespace='',
+                parameters=[
+                    ('DEPTH_CHECK_RADIUS', 10)
+                ])
 
         
         # Node variables
@@ -109,6 +112,16 @@ class CameraVision(CameraDriver):
         self._bottle_relative_pos_publisher.publish(relative_position)
         
 
+    def maskImage(self, hsv_img):
+
+        # Seuillage par colorimétrie
+        mask = cv2.inRange(hsv_img, self._low, self._high)
+
+        # Réduction du bruit
+        mask = cv2.erode(mask, self._kernel, iterations=4)
+        mask = cv2.dilate(mask, self._kernel, iterations=4)
+
+        return mask
 
     # Node internal loop
     def loop(self):
@@ -124,12 +137,8 @@ class CameraVision(CameraDriver):
         # Convert image to HSV for easier thresholding
         image = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV)
 
-        # Seuillage par colorimétrie
-        mask = cv2.inRange(image, self._low, self._high)
-
-        # Réduction du bruit
-        mask = cv2.erode(mask, self._kernel, iterations=4)
-        mask = cv2.dilate(mask, self._kernel, iterations=4)
+        # Create a mask based on HSV threshold, noise reduction
+        mask = self.maskImage(image)
 
 
         # Segmentation using shape matching

grp_astro/src/config/mask_tuner.py

@@ -9,62 +9,29 @@ import pyrealsense2 as rs
 import signal, time, numpy as np
 import sys, cv2
 import time
+sys.path.append("..")
+from cam_vision import CameraVision
 
-# Configure depth and color streams
-pipeline = rs.pipeline()
-config = rs.config()
-
-# Get device product line for setting a supporting resolution
-pipeline_wrapper = rs.pipeline_wrapper(pipeline)
-pipeline_profile = config.resolve(pipeline_wrapper)
-device = pipeline_profile.get_device()
-device_product_line = str(device.get_info(rs.camera_info.product_line))
-
-print( f"Connect: {device_product_line}" )
-found_rgb = True
-for s in device.sensors:
-    print( "Name:" + s.get_info(rs.camera_info.name) )
-    if s.get_info(rs.camera_info.name) == 'RGB Camera':
-        found_rgb = True
-
-if not (found_rgb):
-    print("Depth camera required !!!")
-    exit(0)
-
-config.enable_stream(rs.stream.color, width=848, height=480, format=rs.format.bgr8, framerate=60)
-config.enable_stream(rs.stream.depth, width=848, height=480, format=rs.format.z16, framerate=60)
-
-# Capture ctrl-c event
-isOk= True
-def signalInterruption(signum, frame):
-    global isOk
-    print( "\nCtrl-c pressed" )
-    isOk= False
-
-signal.signal(signal.SIGINT, signalInterruption)
-
-# Start streaming
-pipeline.start(config)
-
+camera = CameraVision(isRos=False) # Init camera 
 # VARIABLES
-low = np.array([179, 255, 255]) # Set to max so detected values will be inferior
-high = np.array([0, 0, 0]) #Set to min so detected values will be superior
-kernel = np.ones((5, 5), np.uint8)
+camera._low = np.array([179, 255, 255]) # Set to max so detected values will be inferior
+camera._high = np.array([0, 0, 0]) #Set to min so detected values will be superior
+camera._kernel = np.ones((5, 5), np.uint8)
 
 
 # Skip the first frames of the camera (for some reason they don't work well)
 for i in range(20):
-    frames = pipeline.wait_for_frames()
+    frames = camera._pipeline.wait_for_frames()
     color_frame = frames.get_color_frame()
 
 
-if color_frame is not None:
+if color_frame is None:
     exit
 
+
 # Convert image to numpy arrays
 color_image = np.asanyarray(color_frame.get_data())
 
-
 # Convert image to HSV for easier thresholding
 hsv_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV)
 
@@ -82,17 +49,12 @@ hMargin = 13
 sMargin = 10
 vMargin = 10
 
-low = np.array([min(H) - hMargin, min(S) - sMargin, min(V) - vMargin])
-high = np.array([max(H) + hMargin, max(S) + sMargin, max(V) + vMargin])
-print("-> Low and high HSV saved in 'hsv_params' file :",low, high)
+camera._low = np.array([min(H) - hMargin, min(S) - sMargin, min(V) - vMargin])
+camera._high = np.array([max(H) + hMargin, max(S) + sMargin, max(V) + vMargin])
+print("-> Low and high HSV saved in 'hsv_params' file :",camera._low, camera._high)
 
 # Vision
-# Seuillage par colorimétrie
-mask = cv2.inRange(hsv_image, low, high)
-
-# Réduction du bruit
-mask = cv2.erode(mask, kernel, iterations=4)
-mask = cv2.dilate(mask, kernel, iterations=4)
+mask = camera.maskImage(hsv_image)
 
 mask_rgb = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
 
@@ -106,8 +68,8 @@ cv2.waitKey(0)
 
 # Save values in a file
 file = open("hsv_params", "w")
-file.write(str(low) + "," + str(high))
+file.write(str(camera._low) + "," + str(camera._high))
 file.close()
 # Stop streaming
 print("\nEnding...")
-pipeline.stop()
+camera._pipeline.stop()
\ No newline at end of file