Single Agent Working

Author: rupesh031

server/camera/computer_vision.py

@@ -29,9 +29,11 @@ def __init__(self, manager, config_path, width=700, height=470):
         # Camera object
         self.cam = None
         self.manager = manager
+        self.camera_ip= "http://192.168.137.119:8080/video"
 
     def init_camera(self):
         """Initialize the camera settings."""
+
         self.cam = cv2.VideoCapture(1, cv2.CAP_DSHOW)
         self.cam.set(cv2.CAP_PROP_FRAME_WIDTH, self.width)
         self.cam.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height)

server/camera/config/config2.json

@@ -23,7 +23,7 @@
   ],
   "player": [
     {
-      "aruco_id": 3,
+      "aruco_id": 4,
       "tags": "bot",
       "id": 3
     }

server/camera/test_images/rlAgent1.png

@@ -47,21 +47,41 @@ def detect_shapes_and_colors(image):
 
 
 
+import cv2
+import numpy as np
+
 def detect_aruco_markers(image):
+    # Convert to grayscale
+    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
 
+    # Apply mild Gaussian blur (reduces noise but keeps details)
+    gray = cv2.GaussianBlur(gray, (3, 3), 0)
+
+    # Define the ArUco dictionary and detector parameters
     markerDictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_5X5_50)
-    detectorParam = cv2.aruco.DetectorParameters()
-    detector = cv2.aruco.ArucoDetector(markerDictionary, detectorParam)
-    corners, ids, rejected = detector.detectMarkers(image)
-  
+    detectorParams = cv2.aruco.DetectorParameters()
+    
+    # Improve robustness to shadows
+    detectorParams.adaptiveThreshWinSizeMin = 5
+    detectorParams.adaptiveThreshWinSizeMax = 23
+    detectorParams.adaptiveThreshWinSizeStep = 10
+    detectorParams.minMarkerPerimeterRate = 0.03
+    detectorParams.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_CONTOUR
+
+    detector = cv2.aruco.ArucoDetector(markerDictionary, detectorParams)
+
+    # Detect ArUco markers
+    corners, ids, rejected = detector.detectMarkers(gray)
 
     list_of_aruco = []
-    if ids is not None :
+    if ids is not None:
         for i in range(len(ids)):
-            list_of_aruco.append((ids[i][0],corners[i][0]))
+            list_of_aruco.append((ids[i][0], corners[i][0]))
+    
 
     return list_of_aruco
 
+
 from collections import defaultdict
 
 
@@ -119,6 +139,7 @@ def assign_positions_to_aruco_entities(entities, list_of_aruco):
                 positions = positions_dict[id].pop()
                 entity = grouped_entities[id].pop()
                 result.append((entity,positions))
+    
 
     return result
 

server/camera/test_images/rlAgent2.png

@@ -126,7 +126,7 @@ def __init__(self):
         self.camera_interface = ComputerVisionManager(self, camera_config,width=700,height=470)
         self.aget_interface =  AgentInterface(self,model_path)
         self.webscoket_interface = WebSocketServer(self)
-
+        self.frame_const = 5
         self.frame_rate = 1
 
     def validate_response(self, response: dict):
@@ -198,7 +198,7 @@ def process_frame(self, response, image):
             #     self.webscoket_interface.send_frame(image,"cvframe")
             #     self.frame_rate=20
             # self.frame_rate-=1
-            self.frame_rate = 10
+            self.frame_rate = self.frame_const
             self.webscoket_interface.send_frame(image,"cvframe1")
 
             required_fields = ["bot_pos", "bot_dir", "ball_coords", "goal_coords", "wall_coords", "bot_id" ]
@@ -227,7 +227,7 @@ def process_frame(self, response, image):
                 self.aget_interface.step(cv_frame_data,image)
             else:
                 pass
-            # print("Invalid frame: Missing required fields ->", set(required_fields) - cv_frame_data.keys())
+                print("Invalid frame: Missing required fields ->", set(required_fields) - cv_frame_data.keys())
 
 
         return

