all testing

Author: AadarshHegde123

.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

GEMstack/knowledge/detection/Cone.pt

@@ -12,7 +12,7 @@ max_accelerator_power:  #Watts.  Power at max accelerator pedal, by gear
   - 10000.0
 max_accelerator_power_reverse: 10000.0 #Watts.  Power (backwards) in reverse gear
 
-acceleration_model : kris_v1
+acceleration_model : hang_v1
 accelerator_active_range : [0.32, 1.0]   #range of accelerator pedal where output acceleration is not flat
 brake_active_range : [0,1]             #range of brake pedal where output deceleration is not flat
 

GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml

@@ -0,0 +1,137 @@
+import cv2
+import numpy as np
+
+# Initialize global variables
+points = []
+
+def order_points(pts):
+    # Sort the points based on their x-coordinates
+    x_sorted = pts[np.argsort(pts[:, 0]), :]
+
+    # Grab the left-most and right-most points from the sorted
+    # x-coordinate points
+    left_most = x_sorted[:2, :]
+    right_most = x_sorted[2:, :]
+
+    # Now, sort the left-most coordinates according to their
+    # y-coordinates so we can grab the top-left and bottom-left
+    # points, respectively
+    left_most = left_most[np.argsort(left_most[:, 1]), :]
+    (tl, bl) = left_most
+
+    # Now, sort the right-most coordinates according to their
+    # y-coordinates so we can grab the top-right and bottom-right
+    # points, respectively
+    right_most = right_most[np.argsort(right_most[:, 1]), :]
+    (tr, br) = right_most
+
+    # Return the coordinates in top-left, top-right,
+    # bottom-right, and bottom-left order
+    return np.array([tl, tr, br, bl], dtype="float32")
+
+def mouse_callback(event, x, y, flags, param):
+    global points
+    if event == cv2.EVENT_LBUTTONDOWN:
+        if len(points) < 4:
+            points.append((x, y))
+            print(f"Point {len(points)} selected: ({x}, {y})")
+            cv2.circle(param['image'], (x, y), 5, (0, 255, 0), -1)
+            cv2.imshow("Select 4 Points", param['image'])
+        if len(points) == 4:
+            print("All points selected. Press any key to proceed.")
+            cv2.waitKey(0)
+            cv2.destroyAllWindows()
+    elif event == cv2.EVENT_RBUTTONDOWN:
+        points = []
+        print("Points cleared. Please select 4 points again.")
+        param['image'] = param['original_image'].copy()
+        cv2.imshow("Select 4 Points", param['image'])
+
+def transform_birdseye(image, src_points, dst_size, camera_in):
+    try:
+        if image is None or image.size == 0:
+            raise ValueError("Invalid image data or failed to load image.")
+
+        # Order points
+        src_pts = order_points(np.array(src_points, dtype="float32"))
+        
+        # Define the output size
+        W, H = dst_size
+        dst_pts = np.array([
+            [0, 0],
+            [W - 1, 0],
+            [W - 1, H - 1],
+            [0, H - 1]
+        ], dtype=np.float32)
+        
+        # Perform undistortion using intrinsics
+        undistorted_image = cv2.undistort(image, camera_in, None)
+        
+        # Compute perspective transform matrix
+        M = cv2.getPerspectiveTransform(src_pts, dst_pts)
+        print("Perspective Transform Matrix:\n", M)
+
+        # Perform perspective warp
+        warped = cv2.warpPerspective(undistorted_image, M, (W, H))
+        return warped, M
+    
+    except Exception as e:
+        print(f"Error during transformation: {e}")
+        return None, None
+
+def main():
+    global points
+    points = []  # Reset points list
+
+    # Camera intrinsics
+    # camera_in = np.array([
+    #     [684.83331299, 0.0, 573.37109375],
+    #     [0.0, 684.60968018, 363.70092773],
+    #     [0.0, 0.0, 1.0]
+    # ], dtype=np.float32)
+
+    camera_in = np.array([
+        [1, 0.0, 0],
+        [0.0, 1, 0],
+        [0.0, 0.0, 1.0]
+    ], dtype=np.float32)
+
+    # Load image
+    # image_path = "/home/aadarshhegde/Documents/GEMstack/data/parking_data-20250312T213928Z-001/parking_data/front_cam97.png"
+    image_path = "/home/aadarshhegde/Documents/GEMstack/data/parking_data-20250312T213928Z-001/parking_data/camera_fl95.png"
+    # image_path = '/home/aadarshhegde/Documents/GEMstack/data/parking_others/parking_data-20250312T213928Z-001/parking_data/camera_fr109.png'
+    image = cv2.imread(image_path)
+
+    if image is None:
+        print("Could not load image. Check the path.")
+        return
+
+    # Clone for display
+    display_image = image.copy()
+    
+    # Select points using mouse
+    print("Select 4 points for the bird's-eye view. Press 'Esc' to finish.")
+    cv2.imshow("Select 4 Points", display_image)
+    cv2.setMouseCallback("Select 4 Points", mouse_callback, {'image': display_image, 'original_image': image.copy()})
+    cv2.waitKey(0)
+
+    if len(points) != 4:
+        print("Error: Please select exactly 4 points.")
+        return
+
+    # Output size (adjust to your needs)
+    out_width = 800
+    out_height = 600
+
+    # Perform the transformation
+    warped, M = transform_birdseye(image, points, (out_width, out_height), camera_in)
+
+    if warped is not None:
+        # Display the results
+        cv2.imshow("Original", image)
+        cv2.imshow("Bird's-Eye View", warped)
+        cv2.waitKey(0)
+        cv2.destroyAllWindows()
+
+if __name__ == "__main__":
+    main()

