diff --git a/src/InterconVis_PedSafety_Enh/cvips_collector.py b/src/InterconVis_PedSafety_Enh/cvips_collector.py index 742c186cb2..4246500e60 100644 --- a/src/InterconVis_PedSafety_Enh/cvips_collector.py +++ b/src/InterconVis_PedSafety_Enh/cvips_collector.py @@ -24,7 +24,9 @@ writing_thread_running = True def configure_weather(world, weather_arg, time_arg): - """设置天气和时间的核心逻辑""" + """ + 【完整保留】最初的天气和时间逻辑 + """ presets = { 'Clear': carla.WeatherParameters.ClearNoon, 'Cloudy': carla.WeatherParameters.CloudyNoon, @@ -32,100 +34,102 @@ def configure_weather(world, weather_arg, time_arg): 'Rain': carla.WeatherParameters.HardRainNoon, 'Storm': carla.WeatherParameters.HardRainSunset } + weather = presets.get(weather_arg, carla.WeatherParameters.ClearNoon) # 修改太阳高度角模拟时间 - if time_arg == 'Day': weather.sun_altitude_angle = 60.0 - elif time_arg == 'Sunset': weather.sun_altitude_angle = 10.0 - elif time_arg == 'Night': weather.sun_altitude_angle = -90.0 - + if time_arg == 'Day': + weather.sun_altitude_angle = 60.0 + elif time_arg == 'Sunset': + weather.sun_altitude_angle = 10.0 + elif time_arg == 'Night': + weather.sun_altitude_angle = -90.0 + world.set_weather(weather) - print(f"🌦️ 环境已切换: {weather_arg} | {time_arg}") + print(f"🌦️ 环境设置成功: {weather_arg} | {time_arg}") -def is_visible(world, camera_actor, target_actor): - """射线检测:判断是否被遮挡""" +def is_visible_refined(world, camera_actor, target_actor): + """ + 【修复版】可见性检测:针对两轮车做了高度优化 + """ cam_trans = camera_actor.get_transform() - cam_loc = cam_trans.location - forward = cam_trans.get_forward_vector() - # 起点偏移防止撞到自己车头 - ray_start = cam_loc + carla.Location(x=forward.x*2.5, y=forward.y*2.5, z=forward.z*2.5) + # 起点:相机位置 + 向上提一点,彻底避开主车车身 + ray_start = cam_trans.location + carla.Location(z=0.5) + cam_trans.get_forward_vector() * 2.0 - target_loc = target_actor.get_transform().location - target_loc.z += 1.0 + # 终点:两轮车较低,瞄准 z=0.6,轿车/行人瞄准 z=1.0 + target_z = 0.6 if 'bike' in target_actor.type_id or 'bicycle' in target_actor.type_id or 'motorcycle' in target_actor.type_id else 1.0 + target_loc = target_actor.get_transform().location + carla.Location(z=target_z) hit_points = world.cast_ray(ray_start, target_loc) - for hit in hit_points: - if hit.location.distance(ray_start) < target_loc.distance(ray_start) - 2.0: - return False - return True + if not hit_points: return True # 路径没撞击到任何东西,肯定可见 + + # 只要第一个碰撞点离目标中心较近,就判定为可见(防止因车体大而误判) + return hit_points[0].location.distance(target_loc) < 3.0 -def draw_box_simple(img, target, w2c, K): - """绘制3D绿框""" +def draw_box(img, target, w2c, K): + """ + 【稳定版】绘制3D框,行人红色,车绿色,两轮车紫色 + """ loc, rot, extent = target['location'], target['rotation'], target['extent'] offset = target.get('center_offset', [0, 0, 0]) + obj_t = carla.Transform(carla.Location(x=loc[0], y=loc[1], z=loc[2]), carla.Rotation(pitch=rot[0], yaw=rot[1], roll=rot[2])) obj_to_world = utils.get_matrix(obj_t) dx, dy, dz = extent[0], extent[1], extent[2] ox, oy, oz = offset[0], offset[1], offset[2] + corners_local = np.array([ [ox+dx, oy+dy, oz+dz, 1], [ox+dx, oy-dy, oz+dz, 1], [ox+dx, oy-dy, oz-dz, 1], [ox+dx, oy+dy, oz-dz, 1], [ox-dx, oy+dy, oz+dz, 1], [ox-dx, oy-dy, oz+dz, 1], [ox-dx, oy-dy, oz-dz, 1], [ox-dx, oy+dy, oz-dz, 1] ]) + img_pts = [] - points_behind = 0 for pt in corners_local: w_pos = np.dot(obj_to_world, pt) p = utils.get_image_point(carla.Location(x=w_pos[0], y=w_pos[1], z=w_pos[2]), K, w2c) - if p is None: - points_behind += 1 - img_pts.append((0,0)) - else: img_pts.append(p) - if points_behind > 4: return img + img_pts.append(p) + edges = [(0,1), (1,2), (2,3), (3,0), (4,5), (5,6), (6,7), (7,4), (0,4), (1,5), (2,6), (3,7)] - color = (0, 255, 0) if target['type'] == 'vehicle' else (0, 0, 255) + if target['type'] == 'walker': color = (0, 0, 255) + elif target['type'] == 'bike': color = (255, 0, 255) + else: color = (0, 255, 0) + for s, e in edges: - if img_pts[s] != (0,0) and img_pts[e] != (0,0): + if img_pts[s] and img_pts[e]: cv2.line(img, tuple(img_pts[s]), tuple(img_pts[e]), color, 2) return img -def save_worker(q): - while writing_thread_running or not q.empty(): - try: - task = q.get(timeout=0.1) - path, fid = task['path'], task['fid'] - task['ego_img'].save_to_disk(f"{path}/ego_rgb/{fid:08d}.jpg") - task['rsu_img'].save_to_disk(f"{path}/rsu_rgb/{fid:08d}.jpg") - with open(f"{path}/label/{fid:08d}.json", 'w') as f: - json.dump(task['json_data'], f, indent=2) - q.task_done() - except: continue - def main(): + # 【恢复】所有原始参数解析 parser = argparse.ArgumentParser() - parser.add_argument('--town', default='Town05') + parser.add_argument('--town', default='Town01') parser.add_argument('--weather', default='Clear', choices=['Clear', 'Cloudy', 'Wet', 'Rain', 'Storm']) parser.add_argument('--time', default='Day', choices=['Day', 'Sunset', 'Night']) parser.add_argument('--num_vehicles', default=50, type=int) - parser.add_argument('--num_walkers', default=50, type=int) # 行人数量 - parser.add_argument('--max_frames', default=1000, type=int) + parser.add_argument('--num_walkers', default=30, type=int) + parser.add_argument('--max_frames', default=500, type=int) args = parser.parse_args() - scene_id = f"{args.town}_{args.weather}_{args.time}" - save_path = os.path.join(OUTPUT_FOLDER, scene_id) - for sub in ['ego_rgb', 'rsu_rgb', 'label']: os.makedirs(os.path.join(save_path, sub), exist_ok=True) + scene_name = f"{args.town}_{args.weather}_{args.time}" + save_path = os.path.join(OUTPUT_FOLDER, scene_name) + for sub in ['ego_rgb', 'rsu_rgb', 'label']: + os.makedirs(os.path.join(save_path, sub), exist_ok=True) client = carla.Client('localhost', 2000) client.set_timeout(20.0) - world = client.load_world(args.town) + world = client.get_world() + if args.town not in world.get_map().name: + world = client.load_world(args.town) - # 核心:设置天气和同步模式 + # 【恢复】环境配置 configure_weather(world, args.weather, args.time) + settings = world.get_settings() settings.synchronous_mode = True settings.fixed_delta_seconds = FIXED_DELTA_SECONDS world.apply_settings(settings) - + tm = client.get_trafficmanager(8000) tm.set_synchronous_mode(True) @@ -133,35 +137,39 @@ def main(): try: bp_lib = world.get_blueprint_library() spawn_points = world.get_map().get_spawn_points() - - # 1. 主车 - ego_vehicle = world.spawn_actor(bp_lib.find('vehicle.tesla.model3'), spawn_points[0]) + random.shuffle(spawn_points) + + # 1. 主车 (Ego) + ego_bp = bp_lib.find('vehicle.tesla.model3') + ego_vehicle = world.spawn_actor(ego_bp, spawn_points[0]) ego_vehicle.set_autopilot(True, tm.get_port()) actor_list.append(ego_vehicle) - # 2. 背景车辆 - for i in range(1, min(len(spawn_points), args.num_vehicles)): + # 2. 生成背景车 (包含自行车/摩托车) + print(f"🚗 正在生成 {args.num_vehicles} 辆背景车...") + batch = [] + for i, transform in enumerate(spawn_points[1:]): + if i >= args.num_vehicles: break bp = random.choice(bp_lib.filter('vehicle.*')) - npc = world.try_spawn_actor(bp, spawn_points[i]) - if npc: - npc.set_autopilot(True, tm.get_port()) - actor_list.append(npc) + batch.append(carla.command.SpawnActor(bp, transform).then( + carla.command.SetAutopilot(carla.command.FutureActor, True, tm.get_port()))) + + for r in client.apply_batch_sync(batch): + if not r.error: actor_list.append(world.get_actor(r.actor_id)) - # 3. 生成行人 + # 3. 【恢复】生成行人 print(f"🚶 正在生成 {args.num_walkers} 个行人...") - walker_bp_lib = bp_lib.filter('walker.pedestrian.*') for _ in range(args.num_walkers): loc = world.get_random_location_from_navigation() if loc: - walker_bp = random.choice(walker_bp_lib) - w = world.try_spawn_actor(walker_bp, carla.Transform(loc)) + w = world.try_spawn_actor(random.choice(bp_lib.filter('walker.pedestrian.*')), carla.Transform(loc)) if w: actor_list.append(w) - # 4. 传感器 + # 4. 相机设置 cam_bp = bp_lib.find('sensor.camera.rgb') cam_bp.set_attribute('image_size_x', str(IMAGE_W)); cam_bp.set_attribute('image_size_y', str(IMAGE_H)) cam_bp.set_attribute('fov', str(FOV)) - ego_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(z=2.0, x=1.2)), attach_to=ego_vehicle) + ego_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(z=2.0, x=1.0)), attach_to=ego_vehicle) rsu_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(x=20, y=10, z=15), carla.Rotation(pitch=-60))) actor_list.extend([ego_cam, rsu_cam]) @@ -172,15 +180,15 @@ def main(): save_queue = queue.Queue() threading.Thread(target=save_worker, args=(save_queue,), daemon=True).start() - spectator = world.get_spectator() K = utils.build_projection_matrix(IMAGE_W, IMAGE_H, FOV) + spectator = world.get_spectator() - print("\n🚀 运行中![Q]键退出 | 天气:", args.weather, "| 时间:", args.time) + print("\n🚀 运行中... 按 Q 退出预览") for frame_idx in range(args.max_frames): world.tick() - # 自动跟随视角 + # 【保留】自动视角跟随 ego_t = ego_vehicle.get_transform() spectator.set_transform(carla.Transform(ego_t.location + carla.Location(x=-12, z=6), carla.Rotation(pitch=-25, yaw=ego_t.rotation.yaw))) @@ -193,37 +201,48 @@ def main(): ego_w2c = utils.build_world_to_camera_matrix(ego_cam.get_transform()) rsu_w2c = utils.build_world_to_camera_matrix(rsu_cam.get_transform()) - # 寻找范围内物体 - all_actors = list(world.get_actors().filter('vehicle.*')) + list(world.get_actors().filter('walker.pedestrian.*')) + # 标注可见目标 + all_actors = world.get_actors().filter('vehicle.*') + all_walkers = world.get_actors().filter('walker.pedestrian.*') + visible_targets = [] - for actor in all_actors: + for actor in list(all_actors) + list(all_walkers): if actor.id == ego_vehicle.id: continue if actor.get_transform().location.distance(ego_t.location) > 80: continue - # 遮挡检测逻辑 - if is_visible(world, ego_cam, actor): + # 遮挡检测判定 + if is_visible_refined(world, ego_cam, actor): t, bb = actor.get_transform(), actor.bounding_box + + # 识别类型 + atype = 'vehicle' + if 'walker' in actor.type_id: atype = 'walker' + elif any(x in actor.type_id for x in ['bike', 'bicycle', 'motorcycle']): atype = 'bike' + visible_targets.append({ - "id": actor.id, "type": "vehicle" if actor.type_id.startswith('vehicle') else "walker", + "id": actor.id, "type": atype, "location": [t.location.x, t.location.y, t.location.z], "rotation": [t.rotation.pitch, t.rotation.yaw, t.rotation.roll], "extent": [bb.extent.x, bb.extent.y, bb.extent.z], "center_offset": [bb.location.x, bb.location.y, bb.location.z] }) - # 实时预览 + # 预览 array = np.frombuffer(data['ego'].raw_data, dtype=np.dtype("uint8")) array = np.reshape(array, (IMAGE_H, IMAGE_W, 4)) frame = array[:, :, :3].copy() - for t in visible_targets: frame = draw_box_simple(frame, t, ego_w2c, K) - cv2.imshow("Preview", frame) + for t in visible_targets: frame = draw_box(frame, t, ego_w2c, K) + cv2.imshow("Final Stable Preview", frame) if cv2.waitKey(1) & 0xFF == ord('q'): break # 保存 if frame_idx % SAVE_INTERVAL == 0: - json_data = {"frame_id": frame_idx, "camera_params": {"fov": FOV, "w": IMAGE_W, "h": IMAGE_H}, - "matrices": {"ego_w2c": ego_w2c.tolist(), "rsu_w2c": rsu_w2c.tolist()}, - "targets": visible_targets} + json_data = { + "frame_id": frame_idx, + "camera_params": {"fov": FOV, "w": IMAGE_W, "h": IMAGE_H}, + "matrices": {"ego_w2c": ego_w2c.tolist(), "rsu_w2c": rsu_w2c.tolist()}, + "targets": visible_targets + } save_queue.put({"path": save_path, "fid": frame_idx, "ego_img": data['ego'], "rsu_img": data['rsu'], "json_data": json_data}) finally: @@ -233,7 +252,15 @@ def main(): settings = world.get_settings(); settings.synchronous_mode = False; world.apply_settings(settings) for actor in actor_list: if actor: actor.destroy() - print("\n清理完毕。") + +def save_worker(q): + while writing_thread_running or not q.empty(): + try: + task = q.get(timeout=0.1) + task['ego_img'].save_to_disk(f"{task['path']}/ego_rgb/{task['fid']:08d}.jpg") + task['rsu_img'].save_to_disk(f"{task['path']}/rsu_rgb/{task['fid']:08d}.jpg") + with open(f"{task['path']}/label/{task['fid']:08d}.json", 'w') as f: json.dump(task['json_data'], f, indent=2) + except: continue if __name__ == '__main__': main() \ No newline at end of file