server/camera/test_images/rlAgent3.png

@@ -74,10 +74,10 @@ def action_to_vel(self,action):
             v = 1
         elif action == 2:  # Move backward
             v = -1
-        elif action == 3:  # Rotate left
-            w = 1
-        elif action == 4:  # Rotate right
+        elif action == 3:  # Rotate right
             w = -1
+        elif action == 4:  # Rotate left
+            w = 1
         elif action == 5:  # Strafe left (approximate with slight left rotation)
             w = 0.5
         elif action == 6:  # Strafe right (approximate with slight right rotation)
@@ -98,8 +98,7 @@ def step(self, cv_frame_data,image):
         #print(cv_frame_data)
         processed_frame_data,rays = self.agent_observation.addObservation(cv_frame_data)
         # print("added observation")
-        processed_frame_img= visualize_frame(cv_frame_data,processed_frame_data,rays,image)
-        self.manager.webscoket_interface.send_frame(processed_frame_img,"cvframe2")
+       
         # print("ray Data",ray_data)
         # print(ray_data)
 
@@ -122,6 +121,8 @@ def step(self, cv_frame_data,image):
 
         print("actions",action,target_velocity)
 
+        processed_frame_img= visualize_frame(cv_frame_data,processed_frame_data,rays,image,action)
+        self.manager.webscoket_interface.send_frame(processed_frame_img,"cvframe2")
         #return action
 
 # Example usage:

server/camera/test_images/rlAgent4.png

@@ -7,7 +7,7 @@
 class Observation:
     def __init__(self):
         self.data = np.zeros((3, 7, 5), dtype=float)  
-        self.ray_length = 400.0  
+        self.ray_length = 350.0  
         self.num_rays = 7
 
         self.new_frame = np.tile([0, 0, 0, 1, 1], (7, 1)).astype(float)
@@ -21,18 +21,25 @@ def __init__(self):
         self.hit_threshold = 50
 
     def generate_rays(self, origin, angle):
-        """Generate rays from the given origin with a spread of angles."""
-        rays = []
-        angles = np.linspace(angle-90, angle , self.num_rays)
-
-        for theta in angles:
-            theta_rad = np.radians(theta)
-            end_x = origin[0] + self.ray_length * np.cos(theta_rad)
-            end_y = origin[1] + self.ray_length * np.sin(theta_rad)
-
-            ray = geom.LineString([origin, (end_x, end_y)])
-            rays.append(ray)
-
+        """Generate rays in an alternating order around the central angle."""
+        
+        delta_angles = np.linspace(45,-135,self.num_rays)  # Define symmetric spread
+
+        # Generate alternating order (0, -1, +1, -2, +2, ..., -n, +n)
+        mid_idx = len(delta_angles) // 2
+        sorted_indices = np.argsort(np.abs(np.arange(len(delta_angles)) - mid_idx))
+        alt_angles = delta_angles[sorted_indices]  # Reorder angles
+        print(angle,alt_angles-angle)
+        # Convert to radians and apply to base angle
+        angles = np.radians(angle + alt_angles)
+        print(angles)
+        # Vectorized endpoint computation
+        end_x = origin[0] + self.ray_length * np.cos(angles)
+        end_y = origin[1] + self.ray_length * np.sin(angles)
+
+        # Create LineString rays
+        rays = [geom.LineString([origin, (ex, ey)]) for ex, ey in zip(end_x, end_y)]
+        
         return rays
 
     def wall_hit(self, frame_width, frame_height, origin):
@@ -78,6 +85,7 @@ def object_hit(self, target, tag_index, origin):
                 hit_distance = origin_point.distance(intersection)
                 hitFraction = hit_distance / self.ray_length
                 self.update_new_frame(idx, True, tag_index, hitFraction)
+                print("Ray Hit Object",idx)
 
     def polygon_hit(self, polygon_coords, tag_index, origin):
         """Check if any rays intersect a polygon boundary and update hit distances."""

server/camera/test_images/rlAgent5.png

