代替点云

Env/scenario_env.py

@@ -306,43 +306,72 @@ class MultiAgentScenarioEnv(ScenarioEnv):
         return False
 
     def _spawn_controlled_agents(self):
+        """
+        生成应该在当前或之前出现的车辆
+        如果round=0且所有车辆的show_time>0，则生成show_time最小的车辆（保证至少有车辆出现）
+        """
+        vehicles_to_spawn = []
+        
         for car in self.car_birth_info_list:
-            if car['show_time'] == self.round:
-                agent_id = f"controlled_{car['id']}"
-                vehicle_config = {}
-                vehicle = self.engine.spawn_object(
-                    PolicyVehicle,
-                    vehicle_config=vehicle_config,
-                    position=car['begin'],
-                    heading=car['heading']
+            if car['show_time'] <= self.round:
+                vehicles_to_spawn.append(car)
+        
+        # 如果当前round没有车辆应该出现，但车辆列表不为空，则生成最早出现的车辆
+        # 这样可以确保在reset时至少有车辆出现
+        # if len(vehicles_to_spawn) == 0 and len(self.car_birth_info_list) > 0:
+        #     if self.config.get("debug", False):
+        #         self.logger.debug(
+        #             f"No vehicles to spawn at round {self.round}, "
+        #             f"spawning earliest vehicle instead"
+        #         )
+        #     # 找到show_time最小的车辆
+        #     earliest_car = min(self.car_birth_info_list, key=lambda x: x['show_time'])
+        #     vehicles_to_spawn.append(earliest_car)
+        
+        for car in vehicles_to_spawn:
+            agent_id = f"controlled_{car['id']}"
+            
+            # 避免重复生成
+            if agent_id in self.controlled_agents:
+                continue
+                
+            vehicle_config = {}
+            vehicle = self.engine.spawn_object(
+                PolicyVehicle,
+                vehicle_config=vehicle_config,
+                position=car['begin'],
+                heading=car['heading']
+            )
+
+            # 重置车辆状态
+            reset_kwargs = {
+                'position': car['begin'],
+                'heading': car['heading']
+            }
+
+            # 如果启用速度继承，设置初始速度
+            if car.get('velocity') is not None:
+                reset_kwargs['velocity'] = car['velocity']
+
+            vehicle.reset(**reset_kwargs)
+
+            # 设置策略和目的地
+            vehicle.set_policy(self.policy)
+            vehicle.set_destination(car['end'])
+            vehicle.set_expert_vehicle_id(car['id'])
+
+            self.controlled_agents[agent_id] = vehicle
+            self.controlled_agent_ids.append(agent_id)
+
+            # 注册到引擎的 active_agents
+            self.engine.agent_manager.active_agents[agent_id] = vehicle
+
+            if self.config.get("debug", False):
+                self.logger.debug(
+                    f"Spawned vehicle {agent_id} at round {self.round} "
+                    f"(show_time={car['show_time']}), position {car['begin']}"
                 )
 
-                # 重置车辆状态
-                reset_kwargs = {
-                    'position': car['begin'],
-                    'heading': car['heading']
-                }
-
-                # 如果启用速度继承，设置初始速度
-                if car.get('velocity') is not None:
-                    reset_kwargs['velocity'] = car['velocity']
-
-                vehicle.reset(**reset_kwargs)
-
-                # 设置策略和目的地
-                vehicle.set_policy(self.policy)
-                vehicle.set_destination(car['end'])
-                vehicle.set_expert_vehicle_id(car['id'])
-
-                self.controlled_agents[agent_id] = vehicle
-                self.controlled_agent_ids.append(agent_id)
-
-                # 注册到引擎的 active_agents
-                self.engine.agent_manager.active_agents[agent_id] = vehicle
-
-                if self.config.get("debug", False):
-                    self.logger.debug(f"Spawned vehicle {agent_id} at round {self.round}, position {car['begin']}")
-
     def _get_all_obs(self):
         self.obs_list = []
 
@@ -364,8 +393,20 @@ class MultiAgentScenarioEnv(ScenarioEnv):
                             traffic_light = 0
                     break
 
-            lidar = self.engine.get_sensor("lidar").perceive(num_lasers=80, distance=30, base_vehicle=vehicle,
-                                                              physics_world=self.engine.physics_world.dynamic_world)
+            # 使用最近10辆车的相对位置与相对速度替代原80维LiDAR点云
+            lidar_cloud_points, detected_objects = self.engine.get_sensor("lidar").perceive(
+                num_lasers=80,
+                distance=30,
+                base_vehicle=vehicle,
+                physics_world=self.engine.physics_world.dynamic_world
+            )
+            nearest_vehicle_info = self.engine.get_sensor("lidar").get_surrounding_vehicles_info(
+                vehicle,
+                detected_objects,
+                perceive_distance=30,
+                num_others=10,
+                add_others_navi=False
+            )
             side_lidar = self.engine.get_sensor("side_detector").perceive(num_lasers=10, distance=8,
                                                                            base_vehicle=vehicle,
                                                                            physics_world=self.engine.physics_world.static_world)
@@ -374,7 +415,7 @@ class MultiAgentScenarioEnv(ScenarioEnv):
                                                                                       physics_world=self.engine.physics_world.static_world)
 
             obs = (list(state['position'][:2]) + list(state['velocity']) + [state['heading_theta']]
-                   + lidar[0] + side_lidar[0] + lane_line_lidar[0] + [traffic_light]
+                   + nearest_vehicle_info + side_lidar[0] + lane_line_lidar[0] + [traffic_light]
                    + list(vehicle.destination))
 
             self.obs_list.append(obs)