add viz

Author: KenC1014

GEMstack/onboard/perception/parking_detection.py

@@ -1,28 +1,57 @@
 import rospy
 import numpy as np
-import cv2
-from cv_bridge import CvBridge
-from sensor_msgs.msg import PointCloud2, Image
+from sensor_msgs.msg import PointCloud2
 from typing import Dict
-import sensor_msgs.point_cloud2 as pc2
-import ros_numpy
-from ultralytics import YOLO
-from message_filters import Subscriber, ApproximateTimeSynchronizer
 from ..component import Component 
 from ...state import AgentState, ObjectPose, ObjectFrameEnum, Obstacle, ObstacleMaterialEnum
 from ..interface.gem import GEMInterface
-from scipy.spatial import ConvexHull
-from .parking_utils import *
-from .visualization_utils import *
+from .utils.detection_utils import *
+from .utils.parking_utils import *
+from .utils.visualization_utils import *
 
 
 class ParkingSpotsDetector3D(Component):
     def __init__(self, vehicle_interface: GEMInterface):
         # Init Variables
         self.vehicle_interface = vehicle_interface
+        self.cone_pts_3D = []
+        self.ordered_cone_ground_centers_2D = []
         self.goal_parking_spot = None
-        self.cone_pts_3d = []
-        self.visualization = False
+        self.parking_obstacles_pose = []
+        self.parking_obstacles_dim = []
+        self.ground_threshold = -0.15
+        self.visualization = True
+        self.T_l2v = np.array([[0.99939639, 0.02547917, 0.023615, 1.1],
+                               [-0.02530848, 0.99965156, -0.00749882, 0.03773583],
+                               [-0.02379784, 0.00689664, 0.999693, 1.95320223],
+                               [0., 0., 0., 1.]])
+
+
+        # Subscribers (note: we need top lidar only for visualization)
+        self.lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, self.callback, queue_size=1)
+
+        # Publishers (all topics are for visualization purposes)
+        self.pub_lidar_top_vehicle_pc2 = rospy.Publisher("lidar_top_vehicle/point_cloud", PointCloud2, queue_size=10)
+        self.pub_vehicle_marker = rospy.Publisher("vehicle/marker", 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)
+        self.pub_polygon_marker = rospy.Publisher("polygon_detection/marker", MarkerArray, queue_size=10)
+        self.pub_obstacles_marker = rospy.Publisher("obstacle_detection/marker", MarkerArray, queue_size=10)
+
+    def callback(self, top_lidar_msg):
+        # Top lidar points in ouster frame
+        lidar_ouster_frame = pc2_to_numpy(top_lidar_msg)
+
+        # Visualize everything in vehicle frame
+        if self.visualization:
+            self.visualize( 
+                self.cone_pts_3D,
+                self.ordered_cone_ground_centers_2D,
+                self.goal_parking_spot,
+                self.parking_obstacles_pose,
+                self.parking_obstacles_dim,
+                lidar_ouster_frame
+            )
 
     def spin(self):
         rospy.spin()
@@ -35,32 +64,62 @@ def state_inputs(self) -> list:
 
     def state_outputs(self) -> list:
         return ['goal', 'obstacles']
-    
-    def order_points_convex_hull(self, points_2d):
-        points_np = np.array(points_2d)
-        hull = ConvexHull(points_np)
-        ordered = [points_np[i] for i in hull.vertices]
-        return ordered
-    
-    def detect_parking_spot(self, cone_3d_centers):
-        # Initial variables
-        goal_parking_spot = None
-        parking_obstacles_pose = []
-        parking_obstacles_dim = []
 
