Added detection of 3 or more green areas

src/drone_racing_ros2/tello_ros/tello_racing_pkg/tello_racing_pkg/tello_drone_race.py

@@ -165,12 +165,9 @@ class GateNavigator(Node):
         # 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 ---
 
@@ -185,11 +182,38 @@ class GateNavigator(Node):
         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:
+            # 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"])
@@ -197,29 +221,26 @@ class GateNavigator(Node):
                         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)
+        # -------------------- 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,8 +248,12 @@ 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