GEMstack/onboard/perception/birds_eye.py

@@ -505,10 +505,10 @@ def update(self, vehicle: VehicleState) -> Dict[str, Obstacle]:
 
         # === Wider Mouth Parking Spot ===
         cones_wide = {
-            'cone4': (32.0, 13.0, 0.5, 0.5),  # Front Left (wide)
-            'cone5': (36.0, 13.0, 0.5, 0.5),  # Back Left
-            'cone6': (29.0, 8.0, 0.5, 0.5),  # Front Right (wide)
-            'cone7': (33.0, 8.0, 0.5, 0.5)   # Back Right
+            'cone4': (82.0, 15.0, 0.5, 0.5),  # Front Left (wide)
+            'cone5': (86.0, 15.0, 0.5, 0.5),  # Back Left
+            'cone6': (79.0, 10.0, 0.5, 0.5),  # Front Right (wide)
+            'cone7': (83.0, 10.0, 0.5, 0.5)   # Back Right
         }
 
         # Populate the obstacle states

GEMstack/onboard/perception/cone_detection.py

@@ -247,16 +247,16 @@ def update(self, state: AllState):
         x, y, yaw = self.parking_goal
         goal_pose = ObjectPose(
                         t=current_time,
-                        x=x,
-                        y=y,
+                        x=x+1,
+                        y=y+1,
                         z=0.0,
                         yaw=yaw,
                         pitch=0.0,
                         roll=0.0,
                         frame=ObjectFrameEnum.START
                     )
 
-        DISTANCE_THRESHOLD = 8.0
+        DISTANCE_THRESHOLD = 20
         if self.euclidean_distance((x,y), state) > DISTANCE_THRESHOLD: # we are not close enough to the parking spot
             return None
 

GEMstack/onboard/perception/parking_detection.py

@@ -0,0 +1,45 @@
+import numpy as np
+
+# Example: list of cone positions in vehicle frame (X forward, Y left, Z up)
+cones = np.array([
+    [10.17, 10.62],
+    [12.44, 11.22],
+    [11.78, 7.60],
+    [12.48, 7.63],
+    [14.68, 7.54],
+    [17.04, 7.53]
+])
+
+# Step 1: Find pairs of cones closest in Y (forward distance) to vehicle
+# Sort by Y (smallest is closest if +Y is forward)
+cones_sorted = cones[cones[:, 1].argsort()]
+
+# Step 2: Assume first 2 cones are mouth
+mouth = cones_sorted[:2]
+rear = cones_sorted[2:4]  # next 2 furthest back
+
+# Step 3: Midpoints of front and rear
+mouth_center = np.mean(mouth, axis=0)
+rear_center = np.mean(rear, axis=0)
+
+# Step 4: Parking center is midpoint between mouth_center and rear_center
+goal_pos = (mouth_center + rear_center) / 2
+
+# Step 5: Orientation (yaw) is the angle from rear to mouth
+delta = mouth_center - rear_center
+yaw = np.arctan2(delta[1], delta[0])
+
+print(f"Goal Position: x={goal_pos[0]:.2f}, y={goal_pos[1]:.2f}, yaw={np.degrees(yaw):.2f} deg")
+
+
+import matplotlib.pyplot as plt
+
+plt.scatter(cones[:, 0], cones[:, 1], c='red', label='Cones')
+plt.scatter(*goal_pos, c='blue', label='Goal')
+plt.arrow(goal_pos[0], goal_pos[1], 0.5*np.cos(yaw), 0.5*np.sin(yaw), 
+          head_width=0.3, color='blue')
+
+plt.legend()
+plt.gca().set_aspect('equal')
+plt.title('Parking Spot and Goal Pose')
+plt.show()

