integrate spot detection

Author: KenC1014

GEMstack/onboard/perception/cone_detection_parking.py

@@ -15,6 +15,7 @@
 from message_filters import Subscriber, ApproximateTimeSynchronizer
 from cv_bridge import CvBridge
 from .visualization_utils import *
+from .parking_utils import *
 import time
 import math
 import ros_numpy
@@ -322,8 +323,8 @@ def initialize(self):
         self.vis_3d_cones_bboxes = False
 
         # Subscribers
-        self.rgb_sub = Subscriber('/camera_fl/arena_camera_node/image_raw', Image)
-        self.lidar_sub = Subscriber('/ouster/points', PointCloud2)
+        self.rgb_sub = Subscriber('/camera/fl/image_raw', Image)
+        self.lidar_sub = Subscriber('/lidar/top/points', PointCloud2)
         self.sync = ApproximateTimeSynchronizer([self.rgb_sub, self.lidar_sub],
                                                 queue_size=10, slop=0.1)
         self.sync.registerCallback(self.synchronized_callback)
@@ -334,6 +335,7 @@ def initialize(self):
         self.pub_cones_image_detection = rospy.Publisher("cones_detection/annotated_image", Image, queue_size=1)
         self.pub_cones_bboxes_markers = rospy.Publisher("cones_detection/bboxes/markers", MarkerArray, queue_size=10)
         self.pub_cones_centers_pc2 = rospy.Publisher("cones_detection/centers/point_cloud", PointCloud2, queue_size=10)
+        self.pub_parking_spot_marker = rospy.Publisher("parking_spot_detection/marker", MarkerArray, queue_size=10)
 
         # Detection model
         self.model_path = os.getcwd() + '/GEMstack/knowledge/detection/cone.pt'
@@ -357,16 +359,7 @@ def initialize(self):
         self.R_c2l = self.T_c2l[:3, :3]
         self.camera_origin_in_lidar = self.T_c2l[:3, 3]
 
-    def viz_object_states(self, cv_image, boxes):
-        cone_3d_centers = list()
-        cone_3d_dims = list()
-
-        for track_id, agent in self.current_agents.items():
-            if agent.pose.x != None and agent.pose.y != None and agent.pose.z != None:
-               cone_3d_centers.append((agent.pose.x, agent.pose.y, agent.pose.z))
-            if agent.dimensions != None and agent.dimensions[0] != None and agent.dimensions[1] != None and agent.dimensions[2] != None:
-                cone_3d_dims.append(agent.dimensions)
-
+    def viz_object_states(self, cone_3d_centers, cone_3d_dims, cv_image, boxes):
         # Transform top lidar pointclouds to vehicle frame for visualization
         if self.vis_lidar_pc:
             latest_lidar_vehicle = transform_lidar_points(self.latest_lidar_unfiltered, self.T_l2v)
@@ -383,8 +376,11 @@ def viz_object_states(self, cv_image, boxes):
             self.pub_cones_image_detection.publish(ros_img)  
 
         # Create vehicle marker
-        ros_vehicle_marker = create_bbox_marker([[0.0, 0.0, 0.0]], [[0.8, 0.5, 0.3]], (0.0, 1.0, 0.0, 1), "vehicle")
+        ros_vehicle_marker = create_bbox_marker([[0.0, 0.0, 0.0]], [[0.8, 0.5, 0.3]], (0.0, 0.0, 1.0, 1), "vehicle")
         self.pub_vehicle_marker.publish(ros_vehicle_marker)
+        # Delete previous parking spot markers
+        ros_delete_parking_spot_markers = delete_markers("parking_spot", 1)
+        self.pub_parking_spot_marker.publish(ros_delete_parking_spot_markers)
         # Draw 3D cone centers and dimensions
         if len(cone_3d_centers) > 0 and len(cone_3d_dims) > 0:
             if self.vis_3d_cones_centers:
@@ -397,11 +393,25 @@ def viz_object_states(self, cv_image, boxes):
 
             if self.vis_3d_cones_bboxes:
                 # Delete previous markers