@@ -1,7 +1,7 @@
 import numpy as np
 import cv2
 
-def visualize_frame(cv_frame_data, processed_frame, rays, image):
+def visualize_frame(cv_frame_data, processed_frame, rays, image, agent_action):
     img = image.copy()  # Work on the provided image
 
     # Draw walls (polygon)
@@ -12,12 +12,12 @@ def visualize_frame(cv_frame_data, processed_frame, rays, image):
     # Draw goal area (filled polygon)
     if 'goal_coords' in cv_frame_data and cv_frame_data['goal_coords'].size > 0:
         goal = np.array(cv_frame_data['goal_coords'], np.int32)
-        cv2.fillPoly(img, [goal], color=(0, 255, 0))  # Red
+        cv2.fillPoly(img, [goal], color=(0, 255, 0))  # Green
 
     # Draw ball (circle)
     if 'ball_coords' in cv_frame_data and len(cv_frame_data['ball_coords']) == 2:
         bx, by = cv_frame_data['ball_coords']
-        cv2.circle(img, (int(bx), int(by)), 5, (0, 0, 255), -1)  # Blue
+        cv2.circle(img, (int(bx), int(by)), 5, (0, 0, 255), -1)  # Red
 
     # Draw bot (rotated triangle)
     if 'bot_pos' in cv_frame_data and 'bot_dir' in cv_frame_data:
@@ -27,33 +27,71 @@ def visualize_frame(cv_frame_data, processed_frame, rays, image):
 
         # Calculate triangle vertices
         front = (bot_x + size * np.cos(bot_dir), bot_y + size * np.sin(bot_dir))
-        left = (bot_x + size/2 * np.cos(bot_dir + np.pi/2), bot_y + size/2 * np.sin(bot_dir + np.pi/2))
-        right = (bot_x + size/2 * np.cos(bot_dir - np.pi/2), bot_y + size/2 * np.sin(bot_dir - np.pi/2))
+        left = (bot_x + size / 2 * np.cos(bot_dir + np.pi / 2), bot_y + size / 2 * np.sin(bot_dir + np.pi / 2))
+        right = (bot_x + size / 2 * np.cos(bot_dir - np.pi / 2), bot_y + size / 2 * np.sin(bot_dir - np.pi / 2))
 
         triangle = np.array([(int(front[0]), int(front[1])),
                              (int(left[0]), int(left[1])),
                              (int(right[0]), int(right[1]))], np.int32)
-        cv2.fillPoly(img, [triangle], color=(0, 255, 0))  # Green
+        cv2.fillPoly(img, [triangle], color=(0, 255, 0))  # Green bot
 
-    
+        # Draw action arrow
+        arrow_length = 30
+        arrow_thickness = 2
+        arrow_color = (0, 165, 255)  # Orange
+
+        # Action movement dictionary
+        action_labels = {
+            1: "Move Forward",
+            2: "Move Backward",
+            3: "Rotate Right",
+            4: "Rotate Left",
+            5: "Dir Left",
+            6: "Dir Right"
+        }
 
