Removing the depth sensing by the radius one

redball_chase.py

@@ -6,15 +6,16 @@ import numpy as np
 from sensor_msgs.msg import Image
 from geometry_msgs.msg import Twist
 from cv_bridge import CvBridge, CvBridgeError
-from sensor_msgs.msg import LaserScan
+import random
+import math
 
-# Define the target stopping distance from the ball in meters
-TARGET_DISTANCE = 1.0  
+# Define the target stopping distance from the ball in pixels
+RADIUS_THRESHOLD = 40  
 
 # Global variables for state management
 current_state = "searching"
 ball_position = None
-
+ball_radius = None
 
 def is_circular(contour, radius):
     """Check if the contour is circular enough to be considered a ball."""
@@ -41,21 +42,34 @@ def find_red_ball(image):
             return (int(x), int(y)), radius
     return None, None
 
-def lidar_callback(data):
-    """Process LiDAR data to adjust DogBot's behavior based on distance to obstacles."""
+def execute_search():
+    """Execute a search pattern: rotate 360, move randomly if the ball isn't found, and repeat."""
     global current_state
-    if current_state == "approaching":
-        # Assuming data.ranges is a list of distance measurements
-        # You might need to process this data differently based on your LiDAR setup
-        distance_to_ball = min(data.ranges)
-        if distance_to_ball <= TARGET_DISTANCE:
-            current_state = "maneuvering"
-            # Placeholder for function to initiate maneuvering behind the ball
-            execute_half_circle()
+    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+    twist_msg = Twist()
+
+    # Rotate 360 degrees
+    twist_msg.angular.z = 0.5  # Adjust as needed
+    rotate_duration = 2 * math.pi / twist_msg.angular.z  # Full circle
+    start_time = rospy.Time.now().to_sec()
+    while rospy.Time.now().to_sec() - start_time < rotate_duration:
+        pub.publish(twist_msg)
+        rospy.sleep(0.1)
+    twist_msg.angular.z = 0
+    pub.publish(twist_msg)  # Stop rotation
+    
+    # Move in a random direction if the ball isn't found
+    if current_state == "searching":
+        twist_msg.linear.x = 0.2
+        move_duration = random.uniform(1, 3)  # Random duration between 1 and 3 seconds
+        start_time = rospy.Time.now().to_sec()
+        while rospy.Time.now().to_sec() - start_time < move_duration:
+            pub.publish(twist_msg)
+        twist_msg.linear.x = 0
+        pub.publish(twist_msg)  # Stop moving
 
 def image_callback(msg):
-    """Process image data to detect the red ball and control DogBot's movement."""
-    global current_state, ball_position
+    global current_state, ball_position, ball_radius
     try:
         cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
     except CvBridgeError as e:
@@ -63,55 +77,32 @@ def image_callback(msg):
 
     center, radius = find_red_ball(cv_image)
     ball_position = center
+    ball_radius = radius
 
-    twist_msg = Twist()
-    if center:
-        x, _ = center
-        err = x - cv_image.shape[1] / 2
-        if current_state == "searching" or current_state == "approaching":
-            current_state = "approaching"
-            if abs(err) > 100:  # Need to rotate DogBot to align with the ball
-                twist_msg.angular.z = -0.005 * err
-            else:  # Move forward towards the ball
-                twist_msg.linear.x = -0.2
-    else:
-        if current_state == "searching":
-            twist_msg.angular.z = 0.5  # Rotate DogBot if the ball is not found
-    cmd_vel_pub.publish(twist_msg)
+    if center and radius >= RADIUS_THRESHOLD:
+        # Ball found and close enough, execute half-circle
+        current_state = "maneuvering"
+        execute_half_circle()
+    elif current_state == "searching":
+        # Ball not found or not close enough, continue searching
+        execute_search()
 
 def execute_half_circle():
-    """Executes a half-circle maneuver to position the robot behind the ball."""
     global current_state
-    
     pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
     twist_msg = Twist()
-
-    # Set angular velocity for a half-circle rotation
-    twist_msg.angular.z = 0.5  # Positive for counter-clockwise, negative for clockwise
-    
-    # Set a small or zero linear velocity to pivot around the ball
-    twist_msg.linear.x = -0.05
-
-    # Calculate duration needed to perform a half-circle maneuver around the ball
-    duration = 2  # Duration in seconds, adjust based on your robot's performance
-
-    # Start the maneuver
+    twist_msg.angular.z = 0.5  # Adjust for half-circle
+    twist_msg.linear.x = 0.05  # Slight forward movement
+    duration = 4  # Adjust for a proper half-circle based on your robot
     start_time = rospy.Time.now().to_sec()
     while rospy.Time.now().to_sec() - start_time < duration:
         pub.publish(twist_msg)
-        rospy.sleep(0.1)  # Sleep briefly to allow message processing
-
-    # Stop the robot after completing the maneuver
     twist_msg.angular.z = 0
     twist_msg.linear.x = 0
-    pub.publish(twist_msg)
-
-    # Update the state to indicate the robot is now positioned behind the ball and ready to push it
-    current_state = "pushing"
-
+    pub.publish(twist_msg)  # Stop
+    current_state = "approaching"
 
 def shutdown():
-    """Ensure DogBot stops moving when the script is terminated."""
     rospy.loginfo("Shutting down")
     cmd_vel_pub.publish(Twist())
     cv2.destroyAllWindows()
@@ -121,7 +112,6 @@ if __name__ == '__main__':
     bridge = CvBridge()
     cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
     rospy.Subscriber("/camera/image", Image, image_callback)
-    rospy.Subscriber("/scan", LaserScan, lidar_callback)  # Subscribe to LiDAR data
 
     rospy.on_shutdown(shutdown)
     cv2.namedWindow("Red Ball Detection", cv2.WINDOW_NORMAL)
@@ -130,3 +120,4 @@ if __name__ == '__main__':
         rospy.spin()
     except KeyboardInterrupt:
         rospy.loginfo("Red ball follower node terminated.")
+