Update planner logic with real-time detection

Author: htliang517

GEMstack/onboard/planning/racing_planning.py

@@ -517,26 +517,26 @@ def update(self, agents: Dict[str, AgentState], vehicle: VehicleState):
             for id, agent in agents.items():
                 if agent.type == AgentEnum.CONE:
                     # ===== RUNNING ONBOARD =====
-                    cones.append({
-                        'id': id,
-                        'x': agent.pose.x,
-                        'y': agent.pose.y,
-                        'orientation': agent.activity
-                    })
-                    # ===== TESTING ONBOARD in BASIC SIM =====
-                    # if n % 2 == 0:
-                    #     curr_activity = 'LEFT'
-                    # elif n % 2 == 1:
-                    #     curr_activity = 'RIGHT'
-                    # else:
-                    #     curr_activity = 'STANDING'
                     # cones.append({
                     #     'id': id,
                     #     'x': agent.pose.x,
                     #     'y': agent.pose.y,
-                    #     'orientation': curr_activity
+                    #     'orientation': agent.activity
                     # })
-                    # n = n + 1
+                    # ===== TESTING ONBOARD in BASIC SIM =====
+                    if n % 2 == 0:
+                        curr_activity = 'LEFT'
+                    elif n % 2 == 1:
+                        curr_activity = 'RIGHT'
+                    else:
+                        curr_activity = 'STANDING'
+                    cones.append({
+                        'id': id,
+                        'x': agent.pose.x,
+                        'y': agent.pose.y,
+                        'orientation': curr_activity
+                    })
+                    n = n + 1
 
             vehicle_dict = {
                 'position': [vehicle.pose.x, vehicle.pose.y],