+        action_text = action_labels.get(agent_action, "No Movement")  # Default if unknown action
+
+        if agent_action == 1:  # Move forward
+            arrow_end = (bot_x + arrow_length * np.cos(bot_dir), bot_y + arrow_length * np.sin(bot_dir))
+        elif agent_action == 2:  # Move backward
+            arrow_end = (bot_x - arrow_length * np.cos(bot_dir), bot_y - arrow_length * np.sin(bot_dir))
+        elif agent_action == 3:  # Rotate left
+            arrow_color = (255, 255, 0)  # Yellow
+            arrow_end = (bot_x + arrow_length * np.cos(bot_dir + np.pi / 4), bot_y + arrow_length * np.sin(bot_dir + np.pi / 4))
+        elif agent_action == 4:  # Rotate right
+            arrow_color = (255, 255, 0)  # Yellow
+            arrow_end = (bot_x + arrow_length * np.cos(bot_dir - np.pi / 4), bot_y + arrow_length * np.sin(bot_dir - np.pi / 4))
+        elif agent_action == 5:  # Strafe left
+            arrow_color = (255, 0, 255)  # Purple
+            arrow_end = (bot_x - arrow_length * np.cos(bot_dir + np.pi / 2), bot_y - arrow_length * np.sin(bot_dir + np.pi / 2))
+        elif agent_action == 6:  # Strafe right
+            arrow_color = (255, 0, 255)  # Purple
+            arrow_end = (bot_x + arrow_length * np.cos(bot_dir - np.pi / 2), bot_y + arrow_length * np.sin(bot_dir - np.pi / 2))
+        else:
+            arrow_end = (bot_x, bot_y)  # No movement
+
+        cv2.arrowedLine(img, (int(bot_x), int(bot_y)), (int(arrow_end[0]), int(arrow_end[1])), arrow_color, arrow_thickness, tipLength=0.3)
+
+        # Draw action text in the top-right corner
+        text_position = (img.shape[1] - 200, 30)  # Top-right corner
+        cv2.putText(img, f"Action: {action_text}", text_position, cv2.FONT_HERSHEY_SIMPLEX, 
+                    0.6, (0, 0, 200), 2, cv2.LINE_AA)  # White text with black outline
+
+        # Draw Rays
         for idx, ray in enumerate(rays):
             start = ray.coords[0]
             end = ray.coords[1]
-           
             hit_info = processed_frame[idx]
             hit_tags = hit_info[:3]
             hit_fraction = hit_info[3]
 
-            # Only draw if there's a hit
-          
             tag_hit = np.argmax(hit_tags)
             colors = [
                 (0, 0, 255),  # Ball (red)
-                (0, 255, 0),  # Goal (blue)
-                (0, 0, 255)   # Wall (green)
+                (0, 255, 0),  # Goal (green)
+                (255, 0, 0)   # Wall (blue)
             ]
-            
 
             # Compute hit point
             dx = end[0] - start[0]
@@ -62,15 +100,13 @@ def visualize_frame(cv_frame_data, processed_frame, rays, image):
             hit_y = start[1] + dy * hit_fraction
 
             # Draw the ray up to the hit point
-            cv2.line(img, (int(start[0]), int(start[1])), (int(hit_x), int(hit_y)), (0, 255, 0) , 1)
+            cv2.line(img, (int(start[0]), int(start[1])), (int(hit_x), int(hit_y)), (0, 255, 0), 1)
 
             # Draw cross at hit point
-            if(not hit_info[4]):
-                # print(hit_info)
-                # print(start,end, hit_x,hit_y)
+            if not hit_info[4]:  # If an object was hit
                 cv2.drawMarker(img, (int(hit_x), int(hit_y)), colors[tag_hit], markerType=cv2.MARKER_CROSS, thickness=2)
 
     # Show the image
     cv2.imshow('Observation Frame', img)
     cv2.waitKey(1)
-    return img
+    return img

server/camera/utils.py

@@ -0,0 +1,35 @@
+import socket
+import threading
+
+# Server setup
+HOST = "0.0.0.0"  # Listen on all interfaces
+PORT = 5000
+
+server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+server.bind((HOST, PORT))
+server.listen(5)
+
+print("✅ Server is listening for ESP32 connections...")
+
+# Handle client connection
+def handle_client(client, addr):
+    print(f"✅ ESP32 Connected from {addr}")
+    try:
+        while True:
+            data = client.recv(1024)
+            if not data:
+                print(f"❌ ESP32 {addr} Disconnected")
+                # break
+            print(f"📩 Received from {addr}: {data.decode('utf-8')}")
+            client.sendall(b"ACK")  # Optional acknowledgment
+    except Exception as e:
+        print(f"❌ Connection Error with {addr}: {e}")
+    finally:
+        client.close()  # Close connection properly
+        print(f"🔌 Connection closed for {addr}")
+
+while True:
+    client, addr = server.accept()
+    client_thread = threading.Thread(target=handle_client, args=(client, addr))
+    client_thread.start()