cone logging logic?

Author: dh-agarwal

GEMstack/onboard/perception/cone_detection.py

@@ -15,7 +15,8 @@
 import time
 import os
 import yaml
-
+import json
+import matplotlib.pyplot as plt
 
 class ConeDetector3D(Component):
     """
@@ -94,6 +95,9 @@ def __init__(
         self.undistort_map2 = None
         self.camera_front = (camera_name=='front')
 
+        # Logger
+        self.detection_log = []
+
     def rate(self) -> float:
         return 8
 
@@ -432,6 +436,45 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
         end = time.time()
         # print('-------processing time', end -start)
         return self.tracked_agents
+    
+    def log_cone_data_to_file(self, agents_dict, step_n, save_path="cone_log.json"):
+        for agent_id, agent in agents_dict.items():
+            self.detection_log.append({
+                'n': step_n,
+                'id': agent_id,
+                'x': agent.pose.x,
+                'y': agent.pose.y,
+                'orientation': str(agent.activity)
+            })
+        with open(save_path, "w") as f:
+            json.dump(self.detection_log, f, indent=2)
+
+    def visualize_cone_positions(self, log_path="cone_log.json"):
+        with open(log_path, "r") as f:
+            data = json.load(f)
+
+        xs, ys, colors = [], [], []
+        for entry in data:
+            xs.append(entry["x"])
+            ys.append(entry["y"])
+            orientation = entry["orientation"]
+            if orientation == "AgentActivityEnum.RIGHT":
+                colors.append("green")
+            elif orientation == "AgentActivityEnum.LEFT":
+                colors.append("red")
+            elif orientation == "AgentActivityEnum.STANDING":
+                colors.append("blue")
+            else:
+                colors.append("gray")
+
+        plt.figure(figsize=(8, 6))
+        plt.scatter(xs, ys, c=colors, label="Cones")
+        plt.xlabel("X Position (m)")
+        plt.ylabel("Y Position (m)")
+        plt.title("Cone Positions Over Time")
+        plt.grid(True)
+        plt.axis("equal")
+        plt.show()
 
     def save_sensor_data(self, vehicle: VehicleState, latest_image) -> None:
         os.makedirs("data", exist_ok=True)