modify update flow

Author: KenC1014

GEMstack/onboard/perception/parking_detection.py

@@ -168,39 +168,25 @@ def update(self, cone_obstacles: Dict[str, ObstacleState]):
                 grouped_ordered_ground_centers_2D.append(ordered_ground_centers_2D)
                 parking_obstacles_poses += parking_obstacles_pose
                 parking_obstacles_dims += parking_obstacles_dim
-        
-        # Return if no goal parking spot is found
-        if len(goal_parking_spots) < 1:
-            return None
-        
-        # Now select the closest parking goal
-        if len(goal_parking_spots) > 1:
-            self.goal_parking_spot = closest_point_to_origin(goal_parking_spots)
-        else:
-            self.goal_parking_spot = goal_parking_spots[0]
 
         # Update local variables for visualization
         self.cone_pts_3D = cone_pts_3D
         self.grouped_ordered_ground_centers_2D = grouped_ordered_ground_centers_2D
         self.parking_obstacles_poses = parking_obstacles_poses
         self.parking_obstacles_dims = parking_obstacles_dims
-
+        
+        # Now select the closest parking goal, return if none is found
         if len(goal_parking_spots) > 1:
-            print("\n")
-            print(f"self.cone_pts_3D: {self.cone_pts_3D}")
-            print("\n")
-            print(f"self.all_ordered_ground_centers_2D: {self.grouped_ordered_ground_centers_2D}")
-            print("\n")
-            print(f"self.goal_parking_spot: {self.goal_parking_spot}")
-            print("\n")
-            print(f"self.parking_obstacles_pose: {self.parking_obstacles_poses}")
-            print("\n")
-            print(f"self.parking_obstacles_dims: {self.parking_obstacles_dims}")
-            print("\n")
+            self.goal_parking_spot = closest_point_to_origin(goal_parking_spots)
+        elif len(goal_parking_spots) == 1:
+            self.goal_parking_spot = goal_parking_spots[0]
+        else:
+            self.goal_parking_spot = None
+            return None
 
         # Constructing parking obstacles
         current_time = self.vehicle_interface.time()
-        obstacle_id = 0
+        obstacle_id = 30
         parking_obstacles = {}
         for o_pose, o_dim in zip(self.parking_obstacles_poses, self.parking_obstacles_dims):
             x, y, z, yaw = o_pose

GEMstack/onboard/perception/spot_corner_detection.py

@@ -7,7 +7,7 @@
 from ..component import Component
 from ..interface.gem import GEMInterface
 from ...state import VehicleState, Obstacle, ObjectPose, ObjectFrameEnum, ObstacleState, ObstacleMaterialEnum
-from sensor_msgs.msg import Image, PointCloud2
+from sensor_msgs.msg import Image
 from .utils.constants import *
 from .utils.detection_utils import *
 from .utils.visualization_utils import *
@@ -35,7 +35,6 @@ def __init__(
 
         # Publishers
         self.pub_detection_fr = rospy.Publisher("/parking_spot_detection/annotated_image/front_right", Image, queue_size=1)
-        self.pub_detection_corners_pc2 = rospy.Publisher("/parking_spot_detectio/corners/point_cloud", PointCloud2, queue_size=10)
 
     # Main sensors callback
     def callback(self, right_cam_msg):
@@ -134,16 +133,11 @@ def visualize(self, image, centers, corners, approxes, corners_3d_vehicle_frame)
             for approx in approxes:
                 cv2.polylines(image, [approx], isClosed=True, color=(0, 255, 0), thickness=5)
 
-        # Draw 3D corners
-        # ros_detection_corners_pc2 = create_point_cloud(np.array(corners_3d_vehicle_frame).reshape(-1, 3), LIDAR_PC_COLOR, VEHICLE_FRAME)
-        # self.pub_detection_corners_pc2.publish(ros_detection_corners_pc2)
-
         # Publish the annotated
         right_cam_annotated_ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
         self.pub_detection_fr.publish(right_cam_annotated_ros_img)
 
 
-
     def spin(self):
         rospy.spin()
 

GEMstack/onboard/perception/utils/constants.py

@@ -30,6 +30,6 @@ def ignore_relative_path(loader, node):
 OBSTACLE_MARKER_COLOR = (1.0, 0.0, 0.0, 0.4)
 LIDAR_PC_COLOR = (255, 0, 0)
 CONE_CENTER_PC_COLOR = (255, 0, 255)
-MAX_POLYGON_MARKERS = 3
-MAX_PARKING_SPOT_MARKERS = 1
+MAX_POLYGON_MARKERS = 5
+MAX_PARKING_SPOT_MARKERS = 5
 MAX_OBSTACLE_MARKERS = 10

GEMstack/onboard/perception/utils/math_utils.py

@@ -10,10 +10,8 @@ def group_points_by_multiple_of_k(points, k):
 
 def closest_point_to_origin(points):
     if not points:
-        return None  # Return None if the list is empty
-
+        return None
     points_array = np.array(points)
-    distances = np.linalg.norm(points_array, axis=1)  # Compute Euclidean distances
-    min_index = np.argmin(distances)  # Index of the closest point
-
+    distances = np.linalg.norm(points_array, axis=1)
+    min_index = np.argmin(distances)
     return tuple(points_array[min_index])

GEMstack/onboard/perception/utils/visualization_utils.py

@@ -212,7 +212,7 @@ def create_parking_spot_marker(closest_spot, length=GEM_E4_LENGTH, width=GEM_E4_
     # Transparency and color (green)
     marker.color.r = 0.0
     marker.color.g = 1.0
-    marker.color.b = 0.0
+    marker.color.b = 1.0
     marker.color.a = 0.5
 
     marker.pose.orientation.w = 1.0