From 13ce813095d3be3bb70ba5c5c9af53cd02495e1b Mon Sep 17 00:00:00 2001
From: Reko Vuorinen <ravuor@utu.fi>
Date: Thu, 24 Apr 2025 14:40:55 +0300
Subject: [PATCH] Added detection of 3 or more green areas

---
 .../tello_racing_pkg/tello_drone_race.py      | 91 ++++++++++++-------
 1 file changed, 58 insertions(+), 33 deletions(-)

diff --git a/src/drone_racing_ros2/tello_ros/tello_racing_pkg/tello_racing_pkg/tello_drone_race.py b/src/drone_racing_ros2/tello_ros/tello_racing_pkg/tello_racing_pkg/tello_drone_race.py
index 30f0583..bd3e581 100755
--- a/src/drone_racing_ros2/tello_ros/tello_racing_pkg/tello_racing_pkg/tello_drone_race.py
+++ b/src/drone_racing_ros2/tello_ros/tello_racing_pkg/tello_racing_pkg/tello_drone_race.py
@@ -164,13 +164,10 @@ class GateNavigator(Node):
         hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
         # Masks for both colors
         green_mask = cv2.inRange(hsv, self.hsv_lower_green, self.hsv_upper_green)
-    
 
         # --- (Optional but recommended) Morphological Operations ---
-        # Apply opening (erosion followed by dilation) to remove small noise dots
-        kernel = np.ones((5,5),np.uint8) # Adjust kernel size as needed
+        kernel = np.ones((5, 5), np.uint8)
         green_mask = cv2.morphologyEx(green_mask, cv2.MORPH_OPEN, kernel)
-        # Apply closing (dilation followed by erosion) to fill small holes
         green_mask = cv2.morphologyEx(green_mask, cv2.MORPH_CLOSE, kernel)
         # --- End Optional Morphological Operations ---
 
@@ -182,44 +179,68 @@ class GateNavigator(Node):
         
         contours, _ = cv2.findContours(green_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
 
-        self.gate_detected = False 
-        best_cx, best_cy = self.image_center_x, self.image_center_y 
+        self.gate_detected = False
+        best_cx, best_cy = self.image_center_x, self.image_center_y
 
+        # -------------------------------------------------------------------------
+        # NEW LOGIC – handle 3 + separate green blobs by averaging their centers
+        # -------------------------------------------------------------------------
         if contours:
-            largest_contour = max(contours, key=cv2.contourArea)
-            area = cv2.contourArea(largest_contour)
-
-            if area > self.min_contour_area:
-                M = cv2.moments(largest_contour)
-                if M["m00"] != 0:
-                    cx = int(M["m10"] / M["m00"])
-                    cy = int(M["m01"] / M["m00"])
-                    best_cx, best_cy = cx, cy
-                    self.gate_detected = True 
-
-                    # Set passing_gate mode active
-                    self.passing_gate = True
-                    self.last_gate_time = self.get_clock().now()
-
-                    # Visualization drawing
-                    cv2.drawContours(cv_image, [largest_contour], -1, (0, 255, 0), 3)
-                    cv2.circle(cv_image, (cx, cy), 7, (0, 0, 255), -1)
-                    cv2.putText(cv_image, f"Gate ({cx},{cy})", (cx - 40, cy - 20),
-                                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
-
-
+            # only keep contours whose area is significant for a gate candidate
+            sig_contours = [c for c in contours if cv2.contourArea(c) > self.min_contour_area]  # << mod
+
+            if len(sig_contours) >= 3:                                                         # << mod
+                centers = []                                                                   # << mod
+                for c in sig_contours:                                                         # << mod
+                    M = cv2.moments(c)                                                         # << mod
+                    if M["m00"] == 0:                                                          # << mod
+                        continue                                                               # << mod
+                    cx = int(M["m10"] / M["m00"])                                              # << mod
+                    cy = int(M["m01"] / M["m00"])                                              # << mod
+                    centers.append((cx, cy))                                                   # << mod
+                    cv2.drawContours(cv_image, [c], -1, (0, 255, 0), 2)                        # << mod
+
+                if centers:                                                                    # << mod
+                    best_cx = int(np.mean([p[0] for p in centers]))                            # << mod
+                    best_cy = int(np.mean([p[1] for p in centers]))                            # << mod
+                    self.gate_detected = True                                                  # << mod
+                    self.passing_gate = True                                                   # << mod
+                    self.last_gate_time = self.get_clock().now()                               # << mod
+                    cv2.circle(cv_image, (best_cx, best_cy), 7, (0, 0, 255), -1)               # << mod
+                    cv2.putText(cv_image, f"Gate ({best_cx},{best_cy})",                       # << mod
+                                (best_cx - 40, best_cy - 20), cv2.FONT_HERSHEY_SIMPLEX,        # << mod
+                                0.5, (255, 255, 255), 2)                                       # << mod
+            else:                                                                              # << mod
+                # ---------- original single-contour logic (unchanged) ----------
+                largest_contour = max(sig_contours, key=cv2.contourArea) if sig_contours else None
+                if largest_contour is not None:
+                    M = cv2.moments(largest_contour)
+                    if M["m00"] != 0:
+                        cx = int(M["m10"] / M["m00"])
+                        cy = int(M["m01"] / M["m00"])
+                        best_cx, best_cy = cx, cy
+                        self.gate_detected = True
+
+                        self.passing_gate = True
+                        self.last_gate_time = self.get_clock().now()
+
+                        cv2.drawContours(cv_image, [largest_contour], -1, (0, 255, 0), 3)
+                        cv2.circle(cv_image, (cx, cy), 7, (0, 0, 255), -1)
+                        cv2.putText(cv_image, f"Gate ({cx},{cy})", (cx - 40, cy - 20),
+                                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
+        # -------------------- end of new / modified section ----------------------
+
+        # ---------------- remainder of function is UNCHANGED --------------------
         # Control Logic Calculation
         twist_msg = Twist() 
         
         if self.gate_detected:
             error_x = best_cx - self.image_center_x
             error_y = best_cy - self.image_center_y
-            # Calculate velocities based on error
             calculated_angular_z = self.kp_yaw * float(error_x)
             calculated_linear_z = self.kp_alt * float(error_y)
             calculated_linear_x = self.target_forward_speed
 
-            # Populate the twist message
             twist_msg.angular.z = calculated_angular_z
             twist_msg.linear.z = calculated_linear_z
             twist_msg.linear.x = calculated_linear_x
@@ -227,9 +248,13 @@ class GateNavigator(Node):
             self.last_gate_time = self.get_clock().now()
             self.passing_gate = True
 
-            # Log calculated values
-            self.get_logger().info(f"Gate Found! Center:({best_cx},{best_cy}), Error:({error_x},{error_y}), Calc Cmd: lin.x={twist_msg.linear.x:.2f}, lin.z={twist_msg.linear.z:.2f}, ang.z={twist_msg.angular.z:.2f}", throttle_duration_sec=1.0)
-      
+            self.get_logger().info(
+                f"Gate Found! Center:({best_cx},{best_cy}), "
+                f"Error:({error_x},{error_y}), "
+                f"Calc Cmd: lin.x={twist_msg.linear.x:.2f}, "
+                f"lin.z={twist_msg.linear.z:.2f}, ang.z={twist_msg.angular.z:.2f}",
+                throttle_duration_sec=1.0)
+
         elif self.passing_gate:
             time_since_gate = (self.get_clock().now() - self.last_gate_time).nanoseconds * 1e-9
             if time_since_gate < 3.0:
-- 
GitLab