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