new cone detector code from perception added. performance much improved

Author: AadarshHegde123

GEMstack/onboard/perception/cone_detection.py

@@ -377,9 +377,12 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
         downsample = False
         if self.latest_image is None or self.latest_lidar is None:
             return {}
-
+        lastest_image = self.latest_image.copy()
         # Ensure data/ exists and build timestamp
-
+        if downsample:
+            lidar_down = downsample_points(self.latest_lidar, voxel_size=0.1)
+        else:
+            lidar_down = self.latest_lidar.copy()
         current_time = self.vehicle_interface.time()
         if self.start_time is None:
             self.start_time = current_time
@@ -389,7 +392,7 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
             os.makedirs("data", exist_ok=True)
             tstamp = int(self.vehicle_interface.time() * 1000)
             # 1) Dump raw image
-            cv2.imwrite(f"data/{tstamp}_image.png", self.latest_image)
+            cv2.imwrite(f"data/{tstamp}_image.png", lastest_image)
             # 2) Dump raw LiDAR
             np.savez(f"data/{tstamp}_lidar.npz", lidar=self.latest_lidar)
             # 3) Write BEFORE_TRANSFORM
@@ -425,11 +428,10 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
                     f"frame={mode}\n"
                 )
         start = time.time()
-        undistorted_img, current_K = self.undistort_image(self.latest_image, self.K, self.D)
+        undistorted_img, current_K = self.undistort_image(lastest_image, self.K, self.D)
         end = time.time()
         # print('-------processing time undistort_image---', end -start)
         self.current_K = current_K
-        self.latest_image = undistorted_img
         orig_H, orig_W = undistorted_img.shape[:2]
 
         # --- Begin modifications for three-angle detection ---
@@ -484,10 +486,7 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
                             cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
             cv2.imshow("Detection - Cone 2D", undistorted_img)
 
-        if downsample:
-            lidar_down = downsample_points(self.latest_lidar, voxel_size=0.1)
-        else:
-            lidar_down = self.latest_lidar.copy()
+
         start = time.time()
         pts_cam = transform_points_l2c(lidar_down, self.T_l2c)
         projected_pts = project_points(pts_cam, self.current_K, lidar_down)
@@ -636,13 +635,13 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
         if not self.enable_tracking:
             for agent_id, agent in self.current_agents.items():
                 p = agent.pose
-                # rospy.loginfo(
-                #     f"Agent ID: {agent_id}\n"
-                #     f"Pose: (x: {p.x:.3f}, y: {p.y:.3f}, z: {p.z:.3f}, "
-                #     f"yaw: {p.yaw:.3f}, pitch: {p.pitch:.3f}, roll: {p.roll:.3f})\n"
-                #     f"Velocity: (vx: {agent.velocity[0]:.3f}, vy: {agent.velocity[1]:.3f}, vz: {agent.velocity[2]:.3f})\n"
-                #     f"type:{agent.activity}"
-                # )
+                rospy.loginfo(
+                    f"Agent ID: {agent_id}\n"
+                    f"Pose: (x: {p.x:.3f}, y: {p.y:.3f}, z: {p.z:.3f}, "
+                    f"yaw: {p.yaw:.3f}, pitch: {p.pitch:.3f}, roll: {p.roll:.3f})\n"
+                    f"Velocity: (vx: {agent.velocity[0]:.3f}, vy: {agent.velocity[1]:.3f}, vz: {agent.velocity[2]:.3f})\n"
+                    f"type:{agent.activity}"
+                )
             end = time.time()
             # print('-------processing time', end -start)
             return self.current_agents