-        # Get 2D cone centers
-        cone_ground_centers = np.array(cone_3d_centers)
-        cone_ground_centers_2D = cone_ground_centers[:, :2]
-        ordered_cone_ground_centers_2D = self.order_points_convex_hull(cone_ground_centers_2D)
-        # print(f"-----cone_ground_centers_2D: {cone_ground_centers_2D}")
-        candidates = find_all_candidate_parking_spots(ordered_cone_ground_centers_2D)
-        # print(f"-----candidates: {candidates}")
-        if len(candidates) > 0:
-            parking_obstacles_pose, parking_obstacles_dim = get_parking_obstacles(ordered_cone_ground_centers_2D)
-            # print(f"-----parking_obstacles: {self.parking_obstacles_pose}")
-            goal_parking_spot = select_best_candidate(candidates, ordered_cone_ground_centers_2D)
-            # print(f"-----goal_parking_spot: {self.goal_parking_spot}")
-        return goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D
+
+    def visualize(self, 
+                cone_pts_3D,
+                ordered_cone_ground_centers_2D,
+                goal_parking_spot,
+                parking_obstacles_pose,
+                parking_obstacles_dim,
+                lidar_ouster_frame):
+        # Transform top lidar pointclouds to vehicle frame for visualization
+        latest_lidar_vehicle = transform_points(lidar_ouster_frame, self.T_l2v)
+        latest_lidar_vehicle = filter_ground_points(latest_lidar_vehicle, self.ground_threshold)
+        ros_lidar_top_vehicle_pc2 = create_point_cloud(latest_lidar_vehicle, (255, 0, 0), "vehicle")
+        self.pub_lidar_top_vehicle_pc2.publish(ros_lidar_top_vehicle_pc2)
+
+        # Create vehicle marker
+        ros_vehicle_marker = create_markers([[0.0, 0.0, 0.0, 0.0]], 
+                                            [[0.8, 0.5, 0.3]], 
+                                            (0.0, 0.0, 1.0, 1), 
+                                            "markers", "vehicle")
+        self.pub_vehicle_marker.publish(ros_vehicle_marker)
+
+        # Delete previous markers
+        ros_delete_polygon_marker = delete_markers("polygon", 1)
+        self.pub_polygon_marker.publish(ros_delete_polygon_marker)
+        ros_delete_parking_spot_markers = delete_markers("parking_spot", 1)
+        self.pub_parking_spot_marker.publish(ros_delete_parking_spot_markers)
+        ros_delete_obstacles_markers = delete_markers("obstacles", 5)
+        self.pub_obstacles_marker.publish(ros_delete_obstacles_markers)
+
+        # Draw polygon first
+        if len(ordered_cone_ground_centers_2D) > 0:
+            ros_polygon_marker = create_polygon_marker(ordered_cone_ground_centers_2D, ref_frame="vehicle")
+            self.pub_polygon_marker.publish(ros_polygon_marker)
+
+        # Create parking spot marker
+        if goal_parking_spot:
+            ros_parking_spot_marker = create_parking_spot_marker(goal_parking_spot, ref_frame="vehicle")
+            self.pub_parking_spot_marker.publish(ros_parking_spot_marker)
+
+        # Create parking obstacles marker
+        if parking_obstacles_pose and parking_obstacles_dim:
+            ros_obstacles_marker =  create_markers(parking_obstacles_pose, 
+                                                      parking_obstacles_dim, 
+                                                      (1.0, 0.0, 0.0, 0.4), 
+                                                      "obstacles", "vehicle")
+            self.pub_obstacles_marker.publish(ros_obstacles_marker)
+
+        # Draw 3D cone centers
+        if len(cone_pts_3D) > 0:
+            cone_ground_centers = np.array(cone_pts_3D)
+            cone_ground_centers[:, 2] = 0.0
+            cone_ground_centers = [tuple(point) for point in cone_ground_centers]
+            ros_cones_centers_pc2 = create_point_cloud(cone_ground_centers, color=(255, 0, 255))
+            self.pub_cones_centers_pc2.publish(ros_cones_centers_pc2)
+
 
     def update(self, agents: Dict[str, AgentState]):
         # Initial variables
@@ -76,8 +135,15 @@ def update(self, agents: Dict[str, AgentState]):
             cone_pts_3D.append(cone_pt_3D)
 
         # Detect parking spot
-        if len(cone_pts_3D) >= 4:
-            goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D = self.detect_parking_spot(cone_pts_3D)
+        if len(cone_pts_3D) == 4:
+            goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D = detect_parking_spot(cone_pts_3D)
+
+        # Update local variables for visualization
+        self.cone_pts_3D = cone_pts_3D
+        self.ordered_cone_ground_centers_2D = ordered_cone_ground_centers_2D
+        self.goal_parking_spot = goal_parking_spot
+        self.parking_obstacles_pose = parking_obstacles_pose
+        self.parking_obstacles_dim = parking_obstacles_dim
 
         # Return if no goal parking spot is found
         if not goal_parking_spot:

