diff --git a/src/enhance_pedestrian_safety/config_manager.py b/src/enhance_pedestrian_safety/config_manager.py index 38622ee18a..9e80f69f2d 100644 --- a/src/enhance_pedestrian_safety/config_manager.py +++ b/src/enhance_pedestrian_safety/config_manager.py @@ -36,6 +36,26 @@ def load_config(config_file=None): 'rotation_frequency': 10 } }, + + 'v2x': { + 'enabled': True, + 'communication_range': 300.0, + 'bandwidth': 10.0, + 'latency_mean': 0.05, + 'latency_std': 0.01, + 'packet_loss_rate': 0.01, + 'message_types': ['bsm', 'spat', 'map', 'rsm'] + }, + 'cooperative': { + 'num_coop_vehicles': 2, + 'enable_shared_perception': True, + 'enable_traffic_warnings': True, + 'enable_maneuver_coordination': False, + 'data_fusion_interval': 1.0, + 'max_shared_objects': 50 + }, + + 'output': { 'data_dir': 'cvips_dataset', 'save_raw': True, @@ -43,6 +63,11 @@ def load_config(config_file=None): 'save_annotations': False, 'save_lidar': True, 'save_fusion': True, + + 'save_cooperative': True, + 'save_v2x_messages': True, + + 'validate_data': True, 'run_analysis': False } @@ -86,9 +111,21 @@ def merge_args(config, args): if args.num_pedestrians: config['traffic']['pedestrians'] = args.num_pedestrians + + if args.num_coop_vehicles: + config['cooperative']['num_coop_vehicles'] = args.num_coop_vehicles + if args.capture_interval: config['sensors']['capture_interval'] = args.capture_interval + if args.enable_v2x: + config['v2x']['enabled'] = True + + + if args.capture_interval: + config['sensors']['capture_interval'] = args.capture_interval + + if args.enable_lidar: config['sensors']['lidar_sensors'] = 1 config['output']['save_lidar'] = True @@ -96,6 +133,12 @@ def merge_args(config, args): if args.enable_fusion: config['output']['save_fusion'] = True + + if args.enable_cooperative: + config['output']['save_cooperative'] = True + + + if args.enable_annotations: config['output']['save_annotations'] = True diff --git a/src/enhance_pedestrian_safety/data_analyzer.py b/src/enhance_pedestrian_safety/data_analyzer.py index a5802c2dc4..f805fe2fcb 100644 --- a/src/enhance_pedestrian_safety/data_analyzer.py +++ b/src/enhance_pedestrian_safety/data_analyzer.py @@ -17,6 +17,10 @@ def analyze_dataset(data_dir): 'file_distribution': DataAnalyzer._analyze_file_distribution(data_dir), 'object_statistics': DataAnalyzer._analyze_objects(data_dir), 'temporal_analysis': DataAnalyzer._analyze_temporal(data_dir), + + 'cooperative_data': DataAnalyzer._analyze_cooperative_data(data_dir), + + 'quality_metrics': DataAnalyzer._calculate_quality_metrics(data_dir) } @@ -77,6 +81,23 @@ def _analyze_file_distribution(data_dir): distribution = {} # 分析原始图像 + + raw_dirs = [] + raw_path = os.path.join(data_dir, "raw") + if os.path.exists(raw_path): + raw_dirs = [d for d in os.listdir(raw_path) if os.path.isdir(os.path.join(raw_path, d))] + + for raw_dir in raw_dirs: + path = os.path.join(data_dir, "raw", raw_dir) + camera_stats = {} + for camera_dir in os.listdir(path): + camera_path = os.path.join(path, camera_dir) + if os.path.isdir(camera_path): + images = [f for f in os.listdir(camera_path) + if f.endswith(('.png', '.jpg', '.jpeg'))] + camera_stats[camera_dir] = len(images) + distribution[f'raw_{raw_dir}'] = camera_stats + raw_dirs = ['vehicle', 'infrastructure'] for raw_dir in raw_dirs: path = os.path.join(data_dir, "raw", raw_dir) @@ -90,6 +111,7 @@ def _analyze_file_distribution(data_dir): camera_stats[camera_dir] = len(images) distribution[f'raw_{raw_dir}'] = camera_stats + # 分析拼接图像 stitched_dir = os.path.join(data_dir, "stitched") if os.path.exists(stitched_dir): @@ -103,6 +125,22 @@ def _analyze_file_distribution(data_dir): json_files = [f for f in os.listdir(annotations_dir) if f.endswith('.json')] distribution['annotations'] = len(json_files) + + # 分析LiDAR数据 + lidar_dir = os.path.join(data_dir, "lidar") + if os.path.exists(lidar_dir): + bin_files = [f for f in os.listdir(lidar_dir) if f.endswith('.bin')] + npy_files = [f for f in os.listdir(lidar_dir) if f.endswith('.npy')] + distribution['lidar'] = {'bin': len(bin_files), 'npy': len(npy_files)} + + # 分析融合数据 + fusion_dir = os.path.join(data_dir, "fusion") + if os.path.exists(fusion_dir): + json_files = [f for f in os.listdir(fusion_dir) if f.endswith('.json')] + distribution['fusion'] = len(json_files) + + + return distribution @staticmethod @@ -111,8 +149,12 @@ def _analyze_objects(data_dir): annotations_dir = os.path.join(data_dir, "annotations") if not os.path.exists(annotations_dir): + + return {'total_objects': 0, 'by_class': {}, 'by_frame': {}, 'class_distribution': {}} + return {'total_objects': 0, 'by_class': {}, 'by_frame': {}} + object_stats = { 'total_objects': 0, 'by_class': defaultdict(int), @@ -174,26 +216,118 @@ def _analyze_temporal(data_dir): return temporal_stats @staticmethod + + def _analyze_cooperative_data(data_dir): + """分析协同数据""" + coop_dir = os.path.join(data_dir, "cooperative") + + if not os.path.exists(coop_dir): + return { + 'v2x_messages': 0, + 'shared_perception': 0, + 'vehicles_count': 0, + 'communication_stats': {} + } + + # V2X消息统计 + v2x_dir = os.path.join(coop_dir, "v2x_messages") + v2x_files = [] + if os.path.exists(v2x_dir): + v2x_files = [f for f in os.listdir(v2x_dir) if f.endswith('.json')] + + # 共享感知统计 + perception_dir = os.path.join(coop_dir, "shared_perception") + perception_files = [] + if os.path.exists(perception_dir): + perception_files = [f for f in os.listdir(perception_dir) if f.endswith('.json')] + + # 读取协同摘要 + coop_summary = {} + summary_file = os.path.join(coop_dir, "cooperative_summary.json") + if os.path.exists(summary_file): + try: + with open(summary_file, 'r') as f: + coop_summary = json.load(f) + except: + pass + + # 分析V2X消息内容 + v2x_stats = { + 'total_messages': len(v2x_files), + 'message_types': defaultdict(int), + 'average_message_size': 0 + } + + if v2x_files: + total_size = 0 + for v2x_file in v2x_files[:min(10, len(v2x_files))]: # 抽样分析 + try: + with open(os.path.join(v2x_dir, v2x_file), 'r') as f: + data = json.load(f) + message_type = data.get('message', {}).get('message_type', 'unknown') + v2x_stats['message_types'][message_type] += 1 + + file_size = os.path.getsize(os.path.join(v2x_dir, v2x_file)) + total_size += file_size + except: + pass + + v2x_stats['average_message_size'] = round(total_size / max(len(v2x_files), 1), 2) + + analysis = { + 'v2x_messages': len(v2x_files), + 'shared_perception_frames': len(perception_files), + 'total_vehicles': coop_summary.get('total_vehicles', 0), + 'ego_vehicles': coop_summary.get('ego_vehicles', 0), + 'cooperative_vehicles': coop_summary.get('cooperative_vehicles', 0), + 'v2x_stats': v2x_stats, + 'shared_objects_count': coop_summary.get('shared_objects_count', 0), + 'communication_range': coop_summary.get('communication_range', 0), + 'collaborative_detections': coop_summary.get('v2x_stats', {}).get('collaborative_detections', 0) + } + + return analysis + + @staticmethod + + def _calculate_quality_metrics(data_dir): """计算质量指标""" quality_metrics = { 'completeness_score': 0, 'consistency_score': 0, 'diversity_score': 0, + + 'cooperative_score': 0, + + 'issues_found': [] } # 检查完整性 + + required_dirs = ["raw/vehicle", "raw/infrastructure", "stitched", "metadata", "cooperative"] + missing_dirs = [] + + for dir_path in required_dirs: + full_path = os.path.join(data_dir, dir_path) + if not os.path.exists(full_path): + required_dirs = ["raw/vehicle", "raw/infrastructure", "stitched", "metadata"] missing_dirs = [] for dir_path in required_dirs: if not os.path.exists(os.path.join(data_dir, dir_path)): + missing_dirs.append(dir_path) if missing_dirs: quality_metrics['issues_found'].append(f"缺失目录: {missing_dirs}") + + quality_metrics['completeness_score'] = 100 - (len(missing_dirs) * 20) + quality_metrics['completeness_score'] = 50 + else: quality_metrics['completeness_score'] = 100 @@ -209,7 +343,11 @@ def _calculate_quality_metrics(data_dir): camera_counts.append(len(images)) if camera_counts: + + max_diff = max(camera_counts) - min(camera_counts) if camera_counts else 0 + max_diff = max(camera_counts) - min(camera_counts) + if max_diff > 5: quality_metrics['issues_found'].append(f"摄像头图像数量不一致: 差异{max_diff}") quality_metrics['consistency_score'] = 70 @@ -228,16 +366,37 @@ def _calculate_quality_metrics(data_dir): quality_metrics['diversity_score'] = 50 quality_metrics['issues_found'].append(f"物体类别较少: {num_classes}类") + + # 协同评分 + cooperative_data = DataAnalyzer._analyze_cooperative_data(data_dir) + if cooperative_data['v2x_messages'] > 10 and cooperative_data['shared_perception_frames'] > 5: + quality_metrics['cooperative_score'] = 90 + elif cooperative_data['v2x_messages'] > 0 or cooperative_data['shared_perception_frames'] > 0: + quality_metrics['cooperative_score'] = 60 + else: + quality_metrics['cooperative_score'] = 30 + quality_metrics['issues_found'].append("协同数据较少") + + + return quality_metrics @staticmethod def _calculate_overall_score(analysis): """计算总体评分""" weights = { + + 'completeness': 0.25, + 'consistency': 0.20, + 'diversity': 0.15, + 'cooperative': 0.20, + 'temporal': 0.20 + 'completeness': 0.3, 'consistency': 0.25, 'diversity': 0.25, 'temporal': 0.2 + } quality = analysis['quality_metrics'] @@ -245,7 +404,12 @@ def _calculate_overall_score(analysis): score = ( quality['completeness_score'] * weights['completeness'] + quality['consistency_score'] * weights['consistency'] + + + quality['diversity_score'] * weights['diversity'] + + quality['cooperative_score'] * weights['cooperative'] + quality['diversity_score'] * weights['diversity'] + ) # 时间因素 @@ -257,8 +421,12 @@ def _calculate_overall_score(analysis): else: score += 5 * weights['temporal'] + + return round(min(score, 100), 1) + return round(score, 1) + @staticmethod def _save_analysis_report(data_dir, analysis): """保存分析报告""" @@ -292,7 +460,15 @@ def _print_analysis_summary(analysis): if isinstance(value, dict): print(f" {key}:") for subkey, subvalue in value.items(): + + if isinstance(subvalue, dict): + for subsubkey, subsubvalue in subvalue.items(): + print(f" {subsubkey}: {subsubvalue}") + else: + print(f" {subkey}: {subvalue}") + print(f" {subkey}: {subvalue}") + else: print(f" {key}: {value}") @@ -305,6 +481,25 @@ def _print_analysis_summary(analysis): percentage = objects['class_distribution'].get(obj_class, 0) print(f" {obj_class}: {count} ({percentage}%)") + + # 协同数据分析 + cooperative = analysis['cooperative_data'] + print(f"\n协同数据分析:") + print(f" V2X消息: {cooperative['v2x_messages']}") + print(f" 共享感知帧: {cooperative['shared_perception_frames']}") + print(f" 车辆总数: {cooperative['total_vehicles']}") + print(f" 主车: {cooperative['ego_vehicles']}") + print(f" 协同车: {cooperative['cooperative_vehicles']}") + print(f" 共享对象数: {cooperative['shared_objects_count']}") + print(f" 协作检测数: {cooperative['collaborative_detections']}") + + if cooperative['v2x_stats']['message_types']: + print(f" V2X消息类型:") + for msg_type, count in cooperative['v2x_stats']['message_types'].items(): + print(f" {msg_type}: {count}") + + + temporal = analysis['temporal_analysis'] print(f"\n时间分析:") print(f" 总时长: {temporal['total_duration']:.1f}秒") @@ -316,6 +511,10 @@ def _print_analysis_summary(analysis): print(f" 一致性: {quality['consistency_score']}/100") print(f" 多样性: {quality['diversity_score']}/100") + print(f" 协同性: {quality['cooperative_score']}/100") + + + if quality['issues_found']: print(f" 发现的问题 ({len(quality['issues_found'])}):") for issue in quality['issues_found'][:3]: @@ -325,11 +524,20 @@ def _print_analysis_summary(analysis): print(f"\n总体评分: {analysis['overall_score']}/100") + + overall_score = analysis['overall_score'] + if overall_score >= 90: + print("✓ 数据集质量优秀") + elif overall_score >= 75: + print("✓ 数据集质量良好") + elif overall_score >= 60: + if analysis['overall_score'] >= 90: print("✓ 数据集质量优秀") elif analysis['overall_score'] >= 75: print("✓ 数据集质量良好") elif analysis['overall_score'] >= 60: + print("⚠ 数据集质量一般") else: print("✗ 数据集质量需要改进") diff --git a/src/enhance_pedestrian_safety/data_validator.py b/src/enhance_pedestrian_safety/data_validator.py index f015231a42..80c65d52f9 100644 --- a/src/enhance_pedestrian_safety/data_validator.py +++ b/src/enhance_pedestrian_safety/data_validator.py @@ -14,7 +14,13 @@ def validate_dataset(data_dir): 'stitched_images': DataValidator._validate_stitched_images(data_dir), 'annotations': DataValidator._validate_annotations(data_dir), 'metadata': DataValidator._validate_metadata(data_dir), + + 'lidar_data': DataValidator._validate_lidar_data(data_dir), + 'cooperative_data': DataValidator._validate_cooperative_data(data_dir), + 'fusion_data': DataValidator._validate_fusion_data(data_dir) + 'lidar_data': DataValidator._validate_lidar_data(data_dir) + } validation_results['overall_score'] = DataValidator._calculate_score(validation_results) @@ -30,7 +36,15 @@ def _check_directory_structure(data_dir): "raw/vehicle", "raw/infrastructure", "stitched", + + "metadata", + "cooperative", + "cooperative/v2x_messages", + "cooperative/shared_perception", + "fusion" + "metadata" + ] missing_dirs = [] @@ -51,15 +65,28 @@ def _check_directory_structure(data_dir): @staticmethod def _validate_raw_images(data_dir): + + raw_path = os.path.join(data_dir, "raw") + + if not os.path.exists(raw_path): + return {'vehicle': {'status': 'MISSING', 'count': 0, 'errors': []}, + 'infrastructure': {'status': 'MISSING', 'count': 0, 'errors': []}} + + raw_dirs = [d for d in os.listdir(raw_path) if os.path.isdir(os.path.join(raw_path, d))] + raw_dirs = ["vehicle", "infrastructure"] + results = {} for raw_dir in raw_dirs: path = os.path.join(data_dir, "raw", raw_dir) + + if not os.path.exists(path): results[raw_dir] = {'status': 'MISSING', 'count': 0, 'errors': []} continue + camera_dirs = [] if os.path.exists(path): camera_dirs = [d for d in os.listdir(path) if os.path.isdir(os.path.join(path, d))] @@ -143,6 +170,16 @@ def _validate_annotations(data_dir): try: with open(json_path, 'r') as f: data = json.load(f) + + + # 检查基本结构 + required_keys = ['frame_id', 'objects'] + for key in required_keys: + if key not in data: + errors.append(f"缺失必要键: {key} in {json_file}") + + + valid_files += 1 except Exception as e: errors.append(f"无效的JSON文件: {json_file} - {str(e)}") @@ -201,19 +238,164 @@ def _validate_lidar_data(data_dir): return {'status': 'MISSING', 'count': 0, 'errors': []} bin_files = [f for f in os.listdir(lidar_dir) if f.endswith('.bin')] + + npy_files = [f for f in os.listdir(lidar_dir) if f.endswith('.npy')] + json_files = [f for f in os.listdir(lidar_dir) if f.endswith('.json')] + + errors = [] + valid_bin_files = 0 + errors = [] valid_files = 0 + for bin_file in bin_files: bin_path = os.path.join(lidar_dir, bin_file) try: if os.path.getsize(bin_path) > 0: + + valid_bin_files += 1 + valid_files += 1 + else: errors.append(f"空文件: {bin_file}") except: errors.append(f"文件访问失败: {bin_file}") + + total_files = len(bin_files) + len(npy_files) + len(json_files) + + if len(errors) == 0 and valid_bin_files > 0: + status = 'PASS' + elif len(errors) < 3 and valid_bin_files > 0: + status = 'WARNING' + else: + status = 'FAIL' + + return { + 'status': status, + 'count': total_files, + 'bin_files': len(bin_files), + 'npy_files': len(npy_files), + 'json_files': len(json_files), + 'errors': errors + } + + @staticmethod + def _validate_cooperative_data(data_dir): + """验证协同数据""" + coop_dir = os.path.join(data_dir, "cooperative") + + if not os.path.exists(coop_dir): + return {'status': 'MISSING', 'count': 0, 'errors': [], 'v2x_messages': 0, 'shared_perception': 0} + + errors = [] + + # 检查V2X消息目录 + v2x_dir = os.path.join(coop_dir, "v2x_messages") + v2x_files = [] + if os.path.exists(v2x_dir): + v2x_files = [f for f in os.listdir(v2x_dir) if f.endswith('.json')] + for v2x_file in v2x_files[:min(5, len(v2x_files))]: # 检查前5个文件 + try: + with open(os.path.join(v2x_dir, v2x_file), 'r') as f: + data = json.load(f) + + # 检查必要字段 + required_keys = ['message', 'recipients', 'transmission_time'] + for key in required_keys: + if key not in data: + errors.append(f"V2X消息缺失字段 {key}: {v2x_file}") + except Exception as e: + errors.append(f"V2X消息文件无效: {v2x_file} - {str(e)}") + else: + errors.append("V2X消息目录不存在") + + # 检查共享感知目录 + perception_dir = os.path.join(coop_dir, "shared_perception") + perception_files = [] + if os.path.exists(perception_dir): + perception_files = [f for f in os.listdir(perception_dir) if f.endswith('.json')] + for perception_file in perception_files[:min(5, len(perception_files))]: + try: + with open(os.path.join(perception_dir, perception_file), 'r') as f: + data = json.load(f) + + # 检查必要字段 + required_keys = ['frame_id', 'timestamp', 'shared_objects'] + for key in required_keys: + if key not in data: + errors.append(f"共享感知文件缺失字段 {key}: {perception_file}") + except Exception as e: + errors.append(f"共享感知文件无效: {perception_file} - {str(e)}") + else: + errors.append("共享感知目录不存在") + + # 检查协同摘要 + summary_file = os.path.join(coop_dir, "cooperative_summary.json") + if not os.path.exists(summary_file): + errors.append("协同摘要文件不存在") + else: + try: + with open(summary_file, 'r') as f: + data = json.load(f) + + # 检查摘要字段 + required_keys = ['total_vehicles', 'ego_vehicles', 'cooperative_vehicles', 'v2x_stats'] + for key in required_keys: + if key not in data: + errors.append(f"协同摘要缺失字段: {key}") + except Exception as e: + errors.append(f"协同摘要文件无效: {str(e)}") + + total_files = len(v2x_files) + len(perception_files) + + if len(errors) == 0 and total_files > 0: + status = 'PASS' + elif len(errors) < 3 and total_files > 0: + status = 'WARNING' + else: + status = 'FAIL' + + return { + 'status': status, + 'count': total_files, + 'v2x_messages': len(v2x_files), + 'shared_perception': len(perception_files), + 'errors': errors + } + + @staticmethod + def _validate_fusion_data(data_dir): + """验证融合数据""" + fusion_dir = os.path.join(data_dir, "fusion") + + if not os.path.exists(fusion_dir): + return {'status': 'MISSING', 'count': 0, 'errors': []} + + json_files = [f for f in os.listdir(fusion_dir) if f.endswith('.json')] + errors = [] + valid_files = 0 + + for json_file in json_files[:min(5, len(json_files))]: # 检查前5个文件 + json_path = os.path.join(fusion_dir, json_file) + try: + with open(json_path, 'r') as f: + data = json.load(f) + + # 检查必要字段 + required_keys = ['frame_id', 'timestamp', 'sensors'] + for key in required_keys: + if key not in data: + errors.append(f"融合文件缺失字段 {key}: {json_file}") + + valid_files += 1 + except Exception as e: + errors.append(f"融合文件无效: {json_file} - {str(e)}") + + + if len(errors) == 0 and valid_files > 0: status = 'PASS' elif len(errors) < 3 and valid_files > 0: @@ -223,19 +405,34 @@ def _validate_lidar_data(data_dir): return { 'status': status, + + 'count': len(json_files), + 'count': valid_files, + 'errors': errors } @staticmethod def _calculate_score(results): weights = { + + 'directory_structure': 0.10, + 'raw_images': 0.20, + 'stitched_images': 0.10, + 'annotations': 0.10, + 'metadata': 0.10, + 'lidar_data': 0.15, + 'cooperative_data': 0.15, + 'fusion_data': 0.10 + 'directory_structure': 0.15, 'raw_images': 0.25, 'stitched_images': 0.15, 'annotations': 0.1, 'metadata': 0.15, 'lidar_data': 0.2 + } score = 0 @@ -280,13 +477,41 @@ def _print_validation_report(results): print(f"\n{key.replace('_', ' ').title()}:") if 'status' in result: + + status_icon = '✓' if result['status'] == 'PASS' else '⚠' if result['status'] == 'WARNING' else '✗' + print(f" 状态: {status_icon} {result['status']}") + print(f" 状态: {result['status']}") + else: print(f" 状态: 未知") if 'count' in result: print(f" 数量: {result['count']}") + + # 特定类型数据的额外信息 + if key == 'raw_images' and isinstance(result, dict): + for subkey, subresult in result.items(): + if isinstance(subresult, dict) and 'count' in subresult: + print(f" {subkey}: {subresult['count']} 图像") + + if key == 'cooperative_data' and isinstance(result, dict): + if 'v2x_messages' in result: + print(f" V2X消息: {result['v2x_messages']}") + if 'shared_perception' in result: + print(f" 共享感知: {result['shared_perception']}") + + if key == 'lidar_data' and isinstance(result, dict): + if 'bin_files' in result: + print(f" BIN文件: {result['bin_files']}") + if 'npy_files' in result: + print(f" NPY文件: {result['npy_files']}") + if 'json_files' in result: + print(f" JSON文件: {result['json_files']}") + + + if 'errors' in result and result['errors']: print(f" 错误 ({len(result['errors'])}):") for error in result['errors'][:3]: diff --git a/src/enhance_pedestrian_safety/main.py b/src/enhance_pedestrian_safety/main.py index 64f7caf9ea..bd8376d1a8 100644 --- a/src/enhance_pedestrian_safety/main.py +++ b/src/enhance_pedestrian_safety/main.py @@ -17,6 +17,11 @@ from data_analyzer import DataAnalyzer from lidar_processor import LidarProcessor, MultiSensorFusion +from multi_vehicle_manager import MultiVehicleManager +from v2x_communication import V2XCommunication + + + carla_egg_path, remaining_argv = setup_carla_path() carla = import_carla_module() @@ -254,14 +259,23 @@ def __init__(self, world, config, data_dir): if config['output'].get('save_fusion', False): self.fusion_manager = MultiSensorFusion(data_dir) + + def setup_cameras(self, vehicle, center_location, vehicle_id=0): + vehicle_cams = self._setup_vehicle_cameras(vehicle, vehicle_id) + def setup_cameras(self, vehicle, center_location): vehicle_cams = self._setup_vehicle_cameras(vehicle) + infra_cams = self._setup_infrastructure_cameras(center_location) Log.info(f"摄像头: {vehicle_cams}车辆 + {infra_cams}基础设施") return vehicle_cams + infra_cams + + def _setup_vehicle_cameras(self, vehicle, vehicle_id): + def _setup_vehicle_cameras(self, vehicle): + if not vehicle: return 0 @@ -274,7 +288,11 @@ def _setup_vehicle_cameras(self, vehicle): installed = 0 for cam_name, config_data in camera_configs.items(): + + if self._create_camera(cam_name, config_data, vehicle, 'vehicle', vehicle_id): + if self._create_camera(cam_name, config_data, vehicle, 'vehicle'): + installed += 1 return installed @@ -304,7 +322,11 @@ def _setup_infrastructure_cameras(self, center_location): return installed + + def setup_lidar(self, vehicle, vehicle_id=0): + def setup_lidar(self, vehicle): + if not vehicle or not self.config['sensors'].get('lidar_sensors', 0) > 0: return 0 @@ -319,7 +341,10 @@ def setup_lidar(self, vehicle): lidar_bp.set_attribute('points_per_second', str(lidar_config.get('points_per_second', 56000))) lidar_bp.set_attribute('rotation_frequency', str(lidar_config.get('rotation_frequency', 10))) + + # 添加更多LiDAR参数 + lidar_bp.set_attribute('upper_fov', '10') lidar_bp.set_attribute('lower_fov', '-20') lidar_bp.set_attribute('horizontal_fov', '360') @@ -337,11 +362,17 @@ def lidar_callback(lidar_data): try: metadata = self.lidar_processor.process_lidar_data(lidar_data, self.frame_counter) if metadata and self.fusion_manager: + + vehicle_image_path = None + with self.buffer_lock: + if self.vehicle_buffer: + # 尝试获取最新的车辆图像 vehicle_image_path = None with self.buffer_lock: if self.vehicle_buffer: # 取第一个摄像头的图像 + for cam_name, img_path in self.vehicle_buffer.items(): if os.path.exists(img_path): vehicle_image_path = img_path @@ -366,7 +397,12 @@ def lidar_callback(lidar_data): except Exception as e: print(f"LiDAR安装失败: {e}") return 0 + + + def _create_camera(self, name, config, parent, sensor_type, vehicle_id=0): + def _create_camera(self, name, config, parent, sensor_type): + try: blueprint = self.world.get_blueprint_library().find('sensor.camera.rgb') @@ -384,10 +420,21 @@ def _create_camera(self, name, config, parent, sensor_type): else: camera = self.world.spawn_actor(blueprint, transform) + + # 为不同车辆创建不同目录 + if sensor_type == 'vehicle' and vehicle_id > 0: + save_dir = os.path.join(self.data_dir, "raw", f"vehicle_{vehicle_id}", name) + else: + save_dir = os.path.join(self.data_dir, "raw", sensor_type, name) + + os.makedirs(save_dir, exist_ok=True) + + callback = self._create_callback(save_dir, name, sensor_type, vehicle_id) save_dir = os.path.join(self.data_dir, "raw", sensor_type, name) os.makedirs(save_dir, exist_ok=True) callback = self._create_callback(save_dir, name, sensor_type) + camera.listen(callback) self.sensors.append(camera) @@ -397,7 +444,11 @@ def _create_camera(self, name, config, parent, sensor_type): Log.warning(f"创建摄像头 {name} 失败: {e}") return False + + def _create_callback(self, save_dir, name, sensor_type, vehicle_id=0): + def _create_callback(self, save_dir, name, sensor_type): + capture_interval = self.config['sensors']['capture_interval'] def callback(image): @@ -414,7 +465,12 @@ def callback(image): if sensor_type == 'vehicle': self.vehicle_buffer[name] = filename if len(self.vehicle_buffer) >= 4: + + self.image_processor.stitch(self.vehicle_buffer, self.frame_counter, + f'vehicle_{vehicle_id}') + self.image_processor.stitch(self.vehicle_buffer, self.frame_counter, 'vehicle') + self.vehicle_buffer.clear() else: self.infra_buffer[name] = filename @@ -459,14 +515,24 @@ def __init__(self, config): self.config = config self.client = None self.world = None + + self.ego_vehicles = [] # 多个主车 + self.ego_vehicle = None + self.scene_center = None self.setup_directories() self.traffic_manager = None + + self.sensor_managers = {} # 车辆ID -> SensorManager + self.multi_vehicle_manager = None + self.v2x_communication = None + self.sensor_manager = None + self.start_time = None self.is_running = False self.collected_frames = 0 @@ -481,12 +547,21 @@ def setup_directories(self): ) directories = [ + + "raw/vehicle_1", # 主车1 + "raw/vehicle_2", # 主车2(如果有) + "raw/vehicle", + "raw/infrastructure", "stitched", "lidar", "fusion", "calibration", + + "cooperative", + "v2x_messages", + "metadata" ] @@ -531,17 +606,84 @@ def setup_scene(self): self.traffic_manager = TrafficManager(self.world, self.config) + + # 生成多个主车 + num_ego_vehicles = min(self.config['cooperative'].get('num_coop_vehicles', 2) + 1, 3) + for i in range(num_ego_vehicles): + ego_vehicle = self.traffic_manager.spawn_ego_vehicle() + if ego_vehicle: + self.ego_vehicles.append(ego_vehicle) + Log.info(f"主车 {i + 1} 生成: {ego_vehicle.type_id}") + + if not self.ego_vehicles: + self.ego_vehicle = self.traffic_manager.spawn_ego_vehicle() if not self.ego_vehicle: + Log.error("主车生成失败") return False self.traffic_manager.spawn_traffic(self.scene_center) + + # 初始化V2X通信 + if self.config['v2x']['enabled']: + self.v2x_communication = V2XCommunication(self.config['v2x']) + + # 注册车辆到V2X网络 + for i, vehicle in enumerate(self.ego_vehicles): + location = vehicle.get_location() + self.v2x_communication.register_node( + f'vehicle_{vehicle.id}', + (location.x, location.y, location.z), + {'type': 'vehicle', 'capabilities': ['bsm', 'rsm']} + ) + + # 初始化多车辆管理器 + self.multi_vehicle_manager = MultiVehicleManager( + self.world, + self.config, + self.output_dir + ) + + # 设置主车 + self.multi_vehicle_manager.ego_vehicles = self.ego_vehicles + + # 生成协同车辆 + num_coop_vehicles = self.config['cooperative'].get('num_coop_vehicles', 2) + coop_vehicles = self.multi_vehicle_manager.spawn_cooperative_vehicles(num_coop_vehicles) + + # 注册协同车辆到V2X网络 + if self.v2x_communication: + for vehicle in coop_vehicles: + location = vehicle.get_location() + self.v2x_communication.register_node( + f'vehicle_{vehicle.id}', + (location.x, location.y, location.z), + {'type': 'vehicle', 'capabilities': ['bsm', 'rsm']} + ) + + + time.sleep(3.0) return True def setup_sensors(self): + + # 为每个主车设置传感器 + for i, vehicle in enumerate(self.ego_vehicles): + sensor_manager = SensorManager(self.world, self.config, self.output_dir) + + cameras = sensor_manager.setup_cameras(vehicle, self.scene_center, i + 1) + if cameras == 0: + Log.error(f"车辆 {i + 1} 没有摄像头安装成功") + return False + + lidars = sensor_manager.setup_lidar(vehicle, i + 1) + Log.info(f"车辆 {i + 1} 传感器: {cameras}摄像头 + {lidars}LiDAR") + + self.sensor_managers[vehicle.id] = sensor_manager + self.sensor_manager = SensorManager(self.world, self.config, self.output_dir) cameras = self.sensor_manager.setup_cameras(self.ego_vehicle, self.scene_center) @@ -552,6 +694,7 @@ def setup_sensors(self): lidars = self.sensor_manager.setup_lidar(self.ego_vehicle) Log.info(f"传感器: {cameras}摄像头 + {lidars}LiDAR") + return True def collect_data(self): @@ -563,16 +706,47 @@ def collect_data(self): last_update = time.time() + last_v2x_update = time.time() + last_perception_share = time.time() + try: while time.time() - self.start_time < duration and self.is_running: current_time = time.time() elapsed = current_time - self.start_time + + # 更新车辆状态 + if self.multi_vehicle_manager: + self.multi_vehicle_manager.update_vehicle_states() + + # V2X通信更新 + if self.v2x_communication and current_time - last_v2x_update >= 0.1: + self._update_v2x_communication() + last_v2x_update = current_time + + # 共享感知数据 + if (self.config['cooperative'].get('enable_shared_perception', True) and + current_time - last_perception_share >= 2.0): + self._share_perception_data() + last_perception_share = current_time + + # 定期保存共享感知 + frame_count = sum(mgr.get_frame_count() for mgr in self.sensor_managers.values()) + if frame_count % 10 == 0 and self.multi_vehicle_manager: + self.multi_vehicle_manager.save_shared_perception(frame_count) + + if current_time - last_update >= 5.0: + total_frames = sum(mgr.get_frame_count() for mgr in self.sensor_managers.values()) + progress = (elapsed / duration) * 100 + + Log.info(f"进度: {elapsed:.0f}/{duration}秒 ({progress:.1f}%) | 总帧数: {total_frames}") + if current_time - last_update >= 5.0: frames = self.sensor_manager.get_frame_count() progress = (elapsed / duration) * 100 Log.info(f"进度: {elapsed:.0f}/{duration}秒 ({progress:.1f}%) | 帧数: {frames}") + last_update = current_time time.sleep(0.05) @@ -583,17 +757,108 @@ def collect_data(self): self.is_running = False elapsed = time.time() - self.start_time + + self.collected_frames = sum(mgr.get_frame_count() for mgr in self.sensor_managers.values()) + self.collected_frames = self.sensor_manager.get_frame_count() + Log.info(f"收集完成: {self.collected_frames}帧, 用时: {elapsed:.1f}秒") self._save_metadata() self._print_summary() + + def _update_v2x_communication(self): + """更新V2X通信""" + if not self.v2x_communication: + return + + # 为每辆车发送基本安全消息 + for vehicle in self.ego_vehicles + self.multi_vehicle_manager.cooperative_vehicles: + if not vehicle.is_alive: + continue + + try: + location = vehicle.get_location() + velocity = vehicle.get_velocity() + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) + + vehicle_data = { + 'position': (location.x, location.y, location.z), + 'speed': speed, + 'heading': vehicle.get_transform().rotation.yaw, + 'acceleration': (0, 0, 0) # 简化处理 + } + + self.v2x_communication.broadcast_basic_safety_message( + f'vehicle_{vehicle.id}', + vehicle_data + ) + except: + pass + + # 处理接收到的消息 + for vehicle in self.ego_vehicles: + messages = self.v2x_communication.get_messages_for_node(f'vehicle_{vehicle.id}') + if messages: + Log.debug(f"车辆 {vehicle.id} 收到 {len(messages)} 条V2X消息") + + def _share_perception_data(self): + """共享感知数据""" + if not self.multi_vehicle_manager or not self.config['cooperative']['enable_shared_perception']: + return + + # 模拟车辆感知数据(简化处理,实际应从传感器获取) + for vehicle in self.ego_vehicles + self.multi_vehicle_manager.cooperative_vehicles: + if not vehicle.is_alive: + continue + + # 模拟检测到的物体 + detected_objects = self._simulate_object_detection(vehicle) + + if detected_objects: + self.multi_vehicle_manager.share_perception_data(vehicle.id, detected_objects) + + def _simulate_object_detection(self, vehicle): + """模拟对象检测(简化)""" + detected_objects = [] + + # 获取车辆周围的其他车辆 + for other_vehicle in self.ego_vehicles + self.multi_vehicle_manager.cooperative_vehicles: + if other_vehicle.id == vehicle.id or not other_vehicle.is_alive: + continue + + try: + location = other_vehicle.get_location() + distance = vehicle.get_location().distance(location) + + # 模拟检测范围(50米) + if distance < 50.0: + obj_data = { + 'class': 'vehicle', + 'position': {'x': location.x, 'y': location.y, 'z': location.z}, + 'velocity': {'x': 0, 'y': 0, 'z': 0}, + 'confidence': max(0.7, 1.0 - distance / 50.0), + 'size': {'width': 2.0, 'length': 4.5, 'height': 1.5}, + 'id': other_vehicle.id + } + detected_objects.append(obj_data) + except: + pass + + return detected_objects + + def _save_metadata(self): metadata = { 'scenario': self.config['scenario'], 'traffic': self.config['traffic'], 'sensors': self.config['sensors'], + + 'v2x': self.config['v2x'], + 'cooperative': self.config['cooperative'], + + 'output': self.config['output'], 'collection': { 'duration': round(time.time() - self.start_time, 2), @@ -602,10 +867,26 @@ def _save_metadata(self): } } + + # 传感器摘要 + sensor_summaries = {} + for vehicle_id, sensor_manager in self.sensor_managers.items(): + sensor_summaries[vehicle_id] = sensor_manager.generate_sensor_summary() + metadata['sensor_summaries'] = sensor_summaries + + # V2X通信状态 + if self.v2x_communication: + metadata['v2x_status'] = self.v2x_communication.get_network_status() + + # 协同摘要 + if self.multi_vehicle_manager: + metadata['cooperative_summary'] = self.multi_vehicle_manager.generate_summary() + if self.sensor_manager: sensor_summary = self.sensor_manager.generate_sensor_summary() metadata['sensor_summary'] = sensor_summary + meta_path = os.path.join(self.output_dir, "metadata", "collection_info.json") with open(meta_path, 'w', encoding='utf-8') as f: json.dump(metadata, f, indent=2, ensure_ascii=False) @@ -640,12 +921,33 @@ def _print_summary(self): total_points += points_in_file print(f" 估计总点数: {total_points:,}") + + # 统计协同数据 + coop_dir = os.path.join(self.output_dir, "cooperative") + if os.path.exists(coop_dir): + v2x_files = len([f for f in os.listdir(os.path.join(coop_dir, "v2x_messages")) if f.endswith('.json')]) + perception_files = len( + [f for f in os.listdir(os.path.join(coop_dir, "shared_perception")) if f.endswith('.json')]) + print(f"协同数据: {v2x_files} V2X消息, {perception_files} 共享感知文件") + + # V2X统计 + if self.v2x_communication: + v2x_status = self.v2x_communication.get_network_status() + print(f"V2X通信: {v2x_status['stats']['messages_sent']} 发送, " + f"{v2x_status['stats']['messages_received']} 接收, " + f"{v2x_status['stats']['messages_dropped']} 丢包") + + # 车辆统计 + print(f"车辆总数: {len(self.ego_vehicles)} 主车 + " + f"{len(self.multi_vehicle_manager.cooperative_vehicles)} 协同车") + # 统计融合数据 fusion_dir = os.path.join(self.output_dir, "fusion") if os.path.exists(fusion_dir): sync_files = [f for f in os.listdir(fusion_dir) if f.endswith('.json')] print(f"融合数据: {len(sync_files)} 个同步文件") + print(f"\n输出目录: {self.output_dir}") print("=" * 60) @@ -662,6 +964,32 @@ def run_analysis(self): def cleanup(self): Log.info("清理场景...") + + # 清理传感器 + for sensor_manager in self.sensor_managers.values(): + sensor_manager.cleanup() + + # 清理交通 + if self.traffic_manager: + self.traffic_manager.cleanup() + + # 清理协同管理 + if self.multi_vehicle_manager: + self.multi_vehicle_manager.cleanup() + + # 清理V2X通信 + if self.v2x_communication: + self.v2x_communication.stop() + + # 清理车辆 + for vehicle in self.ego_vehicles: + if vehicle and vehicle.is_alive: + try: + vehicle.destroy() + except: + pass + + if self.sensor_manager: self.sensor_manager.cleanup() @@ -674,15 +1002,24 @@ def cleanup(self): except: pass + Log.info("清理完成") def main(): + + parser = argparse.ArgumentParser(description='CVIPS 多车辆协同数据收集系统 v10.0') + + # 基础参数 + parser.add_argument('--config', type=str, help='配置文件路径') + parser.add_argument('--scenario', type=str, default='multi_vehicle_cooperative', help='场景名称') + parser = argparse.ArgumentParser(description='CVIPS 多传感器数据收集系统') # 基础参数 parser.add_argument('--config', type=str, help='配置文件路径') parser.add_argument('--scenario', type=str, default='multi_sensor_scene', help='场景名称') + parser.add_argument('--town', type=str, default='Town10HD', choices=['Town03', 'Town04', 'Town05', 'Town10HD'], help='地图') parser.add_argument('--weather', type=str, default='clear', @@ -694,6 +1031,10 @@ def main(): parser.add_argument('--num-vehicles', type=int, default=8, help='背景车辆数') parser.add_argument('--num-pedestrians', type=int, default=6, help='行人数') + parser.add_argument('--num-coop-vehicles', type=int, default=2, help='协同车辆数') + + + # 收集参数 parser.add_argument('--duration', type=int, default=60, help='收集时长(秒)') parser.add_argument('--capture-interval', type=float, default=2.0, help='捕捉间隔(秒)') @@ -702,6 +1043,11 @@ def main(): # 传感器参数 parser.add_argument('--enable-lidar', action='store_true', help='启用LiDAR传感器') parser.add_argument('--enable-fusion', action='store_true', help='启用多传感器融合') + + parser.add_argument('--enable-v2x', action='store_true', help='启用V2X通信') + parser.add_argument('--enable-cooperative', action='store_true', help='启用协同感知') + + parser.add_argument('--enable-annotations', action='store_true', help='启用自动标注') # 功能参数 @@ -716,22 +1062,36 @@ def main(): # 显示配置 print("\n" + "=" * 60) + + print("CVIPS 多车辆协同数据收集系统 v10.0") + print("CVIPS 多传感器数据收集系统 v9.0") + print("=" * 60) print(f"场景: {config['scenario']['name']}") print(f"地图: {config['scenario']['town']}") print(f"天气/时间: {config['scenario']['weather']}/{config['scenario']['time_of_day']}") print(f"时长: {config['scenario']['duration']}秒") + + print(f"交通: {config['traffic']['background_vehicles']}背景车辆 + {config['traffic']['pedestrians']}行人") + print(f"协同: {config['cooperative']['num_coop_vehicles']} 协同车辆") + print(f"交通: {config['traffic']['background_vehicles']}车辆 + {config['traffic']['pedestrians']}行人") + print(f"传感器:") print( f" 摄像头: {config['sensors']['vehicle_cameras']}车辆 + {config['sensors']['infrastructure_cameras']}基础设施") print(f" LiDAR: {'启用' if config['sensors']['lidar_sensors'] > 0 else '禁用'}") print(f" 融合: {'启用' if config['output']['save_fusion'] else '禁用'}") + + print(f" V2X: {'启用' if config['v2x']['enabled'] else '禁用'}") + print(f" 协同: {'启用' if config['output']['save_cooperative'] else '禁用'}") + print(f" 标注: {'启用' if config['output']['save_annotations'] else '禁用'}") + collector = DataCollector(config) try: diff --git a/src/enhance_pedestrian_safety/multi_vehicle_manager.py b/src/enhance_pedestrian_safety/multi_vehicle_manager.py new file mode 100644 index 0000000000..0f4492829c --- /dev/null +++ b/src/enhance_pedestrian_safety/multi_vehicle_manager.py @@ -0,0 +1,434 @@ +import random +import math +import carla +import time +import json +import os +from collections import defaultdict +from typing import List, Dict, Tuple, Optional +from dataclasses import dataclass, asdict + + +@dataclass +class VehicleState: + """车辆状态信息""" + vehicle_id: int + type_id: str + position: Tuple[float, float, float] + velocity: Tuple[float, float, float] + rotation: Tuple[float, float, float] + timestamp: float + sensors_available: List[str] + + +@dataclass +class V2XMessage: + """V2X通信消息""" + sender_id: int + message_type: str # 'state', 'object', 'warning', 'control' + data: Dict + timestamp: float + ttl: float # 生存时间 + priority: int # 优先级 + + +class MultiVehicleManager: + """多车辆协同管理器""" + + def __init__(self, world, config, output_dir): + self.world = world + self.config = config + self.output_dir = output_dir + + # 创建协同数据目录 + self.coop_dir = os.path.join(output_dir, "cooperative") + os.makedirs(self.coop_dir, exist_ok=True) + os.makedirs(os.path.join(self.coop_dir, "v2x_messages"), exist_ok=True) + os.makedirs(os.path.join(self.coop_dir, "shared_perception"), exist_ok=True) + + # 车辆管理 + self.ego_vehicles = [] # 主车辆列表 + self.cooperative_vehicles = [] # 协同车辆列表 + self.vehicle_states = {} # 车辆ID -> 车辆状态 + self.vehicle_sensors = {} # 车辆ID -> 传感器列表 + + # V2X通信 + self.v2x_messages = [] + self.communication_range = config.get('v2x', {}).get('communication_range', 300.0) # 通信范围 + self.message_buffer = defaultdict(list) # 车辆ID -> 接收的消息列表 + + # 协同感知 + self.shared_objects = [] # 共享的感知对象 + self.fusion_results = {} # 融合结果 + + # 统计 + self.stats = { + 'total_messages': 0, + 'successful_transmissions': 0, + 'collaborative_detections': 0, + 'data_exchange_mb': 0.0 + } + + def spawn_cooperative_vehicles(self, num_vehicles: int = 3) -> List[carla.Actor]: + """生成协同车辆""" + blueprint_lib = self.world.get_blueprint_library() + spawn_points = self.world.get_map().get_spawn_points() + + if not spawn_points: + print("警告:无生成点") + return [] + + # 车辆类型 + vehicle_types = [ + 'vehicle.tesla.model3', + 'vehicle.audi.tt', + 'vehicle.nissan.patrol', + 'vehicle.bmw.grandtourer', + 'vehicle.mercedes.coupe' + ] + + spawned_vehicles = [] + + for i in range(min(num_vehicles, len(spawn_points))): + try: + # 选择车辆类型 + vtype = random.choice(vehicle_types) + vehicle_bp = random.choice(blueprint_lib.filter(vtype)) + + # 设置车辆属性 + vehicle_bp.set_attribute('role_name', f'coop_vehicle_{i}') + + # 选择生成点 + spawn_point = spawn_points[i % len(spawn_points)] + + # 调整位置,使车辆不在同一位置 + offset_x = random.uniform(-5.0, 5.0) + offset_y = random.uniform(-5.0, 5.0) + location = carla.Location( + x=spawn_point.location.x + offset_x, + y=spawn_point.location.y + offset_y, + z=spawn_point.location.z + ) + + # 轻微调整朝向 + rotation = carla.Rotation( + pitch=spawn_point.rotation.pitch, + yaw=spawn_point.rotation.yaw + random.uniform(-15, 15), + roll=spawn_point.rotation.roll + ) + + transform = carla.Transform(location, rotation) + + # 生成车辆 + vehicle = self.world.spawn_actor(vehicle_bp, transform) + + # 设置自动驾驶 + vehicle.set_autopilot(True) + + # 添加到列表 + self.cooperative_vehicles.append(vehicle) + spawned_vehicles.append(vehicle) + + # 初始化车辆状态 + self.vehicle_states[vehicle.id] = VehicleState( + vehicle_id=vehicle.id, + type_id=vehicle.type_id, + position=(0, 0, 0), + velocity=(0, 0, 0), + rotation=(0, 0, 0), + timestamp=time.time(), + sensors_available=[] + ) + + print(f"协同车辆 {i + 1} 生成: {vehicle.type_id}") + + except Exception as e: + print(f"生成协同车辆失败: {e}") + + return spawned_vehicles + + def update_vehicle_states(self): + """更新所有车辆状态""" + current_time = time.time() + + for vehicle in self.ego_vehicles + self.cooperative_vehicles: + try: + if vehicle.is_alive: + location = vehicle.get_location() + velocity = vehicle.get_velocity() + rotation = vehicle.get_transform().rotation + + self.vehicle_states[vehicle.id] = VehicleState( + vehicle_id=vehicle.id, + type_id=vehicle.type_id, + position=(location.x, location.y, location.z), + velocity=(velocity.x, velocity.y, velocity.z), + rotation=(rotation.pitch, rotation.yaw, rotation.roll), + timestamp=current_time, + sensors_available=self.vehicle_sensors.get(vehicle.id, []) + ) + except: + pass + + def create_v2x_message(self, sender_id: int, message_type: str, data: Dict, + priority: int = 1) -> V2XMessage: + """创建V2X消息""" + message = V2XMessage( + sender_id=sender_id, + message_type=message_type, + data=data, + timestamp=time.time(), + ttl=5.0, # 5秒生存时间 + priority=priority + ) + + self.v2x_messages.append(message) + self.stats['total_messages'] += 1 + + return message + + def broadcast_message(self, message: V2XMessage): + """广播消息给范围内的车辆""" + if message.sender_id not in self.vehicle_states: + return + + sender_state = self.vehicle_states[message.sender_id] + sender_pos = sender_state.position + + recipients = [] + + # 检查所有车辆是否在通信范围内 + for vehicle in self.ego_vehicles + self.cooperative_vehicles: + if vehicle.id != message.sender_id and vehicle.id in self.vehicle_states: + receiver_state = self.vehicle_states[vehicle.id] + receiver_pos = receiver_state.position + + # 计算距离 + distance = math.sqrt( + (sender_pos[0] - receiver_pos[0]) ** 2 + + (sender_pos[1] - receiver_pos[1]) ** 2 + + (sender_pos[2] - receiver_pos[2]) ** 2 + ) + + if distance <= self.communication_range: + recipients.append(vehicle.id) + + # 添加到接收者消息缓冲区 + self.message_buffer[vehicle.id].append({ + 'message': message, + 'receive_time': time.time(), + 'signal_strength': 1.0 - (distance / self.communication_range) + }) + + self.stats['successful_transmissions'] += len(recipients) + + # 保存消息 + self._save_v2x_message(message, recipients) + + return recipients + + def share_perception_data(self, vehicle_id: int, detected_objects: List[Dict]): + """共享感知数据""" + if not detected_objects: + return + + # 创建感知消息 + perception_data = { + 'vehicle_id': vehicle_id, + 'timestamp': time.time(), + 'objects': detected_objects, + 'vehicle_state': asdict( + self.vehicle_states.get(vehicle_id, VehicleState(0, '', (0, 0, 0), (0, 0, 0), (0, 0, 0), 0, []))) + } + + message = self.create_v2x_message( + vehicle_id, + 'perception', + perception_data, + priority=2 # 感知数据优先级较高 + ) + + # 广播消息 + recipients = self.broadcast_message(message) + + # 融合共享的感知数据 + if recipients: + self._fuse_shared_perception(vehicle_id, detected_objects, recipients) + + return message + + def share_traffic_warning(self, vehicle_id: int, warning_type: str, + location: Tuple[float, float, float], + severity: str = 'medium'): + """共享交通警告""" + warning_data = { + 'warning_type': warning_type, # 'accident', 'congestion', 'hazard', 'construction' + 'location': location, + 'severity': severity, + 'timestamp': time.time(), + 'source_vehicle': vehicle_id + } + + message = self.create_v2x_message( + vehicle_id, + 'warning', + warning_data, + priority=3 # 警告消息优先级最高 + ) + + self.broadcast_message(message) + + return message + + def _fuse_shared_perception(self, source_id: int, objects: List[Dict], recipients: List[int]): + """融合共享的感知数据""" + fused_objects = [] + + for obj in objects: + # 转换为全局坐标系(简化处理,实际需要坐标变换) + global_obj = obj.copy() + global_obj['source_vehicles'] = [source_id] + global_obj['confidence'] = obj.get('confidence', 0.8) # 降低置信度 + + # 检查是否已有类似对象 + matched = False + for existing_obj in self.shared_objects: + # 简单的对象匹配(基于位置) + if self._objects_match(global_obj, existing_obj): + # 更新现有对象 + existing_obj['source_vehicles'].append(source_id) + existing_obj['confidence'] = min(1.0, existing_obj.get('confidence', 0) + 0.1) + existing_obj['update_time'] = time.time() + matched = True + break + + if not matched: + global_obj['detection_time'] = time.time() + fused_objects.append(global_obj) + + self.shared_objects.extend(fused_objects) + + # 清理旧对象 + current_time = time.time() + self.shared_objects = [ + obj for obj in self.shared_objects + if current_time - obj.get('detection_time', 0) < 10.0 # 保留10秒内的对象 + ] + + if fused_objects: + self.stats['collaborative_detections'] += len(fused_objects) + + def _objects_match(self, obj1: Dict, obj2: Dict, distance_threshold: float = 5.0) -> bool: + """判断两个对象是否匹配""" + if obj1.get('class') != obj2.get('class'): + return False + + # 计算位置距离 + pos1 = obj1.get('position', {'x': 0, 'y': 0, 'z': 0}) + pos2 = obj2.get('position', {'x': 0, 'y': 0, 'z': 0}) + + distance = math.sqrt( + (pos1['x'] - pos2['x']) ** 2 + + (pos1['y'] - pos2['y']) ** 2 + + (pos1['z'] - pos2['z']) ** 2 + ) + + return distance < distance_threshold + + def get_shared_perception_for_vehicle(self, vehicle_id: int) -> List[Dict]: + """获取车辆可用的共享感知数据""" + shared_data = [] + + for obj in self.shared_objects: + # 检查对象是否在车辆视野内(简化) + shared_data.append(obj) + + return shared_data + + def coordinate_maneuvers(self, maneuvers: List[Dict]): + """协调多车辆机动""" + # 分配优先级和时序 + coordinated = [] + + for i, maneuver in enumerate(maneuvers): + coordinated_maneuver = maneuver.copy() + coordinated_maneuver['sequence'] = i + coordinated_maneuver['start_time'] = time.time() + i * 2.0 # 间隔2秒 + coordinated.append(coordinated_maneuver) + + # 发送协调消息 + message = self.create_v2x_message( + maneuver.get('vehicle_id', 0), + 'coordination', + coordinated_maneuver, + priority=2 + ) + + self.broadcast_message(message) + + return coordinated + + def _save_v2x_message(self, message: V2XMessage, recipients: List[int]): + """保存V2X消息到文件""" + message_data = { + 'message': asdict(message), + 'recipients': recipients, + 'transmission_time': time.time() + } + + filename = f"v2x_{int(time.time() * 1000)}_{message.sender_id}_{message.message_type}.json" + filepath = os.path.join(self.coop_dir, "v2x_messages", filename) + + with open(filepath, 'w', encoding='utf-8') as f: + json.dump(message_data, f, indent=2, ensure_ascii=False) + + def save_shared_perception(self, frame_num: int): + """保存共享感知数据""" + data = { + 'frame_id': frame_num, + 'timestamp': time.time(), + 'shared_objects': self.shared_objects, + 'active_vehicles': len(self.ego_vehicles + self.cooperative_vehicles), + 'stats': self.stats + } + + filepath = os.path.join(self.coop_dir, "shared_perception", f"frame_{frame_num:06d}.json") + + with open(filepath, 'w', encoding='utf-8') as f: + json.dump(data, f, indent=2, ensure_ascii=False) + + def generate_summary(self): + """生成协同摘要报告""" + summary = { + 'total_vehicles': len(self.ego_vehicles) + len(self.cooperative_vehicles), + 'ego_vehicles': len(self.ego_vehicles), + 'cooperative_vehicles': len(self.cooperative_vehicles), + 'v2x_stats': self.stats, + 'shared_objects_count': len(self.shared_objects), + 'communication_range': self.communication_range, + 'average_messages_per_vehicle': self.stats['total_messages'] / max(1, len(self.ego_vehicles) + len( + self.cooperative_vehicles)) + } + + filepath = os.path.join(self.coop_dir, "cooperative_summary.json") + + with open(filepath, 'w', encoding='utf-8') as f: + json.dump(summary, f, indent=2, ensure_ascii=False) + + return summary + + def cleanup(self): + """清理资源""" + print("清理协同车辆...") + + for vehicle in self.cooperative_vehicles: + try: + if vehicle.is_alive: + vehicle.destroy() + except: + pass + + self.cooperative_vehicles.clear() + self.ego_vehicles.clear() + self.vehicle_states.clear() + self.message_buffer.clear() \ No newline at end of file diff --git a/src/enhance_pedestrian_safety/v2x_communication.py b/src/enhance_pedestrian_safety/v2x_communication.py new file mode 100644 index 0000000000..63079cc46a --- /dev/null +++ b/src/enhance_pedestrian_safety/v2x_communication.py @@ -0,0 +1,328 @@ +import socket +import threading +import json +import time +import struct +import numpy as np +from typing import Dict, List, Any, Optional +from dataclasses import dataclass, asdict +import queue + + +@dataclass +class V2XMessage: + """V2X通信消息""" + message_id: str + sender_id: str + message_type: str # 'bsm', 'spat', 'map', 'rsm', 'rsa' + data: Dict[str, Any] + timestamp: float + ttl: float + priority: int + position: Optional[tuple] = None + + +class V2XCommunication: + """V2X通信模拟器""" + + def __init__(self, config: Dict): + self.config = config + self.enabled = config.get('enabled', True) + + # 通信参数 + self.communication_range = config.get('communication_range', 300.0) # 通信范围(米) + self.bandwidth = config.get('bandwidth', 10.0) # 带宽(Mbps) + self.latency_mean = config.get('latency_mean', 0.05) # 平均延迟(秒) + self.latency_std = config.get('latency_std', 0.01) # 延迟标准差 + self.packet_loss_rate = config.get('packet_loss_rate', 0.01) # 丢包率 + + # 消息管理 + self.message_queue = queue.PriorityQueue() + self.received_messages = [] + self.message_counter = 0 + + # 网络模拟 + self.network_nodes = {} # 节点ID -> 节点信息 + self.connections = {} # 连接状态 + + # 统计信息 + self.stats = { + 'messages_sent': 0, + 'messages_received': 0, + 'messages_dropped': 0, + 'total_latency': 0.0, + 'bandwidth_used': 0.0 + } + + # 启动消息处理线程 + if self.enabled: + self.running = True + self.processor_thread = threading.Thread(target=self._message_processor, daemon=True) + self.processor_thread.start() + + def register_node(self, node_id: str, position: tuple, capabilities: Dict): + """注册网络节点(车辆)""" + self.network_nodes[node_id] = { + 'position': position, + 'capabilities': capabilities, + 'last_seen': time.time(), + 'status': 'online' + } + + def unregister_node(self, node_id: str): + """注销网络节点""" + if node_id in self.network_nodes: + del self.network_nodes[node_id] + + def send_message(self, message: V2XMessage) -> bool: + """发送V2X消息""" + if not self.enabled: + return False + + # 模拟网络延迟和丢包 + if np.random.random() < self.packet_loss_rate: + self.stats['messages_dropped'] += 1 + return False + + # 添加延迟 + latency = np.random.normal(self.latency_mean, self.latency_std) + delivery_time = time.time() + max(0, latency) + + # 添加到消息队列(按优先级和发送时间排序) + priority = -message.priority # 负号因为PriorityQueue是小顶堆 + self.message_queue.put((priority, delivery_time, message)) + + self.stats['messages_sent'] += 1 + self.stats['total_latency'] += latency + + return True + + def broadcast_basic_safety_message(self, node_id: str, vehicle_data: Dict) -> Optional[V2XMessage]: + """广播基本安全消息(BSM)""" + bsm_data = { + 'msgCnt': self.message_counter % 128, + 'id': node_id, + 'secMark': int(time.time() * 1000) % 60000, + 'position': vehicle_data.get('position', (0, 0, 0)), + 'accuracy': vehicle_data.get('accuracy', {'semiMajor': 1.0, 'semiMinor': 1.0}), + 'transmissionAndSpeed': { + 'transmission': vehicle_data.get('transmission', 'automatic'), + 'speed': vehicle_data.get('speed', 0.0) + }, + 'heading': vehicle_data.get('heading', 0.0), + 'steeringWheelAngle': vehicle_data.get('steering_wheel_angle', 0.0), + 'accelerationSet4Way': vehicle_data.get('acceleration', {'long': 0, 'lat': 0, 'vert': 0, 'yaw': 0}), + 'brakeSystemStatus': vehicle_data.get('brake_status', {'wheelBrakes': '00000'}), + 'vehicleSize': vehicle_data.get('size', {'width': 1.8, 'length': 4.5}) + } + + message = V2XMessage( + message_id=f"bsm_{node_id}_{self.message_counter}", + sender_id=node_id, + message_type='bsm', + data=bsm_data, + timestamp=time.time(), + ttl=1.0, + priority=1, + position=vehicle_data.get('position') + ) + + self.message_counter += 1 + + if self.send_message(message): + return message + + return None + + def broadcast_signal_phase_and_timing(self, intersection_id: str, spat_data: Dict) -> Optional[V2XMessage]: + """广播信号相位和时序消息(SPaT)""" + message = V2XMessage( + message_id=f"spat_{intersection_id}_{self.message_counter}", + sender_id=intersection_id, + message_type='spat', + data=spat_data, + timestamp=time.time(), + ttl=5.0, + priority=2, + position=spat_data.get('intersection_position') + ) + + self.message_counter += 1 + + if self.send_message(message): + return message + + return None + + def broadcast_map_data(self, map_id: str, map_data: Dict) -> Optional[V2XMessage]: + """广播地图数据(MAP)""" + message = V2XMessage( + message_id=f"map_{map_id}_{self.message_counter}", + sender_id=map_id, + message_type='map', + data=map_data, + timestamp=time.time(), + ttl=30.0, # 地图数据TTL较长 + priority=1, + position=map_data.get('reference_point') + ) + + self.message_counter += 1 + + if self.send_message(message): + return message + + return None + + def broadcast_roadside_safety_message(self, rsu_id: str, warning_data: Dict) -> Optional[V2XMessage]: + """广播路侧安全消息(RSM/RSA)""" + message = V2XMessage( + message_id=f"rsm_{rsu_id}_{self.message_counter}", + sender_id=rsu_id, + message_type='rsm', + data=warning_data, + timestamp=time.time(), + ttl=3.0, + priority=3, # 安全消息优先级高 + position=warning_data.get('event_position') + ) + + self.message_counter += 1 + + if self.send_message(message): + return message + + return None + + def get_reachable_nodes(self, sender_id: str, sender_position: tuple) -> List[str]: + """获取可达的网络节点""" + reachable = [] + + for node_id, node_info in self.network_nodes.items(): + if node_id == sender_id: + continue + + # 计算距离 + distance = self._calculate_distance(sender_position, node_info['position']) + + if distance <= self.communication_range: + # 模拟信号衰减 + signal_strength = 1.0 - (distance / self.communication_range) + if signal_strength > 0.3: # 最小信号强度阈值 + reachable.append(node_id) + + return reachable + + def _calculate_distance(self, pos1: tuple, pos2: tuple) -> float: + """计算两点间距离""" + if not pos1 or not pos2: + return float('inf') + + return np.sqrt( + (pos1[0] - pos2[0]) ** 2 + + (pos1[1] - pos2[1]) ** 2 + + (pos1[2] - pos2[2]) ** 2 + ) + + def _message_processor(self): + """消息处理线程""" + while self.running: + try: + # 获取下一个要传递的消息 + if not self.message_queue.empty(): + priority, delivery_time, message = self.message_queue.get_nowait() + + current_time = time.time() + + if current_time >= delivery_time: + # 消息已到传递时间 + self._deliver_message(message) + else: + # 重新放回队列 + self.message_queue.put((priority, delivery_time, message)) + time.sleep(0.001) # 短暂休眠 + else: + time.sleep(0.01) + + except queue.Empty: + time.sleep(0.01) + except Exception as e: + print(f"消息处理器错误: {e}") + + def _deliver_message(self, message: V2XMessage): + """传递消息到接收者""" + if not message.position: + return + + # 获取可达节点 + reachable_nodes = self.get_reachable_nodes(message.sender_id, message.position) + + # 模拟带宽限制 + message_size = len(json.dumps(asdict(message)).encode('utf-8')) / 1024 / 1024 # MB + bandwidth_required = message_size * 8 * len(reachable_nodes) # Mbps + + if bandwidth_required > self.bandwidth * 0.8: # 带宽超过80%时随机丢包 + drop_probability = min(1.0, bandwidth_required / self.bandwidth - 0.8) + reachable_nodes = [n for n in reachable_nodes if np.random.random() > drop_probability] + + # 添加到接收消息列表 + for node_id in reachable_nodes: + received_msg = { + 'message': asdict(message), + 'receiver_id': node_id, + 'receive_time': time.time(), + 'signal_strength': 1.0 - (self._calculate_distance(message.position, self.network_nodes[node_id][ + 'position']) / self.communication_range) + } + + self.received_messages.append(received_msg) + self.stats['messages_received'] += 1 + + # 更新带宽使用统计 + self.stats['bandwidth_used'] += bandwidth_required + + def get_messages_for_node(self, node_id: str, message_types: List[str] = None) -> List[Dict]: + """获取指定节点的消息""" + messages = [] + + for msg_record in self.received_messages: + if msg_record['receiver_id'] == node_id: + message = msg_record['message'] + if not message_types or message['message_type'] in message_types: + messages.append(msg_record) + + # 清理已处理的消息 + self.received_messages = [ + msg for msg in self.received_messages + if msg['receiver_id'] != node_id or + (message_types and msg['message']['message_type'] not in message_types) + ] + + return messages + + def get_network_status(self) -> Dict: + """获取网络状态""" + return { + 'active_nodes': len(self.network_nodes), + 'stats': self.stats, + 'bandwidth_utilization': (self.stats['bandwidth_used'] / ( + self.bandwidth * time.time())) * 100 if time.time() > 0 else 0, + 'message_delivery_rate': self.stats['messages_received'] / max(1, self.stats['messages_sent']), + 'average_latency': self.stats['total_latency'] / max(1, self.stats['messages_sent']) + } + + def reset_stats(self): + """重置统计信息""" + self.stats = { + 'messages_sent': 0, + 'messages_received': 0, + 'messages_dropped': 0, + 'total_latency': 0.0, + 'bandwidth_used': 0.0 + } + + def stop(self): + """停止通信模拟""" + self.running = False + if hasattr(self, 'processor_thread'): + self.processor_thread.join(timeout=1.0) \ No newline at end of file