BC算法实现

Env/bc_env.py

@@ -0,0 +1,64 @@
+from Env.scenario_env import MultiAgentScenarioEnv
+import numpy as np
+
+class BCScenarioEnv(MultiAgentScenarioEnv):
+    """
+    Environment for Behavior Cloning Evaluation.
+    Uses the same 45-dim observation as ExpertReplayEnv:
+    - Ego State (5): x, y, vx, vy, heading
+    - Neighbors (40): 10 nearest * (rel_x, rel_y, vx, vy)
+    """
+    def _get_all_obs(self):
+        # Implement custom observation: 30m range, 10 nearest vehicles
+        obs_dict = {}
+        
+        for agent_id, vehicle in self.controlled_agents.items():
+            # 1. Ego State
+            ego_state = [
+                vehicle.position[0], vehicle.position[1],
+                vehicle.velocity[0], vehicle.velocity[1],
+                vehicle.heading_theta
+            ]
+            
+            # 2. Neighbors
+            neighbors = []
+            # Iterate through all vehicles in the engine
+            candidates = []
+            # Use engine.agent_manager.active_agents to find neighbors
+            # Note: This includes background vehicles if they are in active_agents
+            for other_id, other_vehicle in self.engine.agent_manager.active_agents.items():
+                if other_id == agent_id:
+                    continue
+                
+                # Check if vehicle is valid/active
+                # (MetaDrive manages active_agents, so they should be active)
+                
+                dist = np.linalg.norm(vehicle.position - other_vehicle.position)
+                if dist < 30.0:
+                    candidates.append((dist, other_vehicle))
+            
+            # Sort by distance
+            candidates.sort(key=lambda x: x[0])
+            
+            # Take top 10
+            top_10 = candidates[:10]
+            
+            neighbor_feats = []
+            for _, neighbor in top_10:
+                neighbor_feats.extend([
+                    neighbor.position[0] - vehicle.position[0], # Relative pos
+                    neighbor.position[1] - vehicle.position[1],
+                    neighbor.velocity[0], # Absolute vel
+                    neighbor.velocity[1]
+                ])
+                
+            # Pad if < 10
+            missing = 10 - len(top_10)
+            if missing > 0:
+                neighbor_feats.extend([0.0] * (4 * missing))
+                
+            # Flatten
+            obs = np.array(ego_state + neighbor_feats, dtype=np.float32)
+            obs_dict[agent_id] = obs
+            
+        return obs_dict

Env/scenario_env.py

@@ -100,6 +100,33 @@ class MultiAgentScenarioEnv(ScenarioEnv):
         for scenario_id in _obj_to_clean_this_frame:
             self.engine.traffic_manager.current_traffic_data.pop(scenario_id)
 
+        # Fix: Ensure all objects are cleared properly before reset
+        # Instead of manually clearing, we just let the engine handle it, but we might need to 
+        # ensure no stale references in managers.
+        
+        # The error "KeyError" in clear_objects usually means we are trying to clear an object 
+        # that is already gone from _spawned_objects but still tracked by a manager.
+        
+        # Try to clear only objects that actually exist in the engine
+        # existing_objects = list(self.engine.get_objects().keys())
+        # if existing_objects:
+        #      self.engine.clear_objects(existing_objects)
+        
+        # Force clear agent manager's spawned objects to avoid stale references
+        if hasattr(self.engine, 'agent_manager') and self.engine.agent_manager:
+            # Check if it's ScenarioAgentManager or VehicleAgentManager
+            # ScenarioAgentManager might not have spawned_objects directly exposed or named differently
+            # But BaseAgentManager usually has it.
+            # If it's ScenarioAgentManager, it might be using a different structure.
+            
+            # Safe clear for BaseAgentManager subclasses
+            if hasattr(self.engine.agent_manager, 'spawned_objects'):
+                self.engine.agent_manager.spawned_objects.clear()
+            
+            # Also clear active_objects if present (VehicleAgentManager uses this)
+            if hasattr(self.engine.agent_manager, '_active_objects'):
+                self.engine.agent_manager._active_objects.clear()
+                
         self.engine.reset()
         self.reset_sensors()
         self.engine.taskMgr.step()