GEMstack/onboard/perception/utils/detection_utils.py

@@ -0,0 +1,25 @@
+import ros_numpy
+import numpy as np
+
+def transform_points(points, transform):
+    ones_column = np.ones((points.shape[0], 1))
+    points_extended = np.hstack((points, ones_column))
+    points_transformed = ((transform @ (points_extended.T)).T)
+    return points_transformed[:, :3]
+    
+
+def filter_ground_points(lidar_points, ground_threshold = 0):
+    filtered_array = lidar_points[lidar_points[:, 2] > ground_threshold]
+    return filtered_array
+    
+
+def pc2_to_numpy(pc2_msg, want_rgb=False):
+    # Convert the ROS message to a numpy structured array
+    pc = ros_numpy.point_cloud2.pointcloud2_to_array(pc2_msg)
+    # Stack x,y,z fields to a (N,3) array
+    pts = np.stack((np.array(pc['x']).ravel(),
+                    np.array(pc['y']).ravel(),
+                    np.array(pc['z']).ravel()), axis=1)
+    # Apply filtering (for example, x > 0 and z in a specified range)
+    mask = (pts[:, 0] > 0) & (pts[:, 2] < -1.5) & (pts[:, 2] > -2.7)
+    return pts[mask]

…tack/onboard/perception/parking_utils.py …nboard/perception/utils/parking_utils.pyGEMstack/onboard/perception/parking_utils.py renamed to GEMstack/onboard/perception/utils/parking_utils.py GEMstack/onboard/perception/parking_utils.py renamed to GEMstack/onboard/perception/utils/parking_utils.py

@@ -1,6 +1,7 @@
 import cv2
 import math
 import numpy as np
+from scipy.spatial import ConvexHull
 
 
 GEM_E4_LENGTH = 3.6  # m
@@ -188,4 +189,38 @@ def get_parking_obstacles(vertices):
     filtered_positions = [segment_info[i][0] for i in range(len(segment_info)) if i not in indices_to_remove]
     filtered_dimensions = [segment_info[i][1] for i in range(len(segment_info)) if i not in indices_to_remove]
 
-    return filtered_positions, filtered_dimensions
+    return filtered_positions, filtered_dimensions
+
+# def order_points_all(points_2d):
+#     points_np = np.array(points_2d)
+#     hull = ConvexHull(points_np)
+#     ordered = [points_np[i] for i in hull.vertices]
+#     return ordered
+
+def order_points_all(points_2d):
+    points = np.array(points_2d)
+    centroid = np.mean(points, axis=0)
+    angles = np.arctan2(points[:,1] - centroid[1], points[:,0] - centroid[0])
+    sorted_indices = np.argsort(angles)
+    return points[sorted_indices]
+    
+
+def detect_parking_spot(cone_3d_centers):
+    # Initial variables
+    goal_parking_spot = None
+    parking_obstacles_pose = []
+    parking_obstacles_dim = []
+
+    # Get 2D cone centers
+    cone_ground_centers = np.array(cone_3d_centers)
+    cone_ground_centers_2D = cone_ground_centers[:, :2]
+    ordered_cone_ground_centers_2D = order_points_all(cone_ground_centers_2D)
+    # print(f"-----cone_ground_centers_2D: {len(ordered_cone_ground_centers_2D)}")
+    candidates = find_all_candidate_parking_spots(ordered_cone_ground_centers_2D)
+    # print(f"-----candidates: {candidates}")
+    if len(candidates) > 0:
+        parking_obstacles_pose, parking_obstacles_dim = get_parking_obstacles(ordered_cone_ground_centers_2D)
+        # print(f"-----parking_obstacles: {self.parking_obstacles_pose}")
+        goal_parking_spot = select_best_candidate(candidates, ordered_cone_ground_centers_2D)
+        # print(f"-----goal_parking_spot: {self.goal_parking_spot}")
+    return goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D