-                ros_delete_bboxes_markers = delete_bbox_marker()
+                ros_delete_bboxes_markers = delete_markers("markers", 15)
                 self.pub_cones_bboxes_markers.publish(ros_delete_bboxes_markers)
                 # Create bbox markers from cone dimensions
                 ros_cones_bboxes_markers = create_bbox_marker(cone_3d_centers, cone_3d_dims, (1.0, 0.0, 0.0, 0.4), "vehicle")
                 self.pub_cones_bboxes_markers.publish(ros_cones_bboxes_markers)
+
+    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 synchronized_callback(self, image_msg, lidar_msg):
@@ -573,9 +583,23 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
                 f"Velocity: (vx: {agent.velocity[0]:.3f}, vy: {agent.velocity[1]:.3f}, vz: {agent.velocity[2]:.3f})\n"
             )
 
+        # Now retrieve all cone agents from current agent items
+        cone_3d_centers = list()
+        cone_3d_dims = list()
+
+        for track_id, agent in self.current_agents.items():
+            if agent.pose.x != None and agent.pose.y != None and agent.pose.z != None:
+               cone_3d_centers.append((agent.pose.x, agent.pose.y, agent.pose.z))
+            if agent.dimensions != None and agent.dimensions[0] != None and agent.dimensions[1] != None and agent.dimensions[2] != None:
+                cone_3d_dims.append(agent.dimensions)
+
+        # Detect parking spot if 4 or more cones are detected
+        if len(cone_3d_centers) >= 4:
+            self.detect_parking_spot(cone_3d_centers)
+
         # Add Visualization
         cv_image = self.latest_image.copy()
-        self.viz_object_states(cv_image, bboxes)
+        self.viz_object_states(cone_3d_centers, cone_3d_dims, cv_image, bboxes)
 
         return agents
 

GEMstack/onboard/perception/parking_utils.py

@@ -0,0 +1,107 @@
+import cv2
+import numpy as np
+
+
+GEM_E4_LENGTH = 3.6  # m
+GEM_E4_WIDTH  = 1.5  # m
+
+
+def judgeRectInPolygon(rectPts:np.ndarray, polygonPts:np.ndarray):
+    polygon = polygonPts.reshape((-1, 1, 2)).astype(np.float32)
+    for pt in rectPts:
+        inside = cv2.pointPolygonTest(polygon, tuple(pt), measureDist=False)
+        if inside < 0:
+            return False
+    return True
+
+
+def findAllCandidateParkingLot(cornerPts, angleStepDegree=5, positionStrideMeter=0.5):
+    
+    cornerPts = np.array(cornerPts, dtype=np.float32)
+
+    min_x = np.min(cornerPts[:, 0]) + 0.5
+    max_x = np.max(cornerPts[:, 0]) - 0.5
+    min_y = np.min(cornerPts[:, 1]) + 0.5
+    max_y = np.max(cornerPts[:, 1]) - 0.5
+    
+    candidates = []
+    
+    for angleDegree in np.arange(-90, 90, angleStepDegree):
+        rect = ((0, 0), (GEM_E4_LENGTH, GEM_E4_WIDTH), float(angleDegree))
+        carBox = cv2.boxPoints(rect)
+        
+        for cx in np.arange(min_x, max_x, positionStrideMeter):
+            for cy in np.arange(min_y, max_y, positionStrideMeter):
+                car_box_shifted = carBox + np.array([cx, cy])
+                
+                if judgeRectInPolygon(car_box_shifted, cornerPts):
+                    candidates.append((cx, cy, angleDegree))
+    
+    return candidates
+
+
+def drawCarPose(img, center, angleDegree, color=(0, 0, 255), scale=100):
+    rect = (center, (GEM_E4_LENGTH, GEM_E4_WIDTH), float(angleDegree))
+    box = cv2.boxPoints(rect)  # 四个角点
+    box_scaled = np.int32(box * scale)
+
+    cv2.polylines(img, [box_scaled], isClosed=True, color=color, thickness=2)
+
+
+def visualizeCandidateCarPoses(cornerPts, candidates, scale=100):
+    img = np.zeros((1000, 1000, 3), dtype=np.uint8)
+
+    parking_polygon = np.int32(cornerPts * scale)
+    cv2.polylines(img, [parking_polygon], isClosed=True, color=(0, 255, 0), thickness=2)
+
+    for (x, y, angle) in candidates:
+        drawCarPose(img, (x, y), angle, color=(100, 100, 255), scale=scale)
+
+    cv2.imshow("Parking Candidates", img)
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    cornerPts = np.array([
+        [8,9],
+        [4,5],
+        [1,15],
+        [5,9],
+    ]).astype(np.float32)  # any order
+    
+    
+    candidates = findAllCandidateParkingLot(cornerPts)
+    
+    print(f"find {len(candidates)} candidates")
+    print(f"candidates: {candidates[::10]}")
+    for idx, pose in enumerate(candidates[::10]):
+        thetaDegree = -(90 - (-pose[2])) if abs(- (90 - (-pose[2]))) < 90 else 180 + (-(90 - (-pose[2])))
+        print(f"Candidate {idx}, Pose: centerXY:({pose[0]}, {pose[1]}), thetaDegree:{thetaDegree}")
+        # OpenCV (rotation is from the lowest point, range [-90, 0])
+        # counter-clock-wise is negative
+        # clock-wise is positive
+
+
+    visualizeCandidateCarPoses(cornerPts, candidates[::10])
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    
+    