GEMstack/onboard/perception/parking_space_detection.py

@@ -0,0 +1,121 @@
+import numpy as np
+from scipy.spatial import distance
+from collections import defaultdict
+
+def order_points(points):
+    """Orders points in clockwise order around centroid"""
+    centroid = np.mean(points, axis=0)
+    vectors = points - centroid
+    angles = np.arctan2(vectors[:, 1], vectors[:, 0])
+    return points[np.argsort(angles)]
+
+def find_parking_spots(cone_positions, tolerance=0.1):
+    """Improved parking spot detection that avoids duplicates"""
+    cones_2d = cone_positions[:, :2]
+    n = len(cones_2d)
+    pairs = []
+    
+    # Generate all possible cone pairs
+    for i in range(n):
+        for j in range(i+1, n):
+            p1 = cones_2d[i]
+            p2 = cones_2d[j]
+            midpoint = (round((p1[0]+p2[0])/2/tolerance)*tolerance, 
+                        round((p1[1]+p2[1])/2/tolerance)*tolerance)
+            dist = round(distance.euclidean(p1, p2)/tolerance)*tolerance
+            pairs.append((midpoint, dist, i, j))
+    
+    # Group pairs by midpoint and distance
+    groups = defaultdict(list)
+    for p in pairs:
+        groups[(p[0][0], p[0][1], p[1])].append((p[2], p[3]))
+    
+    # Find valid rectangles
+    processed = set()
+    rectangles = []
+    
+    for key in groups:
+        group_pairs = groups[key]
+        
+        # We need exactly 2 pairs to form a rectangle (4 unique points)
+        if len(group_pairs) >= 2:
+            # Combine all possible pairs in this group
+            for i in range(len(group_pairs)):
+                for j in range(i+1, len(group_pairs)):
+                    pair1 = group_pairs[i]
+                    pair2 = group_pairs[j]
+                    indices = set(pair1 + pair2)
+                    
+                    # Only proceed if we have exactly 4 unique points
+                    if len(indices) == 4:
+                        # Check if these points form a valid rectangle
+                        points = cones_2d[list(indices)]
+                        
+                        # Calculate all pairwise distances
+                        dists = distance.pdist(points)
+                        unique_dists = np.unique(np.round(dists/tolerance)*tolerance)
+                        
+                        # A rectangle should have exactly 2 unique side lengths (with tolerance)
+                        if len(unique_dists) == 2:
+                            sorted_indices = tuple(sorted(indices))
+                            if sorted_indices not in processed:
+                                ordered = order_points(points)
+                                rectangles.append(ordered)
+                                processed.add(sorted_indices)
+    
+    return rectangles
+
+def point_in_polygon(point, polygon):
+    """Ray casting algorithm for point-in-polygon detection"""
+    x, y = point
+    n = len(polygon)
+    inside = False
+    
+    for i in range(n):
+        xi, yi = polygon[i]
+        xj, yj = polygon[(i+1) % n]
+        
+        # Check if point is on edge
+        if (np.isclose(x, xi) and np.isclose(y, yi)) or \
+           (np.isclose(x, xj) and np.isclose(y, yj)):
+            return True
+        
+        # Check intersection with edge
+        if ((yi > y) != (yj > y)):
+            x_intersect = (y - yi) * (xj - xi) / (yj - yi) + xi
+            if x < x_intersect:
+                inside = not inside
+                
+    return inside
+
+def detect_parking_spots(cone_positions, object_positions):
+    """Main detection function with improved accuracy"""
+    rectangles = find_parking_spots(cone_positions)
+    parking_spots = []
+    
+    for rect in rectangles:
+        occupied = any(point_in_polygon(obj[:2], rect) for obj in object_positions)
+        parking_spots.append({
+            "corners": rect,
+            "occupied": occupied
+        })
+    
+    return parking_spots
+
+# Example Usage
+if __name__ == "__main__":
+    cone_positions = np.array([
+        [1, 1, 0], [1, 4, 0], [4, 1, 0], [4, 4, 0],  # Spot 1
+        [4, 1, 0], [4, 4, 0], [9, 1, 0], [9, 4, 0]   # Spot 2
+    ])
+
+    object_positions = np.array([
+        [2.5, 2.5, 0]  # Object inside Spot 1
+    ])
+
+    spots = detect_parking_spots(cone_positions, object_positions)
+
+    for i, spot in enumerate(spots):
+        status = "Occupied" if spot["occupied"] else "Available"
+        print(f"Parking Spot {i+1}: {status}")
+        print(f"Corners: {spot['corners']}\n")