initial integration of parking spot detection. Yet to be completed.

Author: AadarshHegde123

GEMstack/onboard/perception/cone_detection_parking.py

@@ -10,6 +10,7 @@
 import open3d as o3d
 from ..component import Component 
 from ...state import VehicleState, AgentState
+from .parking_utils import *
 
 
 class ConeDetector3D(Component):
@@ -41,6 +42,21 @@ def __init__(self):
         self.sync = ApproximateTimeSynchronizer([self.rgb_sub, self.lidar_sub], queue_size=10, slop=0.1)
         self.sync.registerCallback(self.callback)
 
+
+    def detect_parking_spot(self, cone_3d_centers):
+         cone_ground_centers = np.array(cone_3d_centers)
+         cone_ground_centers_2D = cone_ground_centers[:, :2]
+         # print(f"-----cone_ground_centers_2D: {cone_ground_centers_2D}")
+         candidates = findAllCandidateParkingLot(cone_ground_centers_2D)
+         # print(f"-----candidates: {candidates}")
+         if len(candidates) > 0:
+             closest_spot = candidates[0]
+             # print(f"-----closest_spot: {closest_spot}")
+             # Create parking spot marker
+            #  ros_parking_spot_marker = create_parking_spot_marker(closest_spot, ref_frame="vehicle")
+            #  self.pub_parking_spot_marker.publish(ros_parking_spot_marker)
+         return
+
     def callback(self, img_msg, lidar_msg):
         # Convert data
         image = self.bridge.imgmsg_to_cv2(img_msg, "bgr8")
@@ -77,6 +93,10 @@ def callback(self, img_msg, lidar_msg):
         if centroids:
             self.pub_centroids.publish(self.create_pc2(centroids))
 
+        # Parking space detection logic here
+
+
+
     def undistort_image(self, img):
         h, w = img.shape[:2]
         new_K, _ = cv2.getOptimalNewCameraMatrix(self.K, self.D, (w, h), 1)