GEMstack/onboard/perception/visualization_utils.py

@@ -1,9 +1,14 @@
 from sensor_msgs.msg import PointField
 from visualization_msgs.msg import Marker, MarkerArray
+from geometry_msgs.msg import Point
 import sensor_msgs.point_cloud2 as pc2
 import cv2
 import rospy
 import struct
+import math
+
+GEM_E4_LENGTH = 3.6  # m
+GEM_E4_WIDTH  = 1.5  # m
 
 def vis_2d_bbox(image, xywh, box):
     # Setup
@@ -119,14 +124,56 @@ def create_bbox_marker(centroids, dimensions, color = (0.0, 1.0, 1.5, 0.2), ref_
     return marker_array
 
 
-def delete_bbox_marker():
-    """
-    Delete 3D bbox markers given ID ranges
-    """
+def create_parking_spot_marker(closest_spot, length = GEM_E4_LENGTH, width = GEM_E4_WIDTH, ref_frame="map"):
     marker_array = MarkerArray()
-    for i in range(15):
+
+    marker = Marker()
+    marker.header.frame_id = ref_frame
+    marker.header.stamp = rospy.Time.now()
+    marker.ns = "parking_spot"
+    marker.id = 0
+    marker.type = Marker.LINE_STRIP
+    marker.action = Marker.ADD
+    marker.scale.x = 0.05  # Line width
+
+    marker.color.r = 0.0
+    marker.color.g = 1.0
+    marker.color.b = 0.0
+    marker.color.a = 1.0
+
+    x, y, theta_deg = closest_spot
+
+    # Convert theta to radians
+    theta = math.radians(theta_deg)
+
+    # Half-dimensions
+    dx = length / 2.0
+    dy = width / 2.0
+
+    # Define corner offsets in the local frame
+    local_corners = [
+        (-dx, -dy),
+        (-dx, dy),
+        (dx, dy),
+        (dx, -dy),
+        (-dx, -dy)  # close the loop
+    ]
+
+    # Rotate and translate corners to global frame
+    for lx, ly in local_corners:
+        gx = x + lx * math.cos(theta) - ly * math.sin(theta)
+        gy = y + lx * math.sin(theta) + ly * math.cos(theta)
+        marker.points.append(Point(x=gx, y=gy, z=0.0))
+   
+    marker_array.markers.append(marker)
+    return marker_array
+
+
+def delete_markers(ns="markers", max_markers=15):
+    marker_array = MarkerArray()
+    for i in range(max_markers):
         marker = Marker()
-        marker.ns = "markers"
+        marker.ns = ns
         marker.id = i
         marker.action = Marker.DELETE
         marker_array.markers.append(marker)