Code for moving towards the red ball

redball_chase.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python
+
+import rospy
+import cv2
+import numpy as np
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Twist
+from cv_bridge import CvBridge, CvBridgeError
+
+def is_circular(contour, radius):
+    """Check if the contour is circular enough to be considered a ball."""
+    area = cv2.contourArea(contour)
+    if area <= 0:
+        return False
+    circularity = 4 * np.pi * area / (2 * np.pi * radius) ** 2
+    # Circular enough if circularity is close to 1; adjust threshold as needed
+    return 0.7 <= circularity <= 1.3
+
+def find_red_ball(image):
+    # Convert the image to HSV color space
+    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
+
+    # Red color range in HSV; might need fine-tuning
+    lower_red1 = np.array([0, 120, 70])
+    upper_red1 = np.array([10, 255, 255])
+    lower_red2 = np.array([170, 120, 70])
+    upper_red2 = np.array([180, 255, 255])
+
+    mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
+    mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
+    mask = mask1 + mask2
+
+    # Find contours in the mask
+    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+    ball_found = False
+    for contour in contours:
+        (x, y), radius = cv2.minEnclosingCircle(contour)
+        if radius > 5 and is_circular(contour, radius):  # Checks size and circularity
+            center = (int(x), int(y))
+            cv2.circle(image, center, int(radius), (0, 255, 0), 2)
+            cv2.putText(image, 'Red Ball', (center[0] - 20, center[1] - int(radius) - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
+            ball_found = True
+            break
+
+    return image, (center, radius) if ball_found else (image, None)
+
+def image_callback(msg):
+    try:
+        cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
+    except CvBridgeError as e:
+        rospy.logerr(e)
+
+    processed_image, ball_info = find_red_ball(cv_image)
+
+    twist_msg = Twist()
+    if ball_info:
+        x, _, radius = ball_info[0]
+        err = x - cv_image.shape[1] / 2
+        if abs(err) > 100:  # Need to rotate TurtleBot to align with the ball
+            twist_msg.angular.z = -0.005 * err
+        else:  # Move forward towards the ball
+            twist_msg.linear.x = min(0.2 * radius, 0.1)  # Adjust to control speed
+    else:  # Rotate TurtleBot if the ball is not found
+        twist_msg.angular.z = 0.5
+
+    cmd_vel_pub.publish(twist_msg)
+    cv2.imshow("Red Ball Detection", processed_image)
+    cv2.waitKey(3)
+
+def shutdown():
+    rospy.loginfo("Shutting down")
+    cmd_vel_pub.publish(Twist())  # Stop the robot
+    cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    rospy.init_node('red_ball_follower', anonymous=False)
+    bridge = CvBridge()
+    cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+    rospy.Subscriber("/camera/rgb/image_raw", Image, image_callback)
+
+    rospy.on_shutdown(shutdown)
+
+    # Setup window for display (optional, remove if running headless)
+    cv2.namedWindow("Red Ball Detection", cv2.WINDOW_NORMAL)
+
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+        rospy.loginfo("Red ball follower node terminated.")