From 5de89d64e28d26c89e87fcd9fcecb224b3fe3752 Mon Sep 17 00:00:00 2001 From: Z-w-7799 <1011069369@qq.com> Date: Thu, 18 Dec 2025 14:45:21 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E6=96=B0=E5=A2=9ELiDAR=E6=95=B0=E6=8D=AE?= =?UTF-8?q?=E5=A4=84=E7=90=86=EF=BC=8C=E5=A2=9E=E5=BC=BA=E4=BC=A0=E6=84=9F?= =?UTF-8?q?=E5=99=A8=E7=B3=BB=E7=BB=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../annotation_generator.py | 168 +++ .../config_manager.py | 108 ++ .../data_analyzer.py | 337 ++++++ .../data_validator.py | 309 ++++++ .../lidar_processor.py | 274 +++++ src/enhance_pedestrian_safety/main.py | 989 +++++++++++------- .../scene_manager.py | 245 +++++ 7 files changed, 2036 insertions(+), 394 deletions(-) create mode 100644 src/enhance_pedestrian_safety/annotation_generator.py create mode 100644 src/enhance_pedestrian_safety/config_manager.py create mode 100644 src/enhance_pedestrian_safety/data_analyzer.py create mode 100644 src/enhance_pedestrian_safety/data_validator.py create mode 100644 src/enhance_pedestrian_safety/lidar_processor.py create mode 100644 src/enhance_pedestrian_safety/scene_manager.py diff --git a/src/enhance_pedestrian_safety/annotation_generator.py b/src/enhance_pedestrian_safety/annotation_generator.py new file mode 100644 index 0000000000..d443854d96 --- /dev/null +++ b/src/enhance_pedestrian_safety/annotation_generator.py @@ -0,0 +1,168 @@ +import json +import os +import numpy as np +from datetime import datetime + + +class AnnotationGenerator: + """标注生成器""" + + def __init__(self, output_dir): + self.output_dir = output_dir + self.annotations_dir = os.path.join(output_dir, "annotations") + os.makedirs(self.annotations_dir, exist_ok=True) + + self.frame_annotations = {} + self.object_counter = 0 + + def detect_objects(self, world, frame_num, timestamp): + """检测场景中的物体""" + annotations = { + 'frame_id': frame_num, + 'timestamp': timestamp, + 'objects': [], + 'camera_info': {} + } + + try: + actors = world.get_actors() + + for actor in actors: + obj_type = actor.type_id + + if 'vehicle' in obj_type or 'walker' in obj_type: + obj_info = self._extract_object_info(actor) + if obj_info: + annotations['objects'].append(obj_info) + self.object_counter += 1 + + self._save_annotations(frame_num, annotations) + return annotations + + except Exception as e: + print(f"物体检测失败: {e}") + return annotations + + def _extract_object_info(self, actor): + """提取物体信息""" + try: + bbox = actor.bounding_box + location = actor.get_location() + velocity = actor.get_velocity() + + obj_info = { + 'id': actor.id, + 'type': actor.type_id, + 'class': self._get_object_class(actor.type_id), + 'location': { + 'x': float(location.x), + 'y': float(location.y), + 'z': float(location.z) + }, + 'velocity': { + 'x': float(velocity.x), + 'y': float(velocity.y), + 'z': float(velocity.z) + }, + 'bounding_box': { + 'extent': { + 'x': float(bbox.extent.x), + 'y': float(bbox.extent.y), + 'z': float(bbox.extent.z) + } + }, + 'attributes': { + 'is_alive': actor.is_alive + } + } + + return obj_info + + except Exception as e: + return None + + def _get_object_class(self, type_id): + """获取物体类别""" + type_lower = type_id.lower() + + if 'vehicle' in type_lower: + if 'tesla' in type_lower: + return 'car' + elif 'audi' in type_lower: + return 'car' + elif 'mini' in type_lower: + return 'car' + elif 'mercedes' in type_lower: + return 'car' + elif 'nissan' in type_lower: + return 'car' + elif 'bmw' in type_lower: + return 'car' + else: + return 'vehicle' + + elif 'walker' in type_lower: + return 'pedestrian' + + elif 'traffic' in type_lower: + return 'traffic_light' + + else: + return 'unknown' + + def _save_annotations(self, frame_num, annotations): + """保存标注到文件""" + filename = f"frame_{frame_num:06d}.json" + filepath = os.path.join(self.annotations_dir, filename) + + with open(filepath, 'w', encoding='utf-8') as f: + json.dump(annotations, f, indent=2, ensure_ascii=False) + + # 同时更新总标注文件 + self.frame_annotations[frame_num] = annotations + self._update_master_annotation() + + def _update_master_annotation(self): + """更新主标注文件""" + master_file = os.path.join(self.annotations_dir, "annotations.json") + + master_data = { + 'total_frames': len(self.frame_annotations), + 'total_objects': self.object_counter, + 'frames': list(self.frame_annotations.keys()), + 'created': datetime.now().isoformat() + } + + with open(master_file, 'w', encoding='utf-8') as f: + json.dump(master_data, f, indent=2, ensure_ascii=False) + + def generate_summary(self): + """生成标注摘要""" + vehicle_count = 0 + pedestrian_count = 0 + other_count = 0 + + for frame_data in self.frame_annotations.values(): + for obj in frame_data.get('objects', []): + if obj['class'] in ['car', 'vehicle']: + vehicle_count += 1 + elif obj['class'] == 'pedestrian': + pedestrian_count += 1 + else: + other_count += 1 + + summary = { + 'total_frames': len(self.frame_annotations), + 'total_objects': self.object_counter, + 'vehicles': vehicle_count, + 'pedestrians': pedestrian_count, + 'other_objects': other_count, + 'average_objects_per_frame': self.object_counter / len( + self.frame_annotations) if self.frame_annotations else 0 + } + + summary_file = os.path.join(self.annotations_dir, "summary.json") + with open(summary_file, 'w', encoding='utf-8') as f: + json.dump(summary, f, indent=2, ensure_ascii=False) + + return summary \ No newline at end of file diff --git a/src/enhance_pedestrian_safety/config_manager.py b/src/enhance_pedestrian_safety/config_manager.py new file mode 100644 index 0000000000..38622ee18a --- /dev/null +++ b/src/enhance_pedestrian_safety/config_manager.py @@ -0,0 +1,108 @@ +import json +import os +import argparse + + +class ConfigManager: + + @staticmethod + def load_config(config_file=None): + config = { + 'scenario': { + 'name': 'multi_sensor_scene', + 'town': 'Town10HD', + 'weather': 'clear', + 'time_of_day': 'noon', + 'duration': 60, + 'seed': 42 + }, + 'traffic': { + 'ego_vehicles': 1, + 'background_vehicles': 8, + 'pedestrians': 6, + 'traffic_lights': True + }, + 'sensors': { + 'vehicle_cameras': 4, + 'infrastructure_cameras': 4, + 'lidar_sensors': 1, + 'radar_sensors': 0, + 'image_size': [1280, 720], + 'capture_interval': 2.0, + 'lidar_config': { + 'channels': 32, + 'range': 100, + 'points_per_second': 56000, + 'rotation_frequency': 10 + } + }, + 'output': { + 'data_dir': 'cvips_dataset', + 'save_raw': True, + 'save_stitched': True, + 'save_annotations': False, + 'save_lidar': True, + 'save_fusion': True, + 'validate_data': True, + 'run_analysis': False + } + } + + if config_file and os.path.exists(config_file): + try: + with open(config_file, 'r') as f: + user_config = json.load(f) + ConfigManager._deep_update(config, user_config) + except: + pass + + return config + + @staticmethod + def _deep_update(original, update): + for key, value in update.items(): + if key in original and isinstance(original[key], dict) and isinstance(value, dict): + ConfigManager._deep_update(original[key], value) + else: + original[key] = value + + @staticmethod + def merge_args(config, args): + if args.scenario: + config['scenario']['name'] = args.scenario + if args.town: + config['scenario']['town'] = args.town + if args.weather: + config['scenario']['weather'] = args.weather + if args.time_of_day: + config['scenario']['time_of_day'] = args.time_of_day + if args.duration: + config['scenario']['duration'] = args.duration + if args.seed: + config['scenario']['seed'] = args.seed + + if args.num_vehicles: + config['traffic']['background_vehicles'] = args.num_vehicles + if args.num_pedestrians: + config['traffic']['pedestrians'] = args.num_pedestrians + + 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 + + if args.enable_fusion: + config['output']['save_fusion'] = True + + if args.enable_annotations: + config['output']['save_annotations'] = True + + if args.skip_validation: + config['output']['validate_data'] = False + + if args.run_analysis: + config['output']['run_analysis'] = True + + return config \ No newline at end of file diff --git a/src/enhance_pedestrian_safety/data_analyzer.py b/src/enhance_pedestrian_safety/data_analyzer.py new file mode 100644 index 0000000000..a5802c2dc4 --- /dev/null +++ b/src/enhance_pedestrian_safety/data_analyzer.py @@ -0,0 +1,337 @@ +import os +import json +import numpy as np +from collections import defaultdict + + +class DataAnalyzer: + """数据分析器 - 生成数据集统计信息""" + + @staticmethod + def analyze_dataset(data_dir): + """分析数据集并生成详细报告""" + print(f"分析数据集: {data_dir}") + + analysis = { + 'basic_stats': DataAnalyzer._get_basic_stats(data_dir), + 'file_distribution': DataAnalyzer._analyze_file_distribution(data_dir), + 'object_statistics': DataAnalyzer._analyze_objects(data_dir), + 'temporal_analysis': DataAnalyzer._analyze_temporal(data_dir), + 'quality_metrics': DataAnalyzer._calculate_quality_metrics(data_dir) + } + + # 生成评分 + analysis['overall_score'] = DataAnalyzer._calculate_overall_score(analysis) + + # 保存分析结果 + DataAnalyzer._save_analysis_report(data_dir, analysis) + + # 打印摘要 + DataAnalyzer._print_analysis_summary(analysis) + + return analysis + + @staticmethod + def _get_basic_stats(data_dir): + """获取基本统计信息""" + stats = { + 'total_size_mb': DataAnalyzer._get_directory_size(data_dir), + 'file_count': 0, + 'directory_count': 0, + 'data_types': defaultdict(int) + } + + for root, dirs, files in os.walk(data_dir): + stats['directory_count'] += len(dirs) + stats['file_count'] += len(files) + + for file in files: + ext = os.path.splitext(file)[1].lower() + if ext in ['.png', '.jpg', '.jpeg']: + stats['data_types']['images'] += 1 + elif ext == '.json': + stats['data_types']['json'] += 1 + elif ext == '.txt': + stats['data_types']['text'] += 1 + elif ext == '.bin': + stats['data_types']['binary'] += 1 + else: + stats['data_types']['other'] += 1 + + return stats + + @staticmethod + def _get_directory_size(path): + """计算目录大小""" + total = 0 + for dirpath, dirnames, filenames in os.walk(path): + for f in filenames: + fp = os.path.join(dirpath, f) + if os.path.exists(fp): + total += os.path.getsize(fp) + return round(total / (1024 * 1024), 2) + + @staticmethod + def _analyze_file_distribution(data_dir): + """分析文件分布""" + distribution = {} + + # 分析原始图像 + raw_dirs = ['vehicle', 'infrastructure'] + for raw_dir in raw_dirs: + path = os.path.join(data_dir, "raw", raw_dir) + if os.path.exists(path): + 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 + + # 分析拼接图像 + stitched_dir = os.path.join(data_dir, "stitched") + if os.path.exists(stitched_dir): + stitched_images = [f for f in os.listdir(stitched_dir) + if f.endswith(('.jpg', '.jpeg', '.png'))] + distribution['stitched'] = len(stitched_images) + + # 分析标注文件 + annotations_dir = os.path.join(data_dir, "annotations") + if os.path.exists(annotations_dir): + json_files = [f for f in os.listdir(annotations_dir) if f.endswith('.json')] + distribution['annotations'] = len(json_files) + + return distribution + + @staticmethod + 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': {}} + + object_stats = { + 'total_objects': 0, + 'by_class': defaultdict(int), + 'by_frame': defaultdict(int), + 'class_distribution': {} + } + + json_files = [f for f in os.listdir(annotations_dir) + if f.endswith('.json') and f.startswith('frame_')] + + for json_file in json_files: + try: + with open(os.path.join(annotations_dir, json_file), 'r') as f: + data = json.load(f) + + frame_id = data.get('frame_id', 0) + objects = data.get('objects', []) + + object_stats['by_frame'][frame_id] = len(objects) + object_stats['total_objects'] += len(objects) + + for obj in objects: + obj_class = obj.get('class', 'unknown') + object_stats['by_class'][obj_class] += 1 + + except Exception as e: + print(f"分析标注文件 {json_file} 失败: {e}") + + # 计算类分布百分比 + if object_stats['total_objects'] > 0: + for obj_class, count in object_stats['by_class'].items(): + object_stats['class_distribution'][obj_class] = round( + count / object_stats['total_objects'] * 100, 2) + + return object_stats + + @staticmethod + def _analyze_temporal(data_dir): + """分析时间分布""" + temporal_stats = { + 'frame_intervals': [], + 'total_duration': 0, + 'frame_rate': 0 + } + + metadata_file = os.path.join(data_dir, "metadata", "collection_info.json") + if os.path.exists(metadata_file): + try: + with open(metadata_file, 'r') as f: + metadata = json.load(f) + + collection_stats = metadata.get('collection_stats', {}) + temporal_stats['total_duration'] = collection_stats.get('duration_seconds', 0) + temporal_stats['frame_rate'] = collection_stats.get('average_fps', 0) + + except Exception as e: + print(f"分析元数据失败: {e}") + + return temporal_stats + + @staticmethod + def _calculate_quality_metrics(data_dir): + """计算质量指标""" + quality_metrics = { + 'completeness_score': 0, + 'consistency_score': 0, + 'diversity_score': 0, + 'issues_found': [] + } + + # 检查完整性 + 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'] = 50 + else: + quality_metrics['completeness_score'] = 100 + + # 检查一致性(图像数量) + raw_vehicle = os.path.join(data_dir, "raw", "vehicle") + if os.path.exists(raw_vehicle): + camera_counts = [] + for camera_dir in os.listdir(raw_vehicle): + camera_path = os.path.join(raw_vehicle, camera_dir) + if os.path.isdir(camera_path): + images = [f for f in os.listdir(camera_path) + if f.endswith('.png')] + camera_counts.append(len(images)) + + if camera_counts: + 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 + else: + quality_metrics['consistency_score'] = 95 + + # 多样性评分(基于物体类别) + object_stats = DataAnalyzer._analyze_objects(data_dir) + num_classes = len(object_stats.get('by_class', {})) + + if num_classes >= 5: + quality_metrics['diversity_score'] = 90 + elif num_classes >= 3: + quality_metrics['diversity_score'] = 70 + else: + quality_metrics['diversity_score'] = 50 + quality_metrics['issues_found'].append(f"物体类别较少: {num_classes}类") + + return quality_metrics + + @staticmethod + def _calculate_overall_score(analysis): + """计算总体评分""" + weights = { + 'completeness': 0.3, + 'consistency': 0.25, + 'diversity': 0.25, + 'temporal': 0.2 + } + + quality = analysis['quality_metrics'] + + score = ( + quality['completeness_score'] * weights['completeness'] + + quality['consistency_score'] * weights['consistency'] + + quality['diversity_score'] * weights['diversity'] + ) + + # 时间因素 + temporal = analysis['temporal_analysis'] + if temporal['frame_rate'] >= 2.0: + score += 20 * weights['temporal'] + elif temporal['frame_rate'] >= 1.0: + score += 15 * weights['temporal'] + else: + score += 5 * weights['temporal'] + + return round(score, 1) + + @staticmethod + def _save_analysis_report(data_dir, analysis): + """保存分析报告""" + report_file = os.path.join(data_dir, "metadata", "dataset_analysis.json") + + with open(report_file, 'w', encoding='utf-8') as f: + json.dump(analysis, f, indent=2, ensure_ascii=False) + + print(f"数据集分析报告保存: {report_file}") + + @staticmethod + def _print_analysis_summary(analysis): + """打印分析摘要""" + print("\n" + "=" * 60) + print("数据集分析摘要") + print("=" * 60) + + basic = analysis['basic_stats'] + print(f"\n基本统计:") + print(f" 总大小: {basic['total_size_mb']} MB") + print(f" 文件数: {basic['file_count']}") + print(f" 目录数: {basic['directory_count']}") + + print(f" 数据类型:") + for data_type, count in basic['data_types'].items(): + print(f" {data_type}: {count}") + + distribution = analysis['file_distribution'] + print(f"\n文件分布:") + for key, value in distribution.items(): + if isinstance(value, dict): + print(f" {key}:") + for subkey, subvalue in value.items(): + print(f" {subkey}: {subvalue}") + else: + print(f" {key}: {value}") + + objects = analysis['object_statistics'] + print(f"\n物体统计:") + print(f" 总物体数: {objects['total_objects']}") + if objects['by_class']: + print(f" 类别分布:") + for obj_class, count in objects['by_class'].items(): + percentage = objects['class_distribution'].get(obj_class, 0) + print(f" {obj_class}: {count} ({percentage}%)") + + temporal = analysis['temporal_analysis'] + print(f"\n时间分析:") + print(f" 总时长: {temporal['total_duration']:.1f}秒") + print(f" 平均帧率: {temporal['frame_rate']:.2f} FPS") + + quality = analysis['quality_metrics'] + print(f"\n质量指标:") + print(f" 完整性: {quality['completeness_score']}/100") + print(f" 一致性: {quality['consistency_score']}/100") + print(f" 多样性: {quality['diversity_score']}/100") + + if quality['issues_found']: + print(f" 发现的问题 ({len(quality['issues_found'])}):") + for issue in quality['issues_found'][:3]: + print(f" - {issue}") + if len(quality['issues_found']) > 3: + print(f" ... 还有 {len(quality['issues_found']) - 3} 个问题") + + print(f"\n总体评分: {analysis['overall_score']}/100") + + if analysis['overall_score'] >= 90: + print("✓ 数据集质量优秀") + elif analysis['overall_score'] >= 75: + print("✓ 数据集质量良好") + elif analysis['overall_score'] >= 60: + print("⚠ 数据集质量一般") + else: + print("✗ 数据集质量需要改进") + + print("=" * 60) \ No newline at end of file diff --git a/src/enhance_pedestrian_safety/data_validator.py b/src/enhance_pedestrian_safety/data_validator.py new file mode 100644 index 0000000000..f015231a42 --- /dev/null +++ b/src/enhance_pedestrian_safety/data_validator.py @@ -0,0 +1,309 @@ +import os +import json + + +class DataValidator: + + @staticmethod + def validate_dataset(data_dir): + print(f"验证数据集: {data_dir}") + + validation_results = { + 'directory_structure': DataValidator._check_directory_structure(data_dir), + 'raw_images': DataValidator._validate_raw_images(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) + } + + validation_results['overall_score'] = DataValidator._calculate_score(validation_results) + + DataValidator._save_validation_report(data_dir, validation_results) + DataValidator._print_validation_report(validation_results) + + return validation_results + + @staticmethod + def _check_directory_structure(data_dir): + required_dirs = [ + "raw/vehicle", + "raw/infrastructure", + "stitched", + "metadata" + ] + + missing_dirs = [] + for dir_path in required_dirs: + full_path = os.path.join(data_dir, dir_path) + if not os.path.exists(full_path): + missing_dirs.append(dir_path) + + status = 'PASS' if len(missing_dirs) == 0 else 'FAIL' + + result = { + 'status': status, + 'missing_directories': missing_dirs, + 'required_directories': required_dirs + } + + return result + + @staticmethod + def _validate_raw_images(data_dir): + 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))] + + total_images = 0 + errors = [] + + for camera_dir in camera_dirs: + camera_path = os.path.join(path, camera_dir) + images = [f for f in os.listdir(camera_path) if f.endswith(('.png', '.jpg', '.jpeg'))] + + for img_file in images: + img_path = os.path.join(camera_path, img_file) + try: + if os.path.getsize(img_path) == 0: + errors.append(f"空文件: {img_file}") + except: + errors.append(f"文件访问失败: {img_file}") + + total_images += len(images) + + if len(errors) == 0 and total_images > 0: + status = 'PASS' + elif len(errors) < 3 and total_images > 0: + status = 'WARNING' + else: + status = 'FAIL' + + results[raw_dir] = { + 'status': status, + 'count': total_images, + 'errors': errors + } + + return results + + @staticmethod + def _validate_stitched_images(data_dir): + stitched_dir = os.path.join(data_dir, "stitched") + + if not os.path.exists(stitched_dir): + return {'status': 'MISSING', 'count': 0, 'errors': []} + + images = [f for f in os.listdir(stitched_dir) if f.endswith(('.jpg', '.jpeg', '.png'))] + errors = [] + + for img_file in images: + img_path = os.path.join(stitched_dir, img_file) + try: + if os.path.getsize(img_path) == 0: + errors.append(f"空文件: {img_file}") + except: + errors.append(f"文件访问失败: {img_file}") + + if len(errors) == 0 and len(images) > 0: + status = 'PASS' + elif len(errors) < 3 and len(images) > 0: + status = 'WARNING' + else: + status = 'FAIL' + + return { + 'status': status, + 'count': len(images), + 'errors': errors + } + + @staticmethod + def _validate_annotations(data_dir): + annotations_dir = os.path.join(data_dir, "annotations") + + if not os.path.exists(annotations_dir): + return {'status': 'MISSING', 'count': 0, 'errors': []} + + json_files = [f for f in os.listdir(annotations_dir) if f.endswith('.json')] + errors = [] + valid_files = 0 + + for json_file in json_files: + json_path = os.path.join(annotations_dir, json_file) + try: + with open(json_path, 'r') as f: + data = json.load(f) + valid_files += 1 + except Exception as e: + errors.append(f"无效的JSON文件: {json_file} - {str(e)}") + + if len(errors) == 0 and valid_files > 0: + status = 'PASS' + elif len(errors) < 3 and valid_files > 0: + status = 'WARNING' + else: + status = 'FAIL' + + return { + 'status': status, + 'count': valid_files, + 'errors': errors + } + + @staticmethod + def _validate_metadata(data_dir): + metadata_dir = os.path.join(data_dir, "metadata") + + if not os.path.exists(metadata_dir): + return {'status': 'MISSING', 'count': 0, 'errors': []} + + json_files = [f for f in os.listdir(metadata_dir) if f.endswith('.json')] + errors = [] + valid_files = 0 + + for json_file in json_files: + json_path = os.path.join(metadata_dir, json_file) + try: + with open(json_path, 'r') as f: + data = json.load(f) + valid_files += 1 + except Exception as e: + errors.append(f"无效的JSON文件: {json_file} - {str(e)}") + + if len(errors) == 0 and valid_files > 0: + status = 'PASS' + elif len(errors) < 3 and valid_files > 0: + status = 'WARNING' + else: + status = 'FAIL' + + return { + 'status': status, + 'count': valid_files, + 'errors': errors + } + + @staticmethod + def _validate_lidar_data(data_dir): + lidar_dir = os.path.join(data_dir, "lidar") + + if not os.path.exists(lidar_dir): + return {'status': 'MISSING', 'count': 0, 'errors': []} + + bin_files = [f for f in os.listdir(lidar_dir) if f.endswith('.bin')] + 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_files += 1 + else: + errors.append(f"空文件: {bin_file}") + except: + errors.append(f"文件访问失败: {bin_file}") + + if len(errors) == 0 and valid_files > 0: + status = 'PASS' + elif len(errors) < 3 and valid_files > 0: + status = 'WARNING' + else: + status = 'FAIL' + + return { + 'status': status, + 'count': valid_files, + 'errors': errors + } + + @staticmethod + def _calculate_score(results): + weights = { + 'directory_structure': 0.15, + 'raw_images': 0.25, + 'stitched_images': 0.15, + 'annotations': 0.1, + 'metadata': 0.15, + 'lidar_data': 0.2 + } + + score = 0 + for key, weight in weights.items(): + if key not in results: + score += 30 * weight + continue + + result = results[key] + if 'status' not in result: + score += 30 * weight + continue + + if result['status'] == 'PASS': + score += 100 * weight + elif result['status'] == 'WARNING': + score += 70 * weight + elif result['status'] in ['FAIL', 'MISSING']: + score += 30 * weight + else: + score += 50 * weight + + return round(score, 1) + + @staticmethod + def _save_validation_report(data_dir, results): + report_path = os.path.join(data_dir, "metadata", "validation_report.json") + + with open(report_path, 'w', encoding='utf-8') as f: + json.dump(results, f, indent=2, ensure_ascii=False) + + @staticmethod + def _print_validation_report(results): + print("\n" + "=" * 60) + print("数据集验证报告") + print("=" * 60) + + for key, result in results.items(): + if key == 'overall_score': + continue + + print(f"\n{key.replace('_', ' ').title()}:") + + if 'status' in result: + print(f" 状态: {result['status']}") + else: + print(f" 状态: 未知") + + if 'count' in result: + print(f" 数量: {result['count']}") + + if 'errors' in result and result['errors']: + print(f" 错误 ({len(result['errors'])}):") + for error in result['errors'][:3]: + print(f" - {error}") + if len(result['errors']) > 3: + print(f" ... 还有 {len(result['errors']) - 3} 个错误") + + print(f"\n总体评分: {results.get('overall_score', 0)}/100") + + overall_score = results.get('overall_score', 0) + if overall_score >= 90: + print("✓ 数据集质量优秀") + elif overall_score >= 70: + print("✓ 数据集质量良好") + elif overall_score >= 50: + print("⚠ 数据集质量一般") + else: + print("✗ 数据集质量需要改进") + + print("=" * 60) \ No newline at end of file diff --git a/src/enhance_pedestrian_safety/lidar_processor.py b/src/enhance_pedestrian_safety/lidar_processor.py new file mode 100644 index 0000000000..fdcc61637b --- /dev/null +++ b/src/enhance_pedestrian_safety/lidar_processor.py @@ -0,0 +1,274 @@ +import numpy as np +import struct +import os +import json +import threading +from datetime import datetime + + +class LidarProcessor: + + def __init__(self, output_dir): + self.output_dir = output_dir + self.lidar_dir = os.path.join(output_dir, "lidar") + os.makedirs(self.lidar_dir, exist_ok=True) + + self.calibration_dir = os.path.join(output_dir, "calibration") + os.makedirs(self.calibration_dir, exist_ok=True) + + self.frame_counter = 0 + self.data_lock = threading.Lock() + + self._init_calibration_files() + + def _init_calibration_files(self): + lidar_intrinsic = { + "channels": 32, + "range": 100.0, + "points_per_second": 56000, + "rotation_frequency": 10.0, + "horizontal_fov": 360.0, + "vertical_fov": 30.0, + "upper_fov": 10.0, + "lower_fov": -20.0 + } + + lidar_extrinsic = { + "translation": [0.0, 0.0, 2.5], + "rotation": [0.0, 0.0, 0.0], + "matrix": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0] + ] + } + + intrinsic_file = os.path.join(self.calibration_dir, "lidar_intrinsic.json") + extrinsic_file = os.path.join(self.calibration_dir, "lidar_extrinsic.json") + + with open(intrinsic_file, 'w') as f: + json.dump(lidar_intrinsic, f, indent=2) + + with open(extrinsic_file, 'w') as f: + json.dump(lidar_extrinsic, f, indent=2) + + def process_lidar_data(self, lidar_data, frame_num): + with self.data_lock: + self.frame_counter = frame_num + + points = self._carla_lidar_to_numpy(lidar_data) + + if points is None or points.shape[0] == 0: + return None + + bin_path = self._save_as_bin(points, frame_num) + npy_path = self._save_as_npy(points, frame_num) + csv_path = self._save_as_csv(points, frame_num) + + metadata = self._generate_metadata(points, bin_path, npy_path, csv_path) + + return metadata + + def _carla_lidar_to_numpy(self, lidar_data): + try: + points = np.frombuffer(lidar_data.raw_data, dtype=np.float32) + points = np.reshape(points, (int(points.shape[0] / 4), 4)) + + points = points[:, :3] + + return points + + except Exception as e: + print(f"LiDAR数据转换失败: {e}") + + try: + points = [] + for point in lidar_data: + points.append([point.x, point.y, point.z]) + + return np.array(points, dtype=np.float32) + except: + return None + + def _save_as_bin(self, points, frame_num): + bin_path = os.path.join(self.lidar_dir, f"lidar_{frame_num:06d}.bin") + + points_with_intensity = np.zeros((points.shape[0], 4), dtype=np.float32) + points_with_intensity[:, :3] = points + points_with_intensity[:, 3] = 0.0 + + points_with_intensity.tofile(bin_path) + + return bin_path + + def _save_as_npy(self, points, frame_num): + npy_path = os.path.join(self.lidar_dir, f"lidar_{frame_num:06d}.npy") + np.save(npy_path, points) + return npy_path + + def _save_as_csv(self, points, frame_num): + csv_path = os.path.join(self.lidar_dir, f"lidar_{frame_num:06d}.csv") + + sample_points = points[:min(1000, points.shape[0])] + + header = "x,y,z\n" + + with open(csv_path, 'w') as f: + f.write(header) + np.savetxt(f, sample_points, delimiter=',', fmt='%.6f') + + return csv_path + + def _generate_metadata(self, points, bin_path, npy_path, csv_path): + metadata = { + 'frame_id': self.frame_counter, + 'timestamp': datetime.now().isoformat(), + 'point_count': int(points.shape[0]), + 'file_paths': { + 'bin': os.path.basename(bin_path), + 'npy': os.path.basename(npy_path), + 'csv': os.path.basename(csv_path) + }, + 'statistics': { + 'x_range': [float(points[:, 0].min()), float(points[:, 0].max())], + 'y_range': [float(points[:, 1].min()), float(points[:, 1].max())], + 'z_range': [float(points[:, 2].min()), float(points[:, 2].max())], + 'mean': [float(points[:, 0].mean()), float(points[:, 1].mean()), float(points[:, 2].mean())], + 'std': [float(points[:, 0].std()), float(points[:, 1].std()), float(points[:, 2].std())] + }, + 'file_sizes': { + 'bin': os.path.getsize(bin_path) if os.path.exists(bin_path) else 0, + 'npy': os.path.getsize(npy_path) if os.path.exists(npy_path) else 0, + 'csv': os.path.getsize(csv_path) if os.path.exists(csv_path) else 0 + } + } + + meta_path = os.path.join(self.lidar_dir, f"lidar_meta_{self.frame_counter:06d}.json") + with open(meta_path, 'w') as f: + json.dump(metadata, f, indent=2) + + return metadata + + def generate_lidar_summary(self): + if not os.path.exists(self.lidar_dir): + return None + + bin_files = [f for f in os.listdir(self.lidar_dir) if f.endswith('.bin')] + npy_files = [f for f in os.listdir(self.lidar_dir) if f.endswith('.npy')] + meta_files = [f for f in os.listdir(self.lidar_dir) if f.endswith('.json')] + + total_points = 0 + for bin_file in bin_files: + bin_path = os.path.join(self.lidar_dir, bin_file) + if os.path.exists(bin_path): + file_size = os.path.getsize(bin_path) + points_in_file = file_size // (4 * 4) + total_points += points_in_file + + summary = { + 'total_frames': len(bin_files), + 'total_points': total_points, + 'average_points_per_frame': total_points // max(len(bin_files), 1), + 'file_types': { + 'bin': len(bin_files), + 'npy': len(npy_files), + 'meta': len(meta_files) + }, + 'total_size_mb': round(sum(os.path.getsize(os.path.join(self.lidar_dir, f)) + for f in bin_files + npy_files + meta_files) / (1024 * 1024), 2) + } + + summary_path = os.path.join(self.output_dir, "metadata", "lidar_summary.json") + with open(summary_path, 'w') as f: + json.dump(summary, f, indent=2) + + return summary + + +class MultiSensorFusion: + + def __init__(self, output_dir): + self.output_dir = output_dir + self.fusion_dir = os.path.join(output_dir, "fusion") + os.makedirs(self.fusion_dir, exist_ok=True) + + self.calibration_data = {} + self._load_calibration() + + def _load_calibration(self): + calibration_dir = os.path.join(self.output_dir, "calibration") + + if os.path.exists(calibration_dir): + for file in os.listdir(calibration_dir): + if file.endswith('.json'): + filepath = os.path.join(calibration_dir, file) + try: + with open(filepath, 'r') as f: + data = json.load(f) + sensor_name = file.replace('.json', '') + self.calibration_data[sensor_name] = data + except: + pass + + def create_synchronization_file(self, frame_num, sensor_data): + sync_data = { + 'frame_id': frame_num, + 'timestamp': datetime.now().isoformat(), + 'sensors': {} + } + + for sensor_type, data_path in sensor_data.items(): + if data_path and os.path.exists(data_path): + sync_data['sensors'][sensor_type] = { + 'file_path': os.path.basename(data_path), + 'file_size': os.path.getsize(data_path), + 'timestamp': datetime.fromtimestamp(os.path.getctime(data_path)).isoformat() + } + + sync_file = os.path.join(self.fusion_dir, f"sync_{frame_num:06d}.json") + with open(sync_file, 'w') as f: + json.dump(sync_data, f, indent=2) + + return sync_file + + def generate_fusion_report(self): + if not os.path.exists(self.fusion_dir): + return None + + sync_files = [f for f in os.listdir(self.fusion_dir) if f.endswith('.json')] + + report = { + 'total_sync_frames': len(sync_files), + 'calibration_data': list(self.calibration_data.keys()), + 'sensor_types': set(), + 'frame_range': [] + } + + if sync_files: + frame_ids = [] + for sync_file in sync_files: + try: + frame_id = int(sync_file.split('_')[1].split('.')[0]) + frame_ids.append(frame_id) + + filepath = os.path.join(self.fusion_dir, sync_file) + with open(filepath, 'r') as f: + data = json.load(f) + + for sensor_type in data.get('sensors', {}).keys(): + report['sensor_types'].add(sensor_type) + + except: + continue + + if frame_ids: + report['frame_range'] = [min(frame_ids), max(frame_ids)] + + report['sensor_types'] = list(report['sensor_types']) + + report_path = os.path.join(self.output_dir, "metadata", "fusion_report.json") + with open(report_path, 'w') as f: + json.dump(report, f, indent=2) + + return report \ No newline at end of file diff --git a/src/enhance_pedestrian_safety/main.py b/src/enhance_pedestrian_safety/main.py index 41616f043f..64f7caf9ea 100644 --- a/src/enhance_pedestrian_safety/main.py +++ b/src/enhance_pedestrian_safety/main.py @@ -6,561 +6,762 @@ import traceback import math import threading +import json from datetime import datetime -from PIL import Image, ImageDraw, ImageFont from carla_utils import setup_carla_path, import_carla_module +from config_manager import ConfigManager +from annotation_generator import AnnotationGenerator +from data_validator import DataValidator +from scene_manager import SceneManager +from data_analyzer import DataAnalyzer +from lidar_processor import LidarProcessor, MultiSensorFusion carla_egg_path, remaining_argv = setup_carla_path() carla = import_carla_module() -class ImageStitcher: +class Log: + @staticmethod + def info(msg): + print(f"[INFO] {msg}") + + @staticmethod + def warning(msg): + print(f"[WARNING] {msg}") + + @staticmethod + def error(msg): + print(f"[ERROR] {msg}") + + @staticmethod + def debug(msg): + print(f"[DEBUG] {msg}") + + +class WeatherSystem: + WEATHER_PRESETS = { + 'clear': {'cloudiness': 10, 'precipitation': 0, 'wind': 5}, + 'rainy': {'cloudiness': 90, 'precipitation': 80, 'wind': 15}, + 'cloudy': {'cloudiness': 70, 'precipitation': 10, 'wind': 10}, + 'foggy': {'cloudiness': 50, 'precipitation': 0, 'fog_density': 40} + } + + @staticmethod + def create_weather(weather_type, time_of_day): + weather = carla.WeatherParameters() + + if weather_type in WeatherSystem.WEATHER_PRESETS: + preset = WeatherSystem.WEATHER_PRESETS[weather_type] + weather.cloudiness = preset.get('cloudiness', 30) + weather.precipitation = preset.get('precipitation', 0) + weather.wind_intensity = preset.get('wind', 5) + if 'fog_density' in preset: + weather.fog_density = preset['fog_density'] + + if time_of_day == 'noon': + weather.sun_altitude_angle = 75 + elif time_of_day == 'sunset': + weather.sun_altitude_angle = 15 + elif time_of_day == 'night': + weather.sun_altitude_angle = -20 + + return weather + + +class ImageProcessor: def __init__(self, output_dir): self.output_dir = output_dir - self.stitched_dir = os.path.join(output_dir, "stitched_images") + self.stitched_dir = os.path.join(output_dir, "stitched") os.makedirs(self.stitched_dir, exist_ok=True) - self.font = self._load_font() + def stitch(self, image_paths, frame_num, view_type="vehicle"): + try: + from PIL import Image, ImageDraw + except ImportError: + Log.warning("PIL未安装,跳过图像拼接") + return False - def _load_font(self): - font_paths = [ - "arial.ttf", - "/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf", - "/System/Library/Fonts/Helvetica.ttc" - ] + positions = [(10, 10), (660, 10), (10, 390), (660, 390)] - for font_path in font_paths: - try: - return ImageFont.truetype(font_path, 20) - except: - continue - return ImageFont.load_default() - - def stitch_ego_vehicle_images(self, image_paths, frame_num): - positions = { - 'front_wide': (10, 10), - 'front_narrow': (660, 10), - 'right_side': (10, 390), - 'left_side': (660, 390) - } + canvas = Image.new('RGB', (640 * 2 + 20, 360 * 2 + 20), (40, 40, 40)) + draw = ImageDraw.Draw(canvas) - images = [] - for cam_name in positions.keys(): - img_path = image_paths.get(cam_name) + for idx, (cam_name, img_path) in enumerate(list(image_paths.items())[:4]): if img_path and os.path.exists(img_path): try: img = Image.open(img_path).resize((640, 360)) - images.append((cam_name, img)) except: - images.append((cam_name, Image.new('RGB', (640, 360), (100, 100, 100)))) + img = Image.new('RGB', (640, 360), (80, 80, 80)) else: - images.append((cam_name, Image.new('RGB', (640, 360), (100, 100, 100)))) + img = Image.new('RGB', (640, 360), (80, 80, 80)) - if len(images) < 4: - return False + canvas.paste(img, positions[idx]) + draw.text((positions[idx][0] + 5, positions[idx][1] + 5), + cam_name, fill=(255, 255, 200)) - canvas = Image.new('RGB', (640 * 2 + 20, 360 * 2 + 20), (50, 50, 50)) + output_path = os.path.join(self.stitched_dir, f"{view_type}_{frame_num:06d}.jpg") + canvas.save(output_path, "JPEG", quality=90) + return True - for cam_name, img in images: - x, y = positions[cam_name] - canvas.paste(img, (x, y)) - draw = ImageDraw.Draw(canvas) - label = cam_name.replace('_', ' ').title() - draw.text((x + 10, y + 10), label, fill=(255, 255, 255), font=self.font) - draw = ImageDraw.Draw(canvas) - title = f"CVIPS - Frame {frame_num:04d}" - timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S") - draw.text((canvas.width // 2 - 150, 5), title, fill=(255, 255, 255), font=self.font) - draw.text((10, canvas.height - 30), timestamp, fill=(200, 200, 200), font=self.font) +class TrafficManager: + def __init__(self, world, config): + self.world = world + self.config = config + self.vehicles = [] + self.pedestrians = [] - output_path = os.path.join(self.stitched_dir, f"ego_stitched_{frame_num:04d}.jpg") - canvas.save(output_path, "JPEG", quality=95) + seed = config['scenario'].get('seed', random.randint(1, 1000)) + random.seed(seed) + Log.info(f"随机种子: {seed}") - return True + def spawn_ego_vehicle(self): + blueprint_lib = self.world.get_blueprint_library() + common_vehicles = [ + 'vehicle.tesla.model3', + 'vehicle.audi.tt', + 'vehicle.mini.cooperst', + 'vehicle.nissan.micra' + ] -class PedestrianController: - STATE_WAITING = "waiting" - STATE_CROSSING = "crossing" - STATE_WALKING = "walking" - STATE_STOPPED = "stopped" + for vtype in common_vehicles: + if blueprint_lib.filter(vtype): + vehicle_bp = random.choice(blueprint_lib.filter(vtype)) + break + else: + vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*')) - def __init__(self, world): - self.world = world - self.pedestrians = [] - self.running = True + spawn_points = self.world.get_map().get_spawn_points() + if not spawn_points: + return None - def spawn_pedestrian(self, location, behavior_type="crossing"): + spawn_point = random.choice(spawn_points) + try: + vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) + vehicle.set_autopilot(True) + vehicle.apply_control(carla.VehicleControl(throttle=0.2)) + Log.info(f"主车: {vehicle.type_id}") + return vehicle + except Exception as e: + Log.warning(f"主车生成失败: {e}") + return None + + def spawn_traffic(self, center_location): + vehicles = self._spawn_vehicles() + pedestrians = self._spawn_pedestrians(center_location) + + Log.info(f"交通生成: {vehicles}辆车, {pedestrians}个行人") + return vehicles + pedestrians + + def _spawn_vehicles(self): blueprint_lib = self.world.get_blueprint_library() - ped_bps = list(blueprint_lib.filter('walker.pedestrian.*')) + spawn_points = self.world.get_map().get_spawn_points() - if not ped_bps: - return None + if not spawn_points: + return 0 - ped_bp = random.choice(ped_bps) - location.z += 1.0 + num_vehicles = min(self.config['traffic']['background_vehicles'], 10) + spawned = 0 - try: - pedestrian = self.world.spawn_actor(ped_bp, carla.Transform(location)) - controller_bp = blueprint_lib.find('controller.ai.walker') - controller = self.world.spawn_actor(controller_bp, carla.Transform(), attach_to=pedestrian) - controller.start() - - behavior_state = { - 'state': self.STATE_WAITING if behavior_type in ["crossing", "hesitant"] else self.STATE_WALKING, - 'wait_start': time.time(), - 'wait_duration': random.uniform(2.0, 5.0), - 'target': None, - 'original': location, - 'behavior': behavior_type - } + for _ in range(num_vehicles): + try: + vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*')) + spawn_point = random.choice(spawn_points) + vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) + vehicle.set_autopilot(True) + self.vehicles.append(vehicle) + spawned += 1 + except: + pass - if behavior_type == "walking": - behavior_state['target'] = self._get_random_location() + return spawned - self.pedestrians.append((pedestrian, controller, behavior_state)) - return pedestrian + def _spawn_pedestrians(self, center_location): + blueprint_lib = self.world.get_blueprint_library() - except Exception as e: - print(f"生成行人失败: {e}") - return None + num_peds = min(self.config['traffic']['pedestrians'], 8) + spawned = 0 - def _get_random_location(self): - try: - return self.world.get_random_location_from_navigation() - except: - return None + for _ in range(num_peds): + try: + ped_bps = list(blueprint_lib.filter('walker.pedestrian.*')) + if not ped_bps: + continue - def update_behaviors(self): - current_time = time.time() + ped_bp = random.choice(ped_bps) - for pedestrian, controller, state in self.pedestrians: - if not pedestrian.is_alive or not controller.is_alive: - continue + angle = random.uniform(0, 2 * math.pi) + distance = random.uniform(5.0, 12.0) - if state['behavior'] == "crossing": - self._update_crossing(pedestrian, controller, state, current_time) - elif state['behavior'] == "hesitant": - self._update_hesitant(pedestrian, controller, state, current_time) - else: - self._update_walking(pedestrian, controller, state, current_time) - - def _update_crossing(self, pedestrian, controller, state, current_time): - if state['state'] == self.STATE_WAITING: - if current_time - state['wait_start'] >= state['wait_duration']: - target = carla.Location( - x=state['original'].x + random.uniform(15.0, 25.0), - y=state['original'].y + random.uniform(-5.0, 5.0), - z=state['original'].z + location = carla.Location( + x=center_location.x + distance * math.cos(angle), + y=center_location.y + distance * math.sin(angle), + z=center_location.z + 0.5 ) - state['target'] = target - state['state'] = self.STATE_CROSSING - state['cross_start'] = current_time - controller.go_to_location(target) - - elif state['state'] == self.STATE_CROSSING: - distance = pedestrian.get_location().distance(state['target']) - if distance < 2.0 or current_time - state['cross_start'] > 15.0: - state['state'] = self.STATE_STOPPED - - def _update_hesitant(self, pedestrian, controller, state, current_time): - if state['state'] == self.STATE_WAITING: - if current_time - state['wait_start'] >= state['wait_duration']: - target = self._get_random_location() - if target: - state['target'] = target - state['state'] = self.STATE_WALKING - state['walk_start'] = current_time - state['walk_duration'] = random.uniform(3.0, 8.0) - controller.go_to_location(target) - - elif state['state'] == self.STATE_WALKING: - if current_time - state['walk_start'] >= state['walk_duration']: - state['state'] = self.STATE_WAITING - state['wait_start'] = current_time - state['wait_duration'] = random.uniform(1.0, 4.0) - - def _update_walking(self, pedestrian, controller, state, current_time): - if state['state'] == self.STATE_WALKING and state['target']: - distance = pedestrian.get_location().distance(state['target']) - if distance < 2.0: - new_target = self._get_random_location() - if new_target: - state['target'] = new_target - controller.go_to_location(new_target) - - def start_updates(self): - def update_loop(): - while self.running: - try: - self.update_behaviors() - time.sleep(0.5) - except Exception as e: - print(f"行为更新错误: {e}") - time.sleep(1.0) - threading.Thread(target=update_loop, daemon=True).start() + pedestrian = self.world.spawn_actor(ped_bp, carla.Transform(location)) + self.pedestrians.append(pedestrian) + spawned += 1 + except Exception as e: + Log.debug(f"行人生成失败: {e}") + + return spawned def cleanup(self): - self.running = False - for pedestrian, controller, _ in self.pedestrians: + Log.info("清理交通...") + + for vehicle in self.vehicles: + try: + if vehicle.is_alive: + vehicle.destroy() + except: + pass + + for pedestrian in self.pedestrians: try: - if controller.is_alive: - controller.stop() - controller.destroy() if pedestrian.is_alive: pedestrian.destroy() except: pass + + self.vehicles.clear() self.pedestrians.clear() -class DataGenerator: - def __init__(self, args): - self.args = args - self.client = None - self.world = None - self.actors = [] +class SensorManager: + def __init__(self, world, config, data_dir): + self.world = world + self.config = config + self.data_dir = data_dir self.sensors = [] - self.frame_count = 0 - self.last_capture_time = 0 - self.setup_output_directory() - self.stitcher = ImageStitcher(self.output_dir) - self.ped_controller = None + self.frame_counter = 0 + self.last_capture_time = 0 - self.image_buffer = {} + self.vehicle_buffer = {} + self.infra_buffer = {} self.buffer_lock = threading.Lock() - def setup_output_directory(self): - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - self.output_dir = os.path.join("cvips_data", f"{self.args.scenario}_{timestamp}") + self.image_processor = ImageProcessor(data_dir) + self.lidar_processor = None + self.fusion_manager = None - self.raw_dirs = {} - for view in ['front_wide', 'front_narrow', 'right_side', 'left_side']: - dir_path = os.path.join(self.output_dir, "raw", "ego_vehicle", view) - os.makedirs(dir_path, exist_ok=True) - self.raw_dirs[view] = dir_path + if config['sensors'].get('lidar_sensors', 0) > 0: + self.lidar_processor = LidarProcessor(data_dir) - print(f"数据输出目录: {self.output_dir}") + if config['output'].get('save_fusion', False): + self.fusion_manager = MultiSensorFusion(data_dir) - def connect_to_server(self): - for attempt in range(1, 6): - try: - self.client = carla.Client('localhost', 2000) - self.client.set_timeout(15.0) - - if self.args.town: - self.world = self.client.load_world(self.args.town) - else: - self.world = self.client.get_world() - - settings = self.world.get_settings() - settings.synchronous_mode = False - self.world.apply_settings(settings) + def setup_cameras(self, vehicle, center_location): + vehicle_cams = self._setup_vehicle_cameras(vehicle) + infra_cams = self._setup_infrastructure_cameras(center_location) - print(f"连接成功! 地图: {self.world.get_map().name}") - return True + Log.info(f"摄像头: {vehicle_cams}车辆 + {infra_cams}基础设施") + return vehicle_cams + infra_cams - except Exception as e: - print(f"尝试 {attempt}/5 失败: {str(e)[:80]}") - if attempt < 5: - time.sleep(3) + def _setup_vehicle_cameras(self, vehicle): + if not vehicle: + return 0 - return False + camera_configs = { + 'front_wide': {'loc': (2.0, 0, 1.8), 'rot': (0, -3, 0), 'fov': 100}, + 'front_narrow': {'loc': (2.0, 0, 1.6), 'rot': (0, 0, 0), 'fov': 60}, + 'right_side': {'loc': (0.5, 1.0, 1.5), 'rot': (0, -2, 45), 'fov': 90}, + 'left_side': {'loc': (0.5, -1.0, 1.5), 'rot': (0, -2, -45), 'fov': 90} + } - def setup_scene(self): - self.ped_controller = PedestrianController(self.world) + installed = 0 + for cam_name, config_data in camera_configs.items(): + if self._create_camera(cam_name, config_data, vehicle, 'vehicle'): + installed += 1 - self.set_weather() - time.sleep(2.0) + return installed - ego_vehicle = self.spawn_ego_vehicle() - if not ego_vehicle: - return None + def _setup_infrastructure_cameras(self, center_location): + camera_configs = [ + {'name': 'north', 'offset': (0, -20, 12), 'rotation': (0, -25, 180)}, + {'name': 'south', 'offset': (0, 20, 12), 'rotation': (0, -25, 0)}, + {'name': 'east', 'offset': (20, 0, 12), 'rotation': (0, -25, -90)}, + {'name': 'west', 'offset': (-20, 0, 12), 'rotation': (0, -25, 90)} + ] - self.spawn_pedestrians() - self.spawn_background_vehicles() - time.sleep(5.0) + installed = 0 + for cam_config in camera_configs: + sensor_config = { + 'loc': ( + center_location.x + cam_config['offset'][0], + center_location.y + cam_config['offset'][1], + center_location.z + cam_config['offset'][2] + ), + 'rot': cam_config['rotation'], + 'fov': 90 + } - self.ped_controller.start_updates() - return ego_vehicle + if self._create_camera(cam_config['name'], sensor_config, None, 'infrastructure'): + installed += 1 - def set_weather(self): - weather = carla.WeatherParameters() + return installed - if self.args.weather == 'clear': - weather.sun_altitude_angle = 75 - weather.cloudiness = 5.0 - elif self.args.weather == 'rainy': - weather.sun_altitude_angle = 40 - weather.cloudiness = 90.0 - weather.precipitation = 60.0 - else: - weather.sun_altitude_angle = 60 - weather.cloudiness = 70.0 + def setup_lidar(self, vehicle): + if not vehicle or not self.config['sensors'].get('lidar_sensors', 0) > 0: + return 0 - if self.args.time_of_day == 'night': - weather.sun_altitude_angle = -10 - elif self.args.time_of_day == 'sunset': - weather.sun_altitude_angle = 5 + try: + blueprint_lib = self.world.get_blueprint_library() + lidar_bp = blueprint_lib.find('sensor.lidar.ray_cast') + + lidar_config = self.config['sensors'].get('lidar_config', {}) + + lidar_bp.set_attribute('channels', str(lidar_config.get('channels', 32))) + lidar_bp.set_attribute('range', str(lidar_config.get('range', 100))) + 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') + + lidar_location = carla.Location(x=0, y=0, z=2.5) + lidar_rotation = carla.Rotation(0, 0, 0) + lidar_transform = carla.Transform(lidar_location, lidar_rotation) + + lidar_sensor = self.world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle) + + def lidar_callback(lidar_data): + current_time = time.time() + if current_time - self.last_capture_time >= self.config['sensors']['capture_interval']: + if self.lidar_processor: + 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: + # 取第一个摄像头的图像 + for cam_name, img_path in self.vehicle_buffer.items(): + if os.path.exists(img_path): + vehicle_image_path = img_path + break + + sensor_data = { + 'lidar': os.path.join(self.data_dir, "lidar", f"lidar_{self.frame_counter:06d}.bin") + } + if vehicle_image_path: + sensor_data['camera'] = vehicle_image_path + + self.fusion_manager.create_synchronization_file(self.frame_counter, sensor_data) + except Exception as e: + print(f"LiDAR处理失败: {e}") + + lidar_sensor.listen(lidar_callback) + self.sensors.append(lidar_sensor) + + print("LiDAR传感器已安装") + return 1 - self.world.set_weather(weather) + except Exception as e: + print(f"LiDAR安装失败: {e}") + return 0 + def _create_camera(self, name, config, parent, sensor_type): + try: + blueprint = self.world.get_blueprint_library().find('sensor.camera.rgb') - def spawn_ego_vehicle(self): - blueprint_lib = self.world.get_blueprint_library() - vehicle_types = ['vehicle.tesla.model3', 'vehicle.audi.tt', 'vehicle.mini.cooperst'] + img_size = self.config['sensors'].get('image_size', [1280, 720]) + blueprint.set_attribute('image_size_x', str(img_size[0])) + blueprint.set_attribute('image_size_y', str(img_size[1])) + blueprint.set_attribute('fov', str(config.get('fov', 90))) - vehicle_bp = None - for vtype in vehicle_types: - if blueprint_lib.filter(vtype): - vehicle_bp = random.choice(blueprint_lib.filter(vtype)) - break + location = carla.Location(config['loc'][0], config['loc'][1], config['loc'][2]) + rotation = carla.Rotation(config['rot'][0], config['rot'][1], config['rot'][2]) + transform = carla.Transform(location, rotation) - if not vehicle_bp: - vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*')) + if parent: + camera = self.world.spawn_actor(blueprint, transform, attach_to=parent) + else: + camera = self.world.spawn_actor(blueprint, transform) - spawn_points = self.world.get_map().get_spawn_points() - if not spawn_points: - return None + save_dir = os.path.join(self.data_dir, "raw", sensor_type, name) + os.makedirs(save_dir, exist_ok=True) - spawn_point = random.choice(spawn_points) + callback = self._create_callback(save_dir, name, sensor_type) + camera.listen(callback) - try: - vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) - self.actors.append(vehicle) - vehicle.set_autopilot(True) - vehicle.apply_control(carla.VehicleControl(throttle=0.2, steer=0.0)) + self.sensors.append(camera) + return True - print(f"主车辆: {vehicle.type_id}") - return vehicle except Exception as e: - print(f"生成主车辆失败: {e}") - return None - - def spawn_pedestrians(self): - spawn_points = self.world.get_map().get_spawn_points() - if not spawn_points: - return - - print(f"生成 {self.args.num_smart_pedestrians} 个行人...") + Log.warning(f"创建摄像头 {name} 失败: {e}") + return False - behaviors = ['crossing', 'hesitant', 'walking'] + def _create_callback(self, save_dir, name, sensor_type): + capture_interval = self.config['sensors']['capture_interval'] + + def callback(image): + current_time = time.time() + + if current_time - self.last_capture_time >= capture_interval: + self.frame_counter += 1 + self.last_capture_time = current_time + + filename = os.path.join(save_dir, f"{name}_{self.frame_counter:06d}.png") + image.save_to_disk(filename, carla.ColorConverter.Raw) + + with self.buffer_lock: + 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, 'vehicle') + self.vehicle_buffer.clear() + else: + self.infra_buffer[name] = filename + if len(self.infra_buffer) >= 4: + self.image_processor.stitch(self.infra_buffer, self.frame_counter, 'infrastructure') + self.infra_buffer.clear() + + return callback + + def get_frame_count(self): + return self.frame_counter + + def generate_sensor_summary(self): + summary = { + 'total_sensors': len(self.sensors), + 'frame_count': self.frame_counter, + 'lidar_data': None, + 'fusion_data': None + } - for _ in range(self.args.num_smart_pedestrians): - behavior = random.choice(behaviors) - spawn_point = random.choice(spawn_points) + if self.lidar_processor: + summary['lidar_data'] = self.lidar_processor.generate_lidar_summary() - if self.ped_controller.spawn_pedestrian(spawn_point.location, behavior): - self.actors.append(self.ped_controller.pedestrians[-1][0]) + if self.fusion_manager: + summary['fusion_data'] = self.fusion_manager.generate_fusion_report() - def spawn_background_vehicles(self): - blueprint_lib = self.world.get_blueprint_library() - spawn_points = self.world.get_map().get_spawn_points() + return summary - if not spawn_points: - return - - spawned = 0 - for _ in range(min(5, self.args.num_background_vehicles)): + def cleanup(self): + Log.info(f"清理 {len(self.sensors)} 个传感器...") + for sensor in self.sensors: try: - vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*')) - spawn_point = random.choice(spawn_points) - - vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) - self.actors.append(vehicle) - vehicle.set_autopilot(True) - spawned += 1 + sensor.stop() + sensor.destroy() except: pass + self.sensors.clear() - print(f"背景车辆: {spawned} 辆") - def setup_cameras(self, vehicle): - if not vehicle: - return False +class DataCollector: + def __init__(self, config): + self.config = config + self.client = None + self.world = None + self.ego_vehicle = None + self.scene_center = None - blueprint_lib = self.world.get_blueprint_library() + self.setup_directories() - camera_configs = [ - ('front_wide', carla.Location(x=2.0, z=1.8), carla.Rotation(pitch=-3.0), 100), - ('front_narrow', carla.Location(x=2.0, z=1.6), carla.Rotation(pitch=0), 60), - ('right_side', carla.Location(x=0.5, y=1.0, z=1.5), carla.Rotation(pitch=-2.0, yaw=45), 90), - ('left_side', carla.Location(x=0.5, y=-1.0, z=1.5), carla.Rotation(pitch=-2.0, yaw=-45), 90), + self.traffic_manager = None + self.sensor_manager = None + + self.start_time = None + self.is_running = False + self.collected_frames = 0 + + def setup_directories(self): + scenario = self.config['scenario'] + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + + self.output_dir = os.path.join( + self.config['output']['data_dir'], + f"{scenario['name']}_{scenario['town']}_{timestamp}" + ) + + directories = [ + "raw/vehicle", + "raw/infrastructure", + "stitched", + "lidar", + "fusion", + "calibration", + "metadata" ] - installed = 0 + for subdir in directories: + os.makedirs(os.path.join(self.output_dir, subdir), exist_ok=True) + + Log.info(f"数据目录: {self.output_dir}") - for name, location, rotation, fov in camera_configs: + def connect(self): + for attempt in range(1, 6): try: - camera_bp = blueprint_lib.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', '1280') - camera_bp.set_attribute('image_size_y', '720') - camera_bp.set_attribute('fov', str(fov)) - - camera = self.world.spawn_actor( - camera_bp, - carla.Transform(location, rotation), - attach_to=vehicle - ) + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(10.0) - def make_callback(save_dir, cam_name): - def callback(image): - current_time = time.time() + town = self.config['scenario']['town'] + self.world = self.client.load_world(town) - if current_time - self.last_capture_time >= self.args.capture_interval: - self.frame_count += 1 - self.last_capture_time = current_time + settings = self.world.get_settings() + settings.synchronous_mode = False + self.world.apply_settings(settings) - raw_filename = f"{save_dir}/{cam_name}_{self.frame_count:04d}.png" - image.save_to_disk(raw_filename, carla.ColorConverter.Raw) + Log.info(f"连接成功: {town}") + return True - with self.buffer_lock: - self.image_buffer[cam_name] = raw_filename + except Exception as e: + Log.warning(f"连接尝试 {attempt}/5 失败: {str(e)[:50]}") + time.sleep(2) - if len(self.image_buffer) == 4: - self.stitcher.stitch_ego_vehicle_images(self.image_buffer, self.frame_count) - self.image_buffer.clear() + return False - return callback + def setup_scene(self): + weather_cfg = self.config['scenario'] + weather = WeatherSystem.create_weather(weather_cfg['weather'], weather_cfg['time_of_day']) + self.world.set_weather(weather) + Log.info(f"天气: {weather_cfg['weather']}, 时间: {weather_cfg['time_of_day']}") - camera.listen(make_callback(self.raw_dirs[name], name)) - self.actors.append(camera) - self.sensors.append(camera) - installed += 1 + spawn_points = self.world.get_map().get_spawn_points() + if spawn_points: + self.scene_center = spawn_points[len(spawn_points) // 2].location + else: + self.scene_center = carla.Location(0, 0, 0) - except Exception as e: - print(f"安装 {name} 摄像头失败: {e}") + self.traffic_manager = TrafficManager(self.world, self.config) + + 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) + + time.sleep(3.0) + return True - print(f"摄像头安装: {installed}/4") - return installed == 4 + def setup_sensors(self): + self.sensor_manager = SensorManager(self.world, self.config, self.output_dir) + + cameras = self.sensor_manager.setup_cameras(self.ego_vehicle, self.scene_center) + if cameras == 0: + Log.error("没有摄像头安装成功") + return False + + lidars = self.sensor_manager.setup_lidar(self.ego_vehicle) + Log.info(f"传感器: {cameras}摄像头 + {lidars}LiDAR") + + return True def collect_data(self): - print(f"\n开始数据收集...") - print(f"时长: {self.args.total_dura.tion}秒, 间隔: {self.args.capture_interval}秒") + duration = self.config['scenario']['duration'] + Log.info(f"开始数据收集,时长: {duration}秒") + + self.start_time = time.time() + self.is_running = True - start_time = time.time() - self.frame_count = 0 - self.last_capture_time = start_time + last_update = time.time() try: - while time.time() - start_time < self.args.total_duration: - elapsed = time.time() - start_time - remaining = self.args.total_duration - elapsed + while time.time() - self.start_time < duration and self.is_running: + current_time = time.time() + elapsed = current_time - self.start_time - if int(elapsed) % 10 == 0: - progress = (elapsed / self.args.total_duration) * 100 - print(f"进度: {elapsed:.0f}/{self.args.total_duration}秒 ({progress:.1f}%) | " - f"批次: {self.frame_count} | 剩余: {remaining:.0f}秒") + if current_time - last_update >= 5.0: + frames = self.sensor_manager.get_frame_count() + progress = (elapsed / duration) * 100 - time.sleep(0.1) + Log.info(f"进度: {elapsed:.0f}/{duration}秒 ({progress:.1f}%) | 帧数: {frames}") + last_update = current_time - print(f"\n数据收集完成! 总批次: {self.frame_count}") + time.sleep(0.05) except KeyboardInterrupt: - print(f"\n数据收集中断, 已收集 {self.frame_count} 批次") + Log.info("数据收集被用户中断") + finally: + self.is_running = False + elapsed = time.time() - self.start_time + + self.collected_frames = self.sensor_manager.get_frame_count() + Log.info(f"收集完成: {self.collected_frames}帧, 用时: {elapsed:.1f}秒") + + self._save_metadata() + self._print_summary() + + def _save_metadata(self): + metadata = { + 'scenario': self.config['scenario'], + 'traffic': self.config['traffic'], + 'sensors': self.config['sensors'], + 'output': self.config['output'], + 'collection': { + 'duration': round(time.time() - self.start_time, 2), + 'total_frames': self.collected_frames, + 'frame_rate': round(self.collected_frames / max(time.time() - self.start_time, 0.1), 2) + } + } + + 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) - self.display_summary() + Log.info(f"元数据保存: {meta_path}") - def display_summary(self): + def _print_summary(self): print("\n" + "=" * 60) - print("数据收集摘要:") + print("数据收集摘要") print("=" * 60) - stitched_dir = os.path.join(self.output_dir, "stitched_images") + # 统计图像 + stitched_dir = os.path.join(self.output_dir, "stitched") if os.path.exists(stitched_dir): stitched_files = [f for f in os.listdir(stitched_dir) if f.endswith('.jpg')] print(f"拼接图像: {len(stitched_files)} 张") - raw_dir = os.path.join(self.output_dir, "raw") - if os.path.exists(raw_dir): - total_raw = 0 - for root, dirs, files in os.walk(raw_dir): - total_raw += len([f for f in files if f.endswith('.png')]) - print(f"原始图像: {total_raw} 张") + # 统计LiDAR + lidar_dir = os.path.join(self.output_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')] + print(f"LiDAR数据: {len(bin_files)} .bin文件, {len(npy_files)} .npy文件") + + if bin_files: + total_points = 0 + for bin_file in bin_files[:3]: + bin_path = os.path.join(lidar_dir, bin_file) + if os.path.exists(bin_path): + file_size = os.path.getsize(bin_path) + points_in_file = file_size // (4 * 4) + total_points += points_in_file + print(f" 估计总点数: {total_points:,}") + + # 统计融合数据 + 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) + + def run_validation(self): + if self.config['output'].get('validate_data', True): + Log.info("运行数据验证...") + DataValidator.validate_dataset(self.output_dir) - print(f"\n数据目录: {self.output_dir}") + def run_analysis(self): + if self.config['output'].get('run_analysis', False): + Log.info("运行数据分析...") + DataAnalyzer.analyze_dataset(self.output_dir) def cleanup(self): - if self.ped_controller: - self.ped_controller.cleanup() + Log.info("清理场景...") - for sensor in self.sensors: - try: - sensor.stop() - except: - pass + if self.sensor_manager: + self.sensor_manager.cleanup() - destroyed = 0 - for actor in self.actors: + if self.traffic_manager: + self.traffic_manager.cleanup() + + if self.ego_vehicle and self.ego_vehicle.is_alive: try: - if actor.is_alive: - actor.destroy() - destroyed += 1 + self.ego_vehicle.destroy() except: pass - print(f"清理 {destroyed} 个actor") - self.actors.clear() - self.sensors.clear() + Log.info("清理完成") def main(): - parser = argparse.ArgumentParser(description='CVIPS 数据生成器') + parser = argparse.ArgumentParser(description='CVIPS 多传感器数据收集系统') - parser.add_argument('--scenario', type=str, default='pedestrian_scene', help='场景名称') + # 基础参数 + 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='CARLA地图') + choices=['Town03', 'Town04', 'Town05', 'Town10HD'], help='地图') parser.add_argument('--weather', type=str, default='clear', - choices=['clear', 'rainy', 'cloudy'], help='天气条件') + choices=['clear', 'rainy', 'cloudy', 'foggy'], help='天气') parser.add_argument('--time-of-day', type=str, default='noon', choices=['noon', 'sunset', 'night'], help='时间') - parser.add_argument('--num-smart-pedestrians', type=int, default=6, help='行人数') - parser.add_argument('--num-background-vehicles', type=int, default=4, help='背景车辆数') - parser.add_argument('--total-duration', type=int, default=90, help='总时长(秒)') - parser.add_argument('--capture-interval', type=float, default=2.5, help='捕捉间隔(秒)') + + # 交通参数 + parser.add_argument('--num-vehicles', type=int, default=8, help='背景车辆数') + parser.add_argument('--num-pedestrians', type=int, default=6, help='行人数') + + # 收集参数 + parser.add_argument('--duration', type=int, default=60, help='收集时长(秒)') + parser.add_argument('--capture-interval', type=float, default=2.0, help='捕捉间隔(秒)') + parser.add_argument('--seed', type=int, help='随机种子') + + # 传感器参数 + parser.add_argument('--enable-lidar', action='store_true', help='启用LiDAR传感器') + parser.add_argument('--enable-fusion', action='store_true', help='启用多传感器融合') + parser.add_argument('--enable-annotations', action='store_true', help='启用自动标注') + + # 功能参数 + parser.add_argument('--run-analysis', action='store_true', help='运行数据集分析') + parser.add_argument('--skip-validation', action='store_true', help='跳过数据验证') args = parser.parse_args(remaining_argv) - if args.capture_interval < 2.0: - args.capture_interval = 2.0 + # 加载配置 + config = ConfigManager.load_config(args.config) + config = ConfigManager.merge_args(config, args) - print(f"\n场景配置:") - print(f" 场景: {args.scenario}") - print(f" 地图: {args.town}") - print(f" 天气/时间: {args.weather}/{args.time_of_day}") - print(f" 行人: {args.num_smart_pedestrians}个") - print(f" 车辆: {args.num_background_vehicles}辆") - print(f" 时长: {args.total_duration}秒") - print(f" 间隔: {args.capture_interval}秒") + # 显示配置 + print("\n" + "=" * 60) + print("CVIPS 多传感器数据收集系统 v9.0") + print("=" * 60) - generator = DataGenerator(args) + 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"传感器:") + 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" 标注: {'启用' if config['output']['save_annotations'] else '禁用'}") + + collector = DataCollector(config) try: - if not generator.connect_to_server(): + if not collector.connect(): + print("连接CARLA服务器失败") return - ego_vehicle = generator.setup_scene() - if not ego_vehicle: - generator.cleanup() + if not collector.setup_scene(): + print("场景设置失败") + collector.cleanup() return - if not generator.setup_cameras(ego_vehicle): - generator.cleanup() + if not collector.setup_sensors(): + print("传感器设置失败") + collector.cleanup() return - generator.collect_data() + collector.collect_data() + + collector.run_analysis() + collector.run_validation() except KeyboardInterrupt: - print("\n程序中断") + print("\n程序被用户中断") except Exception as e: print(f"\n运行错误: {e}") traceback.print_exc() finally: - generator.cleanup() - print(f"\n数据保存到: {generator.output_dir}") + collector.cleanup() + print(f"\n数据集已保存到: {collector.output_dir}") if __name__ == "__main__": diff --git a/src/enhance_pedestrian_safety/scene_manager.py b/src/enhance_pedestrian_safety/scene_manager.py new file mode 100644 index 0000000000..dbe424d7d4 --- /dev/null +++ b/src/enhance_pedestrian_safety/scene_manager.py @@ -0,0 +1,245 @@ +import os +import json +import random +import math +import carla + + +class SceneManager: + """场景管理器 - 支持多种预定义场景""" + + # 场景预设配置 + SCENES = { + 'intersection_4way': { + 'description': '四路交叉口场景', + 'pedestrian_behavior': ['crossing', 'walking'], + 'vehicle_density': 0.7, + 'pedestrian_density': 0.8, + 'weather_variations': ['clear', 'cloudy'] + }, + 'highway': { + 'description': '高速公路场景', + 'pedestrian_behavior': [], + 'vehicle_density': 0.9, + 'pedestrian_density': 0.0, + 'weather_variations': ['clear', 'rainy'] + }, + 'urban_street': { + 'description': '城市街道场景', + 'pedestrian_behavior': ['walking', 'crossing', 'waiting'], + 'vehicle_density': 0.6, + 'pedestrian_density': 0.7, + 'weather_variations': ['clear', 'cloudy', 'rainy'] + }, + 'night_scene': { + 'description': '夜间场景', + 'pedestrian_behavior': ['walking'], + 'vehicle_density': 0.5, + 'pedestrian_density': 0.4, + 'weather_variations': ['clear'], + 'time_of_day': 'night' + }, + 'rainy_intersection': { + 'description': '雨天交叉口', + 'pedestrian_behavior': ['crossing'], + 'vehicle_density': 0.6, + 'pedestrian_density': 0.5, + 'weather_variations': ['rainy'] + } + } + + @staticmethod + def setup_scene(world, config, scene_type='intersection_4way'): + """设置特定场景""" + if scene_type not in SceneManager.SCENES: + print(f"未知场景类型: {scene_type}") + return config + + scene_config = SceneManager.SCENES[scene_type] + + # 更新配置 + if 'time_of_day' in scene_config: + config['scenario']['time_of_day'] = scene_config['time_of_day'] + + if scene_config['weather_variations']: + config['scenario']['weather'] = random.choice(scene_config['weather_variations']) + + # 调整交通密度 + base_vehicles = config['traffic']['background_vehicles'] + base_pedestrians = config['traffic']['pedestrians'] + + config['traffic']['background_vehicles'] = int(base_vehicles * scene_config['vehicle_density']) + config['traffic']['pedestrians'] = int(base_pedestrians * scene_config['pedestrian_density']) + + # 设置行人行为 + config['traffic']['pedestrian_behaviors'] = scene_config['pedestrian_behavior'] + + # 应用场景特定设置 + SceneManager._apply_scene_specifics(world, scene_type) + + return config + + @staticmethod + def _apply_scene_specifics(world, scene_type): + """应用场景特定的设置""" + try: + if scene_type == 'highway': + # 高速公路场景:设置较高车速 + for actor in world.get_actors(): + if 'vehicle' in actor.type_id: + try: + actor.enable_constant_velocity(carla.Vector3D(25, 0, 0)) + except: + pass + + elif scene_type == 'night_scene': + # 夜间场景:调整灯光 + for actor in world.get_actors(): + if 'vehicle' in actor.type_id: + try: + light_state = carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam + actor.set_light_state(light_state) + except: + pass + + except Exception as e: + print(f"场景特定设置失败: {e}") + + @staticmethod + def spawn_traffic_cones(world, center_location, num_cones=10): + """生成交通锥桶""" + blueprint_lib = world.get_blueprint_library() + cone_bp = blueprint_lib.find('static.prop.roadcone') + + cones = [] + for i in range(num_cones): + try: + angle = random.uniform(0, 2 * math.pi) + distance = random.uniform(3.0, 8.0) + + location = carla.Location( + x=center_location.x + distance * math.cos(angle), + y=center_location.y + distance * math.sin(angle), + z=center_location.z + 0.2 + ) + + rotation = carla.Rotation(0, random.uniform(0, 360), 0) + transform = carla.Transform(location, rotation) + + cone = world.spawn_actor(cone_bp, transform) + cones.append(cone) + + except Exception as e: + print(f"生成交通锥桶失败: {e}") + + return cones + + @staticmethod + def spawn_construction_barriers(world, center_location, num_barriers=5): + """生成施工障碍物""" + blueprint_lib = world.get_blueprint_library() + barrier_bp = blueprint_lib.find('static.prop.constructionbarrier') + + barriers = [] + for i in range(num_barriers): + try: + angle = random.uniform(0, 2 * math.pi) + distance = random.uniform(5.0, 12.0) + + location = carla.Location( + x=center_location.x + distance * math.cos(angle), + y=center_location.y + distance * math.sin(angle), + z=center_location.z + 0.2 + ) + + rotation = carla.Rotation(0, random.uniform(0, 360), 0) + transform = carla.Transform(location, rotation) + + barrier = world.spawn_actor(barrier_bp, transform) + barriers.append(barrier) + + except Exception as e: + print(f"生成施工障碍物失败: {e}") + + return barriers + + @staticmethod + def setup_traffic_accident(world, center_location, severity='minor'): + """设置交通事故场景""" + blueprint_lib = world.get_blueprint_library() + + # 生成事故车辆 + accident_vehicles = [] + for i in range(2): # 两车事故 + try: + vehicle_bp = random.choice(list(blueprint_lib.filter('vehicle.*'))) + + # 稍微偏移位置,模拟碰撞 + offset_x = random.uniform(-3.0, 3.0) if i == 0 else random.uniform(2.0, 5.0) + offset_y = random.uniform(-2.0, 2.0) if i == 0 else random.uniform(-5.0, -2.0) + + location = carla.Location( + x=center_location.x + offset_x, + y=center_location.y + offset_y, + z=center_location.z + 0.5 + ) + + # 设置碰撞角度 + rotation = carla.Rotation( + 0, + random.uniform(-30, 30) + (90 if i == 1 else 0), + 0 + ) + + transform = carla.Transform(location, rotation) + vehicle = world.spawn_actor(vehicle_bp, transform) + + # 根据严重程度设置车辆损坏 + if severity == 'major': + vehicle.set_light_state(carla.VehicleLightState.Hazard) + + accident_vehicles.append(vehicle) + + except Exception as e: + print(f"生成事故车辆失败: {e}") + + # 生成应急车辆(警车、救护车) + emergency_vehicles = [] + try: + police_bp = blueprint_lib.find('vehicle.dodge.charger_police') + if police_bp: + location = carla.Location( + x=center_location.x - 8.0, + y=center_location.y + 4.0, + z=center_location.z + 0.5 + ) + rotation = carla.Rotation(0, 45, 0) + police = world.spawn_actor(police_bp, carla.Transform(location, rotation)) + police.set_light_state(carla.VehicleLightState.Special1) + emergency_vehicles.append(police) + except: + pass + + return accident_vehicles, emergency_vehicles + + @staticmethod + def save_scene_description(output_dir, scene_type, config, extra_info=None): + """保存场景描述文件""" + scene_info = { + 'scene_type': scene_type, + 'description': SceneManager.SCENES.get(scene_type, {}).get('description', '未知场景'), + 'config': config, + 'created': SceneManager._get_timestamp(), + 'extra_info': extra_info or {} + } + + scene_file = os.path.join(output_dir, "metadata", "scene_description.json") + with open(scene_file, 'w', encoding='utf-8') as f: + json.dump(scene_info, f, indent=2, ensure_ascii=False) + + print(f"场景描述保存: {scene_file}") + + @staticmethod + def _get_timestamp(): + from datetime import datetime + return datetime.now().isoformat() \ No newline at end of file From 8704ed79206674fae74ca61434cdb609f762db50 Mon Sep 17 00:00:00 2001 From: Z-w-7799 <1011069369@qq.com> Date: Thu, 18 Dec 2025 22:29:03 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E5=A4=9A=E8=BD=A6=E8=BE=86=E5=8D=8F?= =?UTF-8?q?=E5=90=8C=E4=B8=8EV2X=E9=80=9A=E4=BF=A1=E6=A8=A1=E6=8B=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config_manager.py | 28 ++ .../data_analyzer.py | 175 ++++++- .../data_validator.py | 204 +++++++- src/enhance_pedestrian_safety/main.py | 299 ++++++++++-- .../multi_vehicle_manager.py | 434 ++++++++++++++++++ .../v2x_communication.py | 328 +++++++++++++ 6 files changed, 1379 insertions(+), 89 deletions(-) create mode 100644 src/enhance_pedestrian_safety/multi_vehicle_manager.py create mode 100644 src/enhance_pedestrian_safety/v2x_communication.py diff --git a/src/enhance_pedestrian_safety/config_manager.py b/src/enhance_pedestrian_safety/config_manager.py index 38622ee18a..499384a98f 100644 --- a/src/enhance_pedestrian_safety/config_manager.py +++ b/src/enhance_pedestrian_safety/config_manager.py @@ -36,6 +36,23 @@ 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 +60,8 @@ 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 +105,15 @@ 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.enable_lidar: config['sensors']['lidar_sensors'] = 1 config['output']['save_lidar'] = True @@ -96,6 +121,9 @@ 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..069d6d2ec7 100644 --- a/src/enhance_pedestrian_safety/data_analyzer.py +++ b/src/enhance_pedestrian_safety/data_analyzer.py @@ -17,6 +17,7 @@ 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,18 +78,21 @@ def _analyze_file_distribution(data_dir): distribution = {} # 分析原始图像 - raw_dirs = ['vehicle', 'infrastructure'] + 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) - if os.path.exists(path): - 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 + 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 # 分析拼接图像 stitched_dir = os.path.join(data_dir, "stitched") @@ -103,6 +107,19 @@ 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,7 +128,7 @@ 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': {}} + return {'total_objects': 0, 'by_class': {}, 'by_frame': {}, 'class_distribution': {}} object_stats = { 'total_objects': 0, @@ -173,6 +190,78 @@ 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): """计算质量指标""" @@ -180,20 +269,22 @@ def _calculate_quality_metrics(data_dir): 'completeness_score': 0, 'consistency_score': 0, 'diversity_score': 0, + 'cooperative_score': 0, 'issues_found': [] } # 检查完整性 - required_dirs = ["raw/vehicle", "raw/infrastructure", "stitched", "metadata"] + required_dirs = ["raw/vehicle", "raw/infrastructure", "stitched", "metadata", "cooperative"] missing_dirs = [] for dir_path in required_dirs: - if not os.path.exists(os.path.join(data_dir, dir_path)): + full_path = os.path.join(data_dir, dir_path) + if not os.path.exists(full_path): missing_dirs.append(dir_path) if missing_dirs: quality_metrics['issues_found'].append(f"缺失目录: {missing_dirs}") - quality_metrics['completeness_score'] = 50 + quality_metrics['completeness_score'] = 100 - (len(missing_dirs) * 20) else: quality_metrics['completeness_score'] = 100 @@ -209,7 +300,7 @@ def _calculate_quality_metrics(data_dir): camera_counts.append(len(images)) if camera_counts: - max_diff = max(camera_counts) - min(camera_counts) + max_diff = max(camera_counts) - min(camera_counts) if camera_counts else 0 if max_diff > 5: quality_metrics['issues_found'].append(f"摄像头图像数量不一致: 差异{max_diff}") quality_metrics['consistency_score'] = 70 @@ -228,16 +319,27 @@ 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.3, - 'consistency': 0.25, - 'diversity': 0.25, - 'temporal': 0.2 + 'completeness': 0.25, + 'consistency': 0.20, + 'diversity': 0.15, + 'cooperative': 0.20, + 'temporal': 0.20 } quality = analysis['quality_metrics'] @@ -245,7 +347,8 @@ def _calculate_overall_score(analysis): score = ( quality['completeness_score'] * weights['completeness'] + quality['consistency_score'] * weights['consistency'] + - quality['diversity_score'] * weights['diversity'] + quality['diversity_score'] * weights['diversity'] + + quality['cooperative_score'] * weights['cooperative'] ) # 时间因素 @@ -257,7 +360,7 @@ def _calculate_overall_score(analysis): else: score += 5 * weights['temporal'] - return round(score, 1) + return round(min(score, 100), 1) @staticmethod def _save_analysis_report(data_dir, analysis): @@ -292,7 +395,11 @@ def _print_analysis_summary(analysis): if isinstance(value, dict): print(f" {key}:") for subkey, subvalue in value.items(): - print(f" {subkey}: {subvalue}") + if isinstance(subvalue, dict): + for subsubkey, subsubvalue in subvalue.items(): + print(f" {subsubkey}: {subsubvalue}") + else: + print(f" {subkey}: {subvalue}") else: print(f" {key}: {value}") @@ -305,6 +412,22 @@ 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}秒") @@ -315,6 +438,7 @@ def _print_analysis_summary(analysis): print(f" 完整性: {quality['completeness_score']}/100") 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'])}):") @@ -325,11 +449,12 @@ def _print_analysis_summary(analysis): print(f"\n总体评分: {analysis['overall_score']}/100") - if analysis['overall_score'] >= 90: + overall_score = analysis['overall_score'] + if overall_score >= 90: print("✓ 数据集质量优秀") - elif analysis['overall_score'] >= 75: + elif overall_score >= 75: print("✓ 数据集质量良好") - elif analysis['overall_score'] >= 60: + elif 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..fa67f6425b 100644 --- a/src/enhance_pedestrian_safety/data_validator.py +++ b/src/enhance_pedestrian_safety/data_validator.py @@ -14,7 +14,9 @@ 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) + 'lidar_data': DataValidator._validate_lidar_data(data_dir), + 'cooperative_data': DataValidator._validate_cooperative_data(data_dir), + 'fusion_data': DataValidator._validate_fusion_data(data_dir) } validation_results['overall_score'] = DataValidator._calculate_score(validation_results) @@ -30,7 +32,11 @@ def _check_directory_structure(data_dir): "raw/vehicle", "raw/infrastructure", "stitched", - "metadata" + "metadata", + "cooperative", + "cooperative/v2x_messages", + "cooperative/shared_perception", + "fusion" ] missing_dirs = [] @@ -51,14 +57,17 @@ def _check_directory_structure(data_dir): @staticmethod def _validate_raw_images(data_dir): - raw_dirs = ["vehicle", "infrastructure"] + 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))] 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): @@ -143,6 +152,13 @@ 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 +217,152 @@ 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_files = 0 + valid_bin_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_files += 1 + valid_bin_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 +372,21 @@ def _validate_lidar_data(data_dir): return { 'status': status, - 'count': valid_files, + 'count': len(json_files), 'errors': errors } @staticmethod def _calculate_score(results): weights = { - 'directory_structure': 0.15, - 'raw_images': 0.25, - 'stitched_images': 0.15, - 'annotations': 0.1, - 'metadata': 0.15, - 'lidar_data': 0.2 + '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 } score = 0 @@ -280,13 +431,34 @@ def _print_validation_report(results): print(f"\n{key.replace('_', ' ').title()}:") if 'status' in result: - print(f" 状态: {result['status']}") + status_icon = '✓' if result['status'] == 'PASS' else '⚠' if result['status'] == 'WARNING' else '✗' + print(f" 状态: {status_icon} {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..2f855d36cc 100644 --- a/src/enhance_pedestrian_safety/main.py +++ b/src/enhance_pedestrian_safety/main.py @@ -16,6 +16,8 @@ from scene_manager import SceneManager 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 +256,14 @@ 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_cams = self._setup_vehicle_cameras(vehicle) + def setup_cameras(self, vehicle, center_location, vehicle_id=0): + vehicle_cams = self._setup_vehicle_cameras(vehicle, vehicle_id) 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): + def _setup_vehicle_cameras(self, vehicle, vehicle_id): if not vehicle: return 0 @@ -274,7 +276,7 @@ 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'): + if self._create_camera(cam_name, config_data, vehicle, 'vehicle', vehicle_id): installed += 1 return installed @@ -304,7 +306,7 @@ def _setup_infrastructure_cameras(self, center_location): return installed - def setup_lidar(self, vehicle): + def setup_lidar(self, vehicle, vehicle_id=0): if not vehicle or not self.config['sensors'].get('lidar_sensors', 0) > 0: return 0 @@ -319,7 +321,6 @@ 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 +338,9 @@ 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: - # 取第一个摄像头的图像 for cam_name, img_path in self.vehicle_buffer.items(): if os.path.exists(img_path): vehicle_image_path = img_path @@ -366,7 +365,8 @@ def lidar_callback(lidar_data): except Exception as e: print(f"LiDAR安装失败: {e}") return 0 - def _create_camera(self, name, config, parent, sensor_type): + + def _create_camera(self, name, config, parent, sensor_type, vehicle_id=0): try: blueprint = self.world.get_blueprint_library().find('sensor.camera.rgb') @@ -384,10 +384,15 @@ def _create_camera(self, name, config, parent, sensor_type): else: camera = self.world.spawn_actor(blueprint, transform) - save_dir = os.path.join(self.data_dir, "raw", sensor_type, name) + # 为不同车辆创建不同目录 + 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) + callback = self._create_callback(save_dir, name, sensor_type, vehicle_id) camera.listen(callback) self.sensors.append(camera) @@ -397,7 +402,7 @@ 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): + def _create_callback(self, save_dir, name, sensor_type, vehicle_id=0): capture_interval = self.config['sensors']['capture_interval'] def callback(image): @@ -414,7 +419,8 @@ 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, 'vehicle') + self.image_processor.stitch(self.vehicle_buffer, self.frame_counter, + f'vehicle_{vehicle_id}') self.vehicle_buffer.clear() else: self.infra_buffer[name] = filename @@ -459,13 +465,15 @@ def __init__(self, config): self.config = config self.client = None self.world = None - self.ego_vehicle = None + self.ego_vehicles = [] # 多个主车 self.scene_center = None self.setup_directories() self.traffic_manager = None - self.sensor_manager = None + self.sensor_managers = {} # 车辆ID -> SensorManager + self.multi_vehicle_manager = None + self.v2x_communication = None self.start_time = None self.is_running = False @@ -481,12 +489,15 @@ def setup_directories(self): ) directories = [ - "raw/vehicle", + "raw/vehicle_1", # 主车1 + "raw/vehicle_2", # 主车2(如果有) "raw/infrastructure", "stitched", "lidar", "fusion", "calibration", + "cooperative", + "v2x_messages", "metadata" ] @@ -531,26 +542,74 @@ def setup_scene(self): self.traffic_manager = TrafficManager(self.world, self.config) - self.ego_vehicle = self.traffic_manager.spawn_ego_vehicle() - if not self.ego_vehicle: + # 生成多个主车 + 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: 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): - self.sensor_manager = SensorManager(self.world, self.config, self.output_dir) + # 为每个主车设置传感器 + for i, vehicle in enumerate(self.ego_vehicles): + sensor_manager = SensorManager(self.world, self.config, self.output_dir) - cameras = self.sensor_manager.setup_cameras(self.ego_vehicle, self.scene_center) - if cameras == 0: - Log.error("没有摄像头安装成功") - return False + 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") - lidars = self.sensor_manager.setup_lidar(self.ego_vehicle) - Log.info(f"传感器: {cameras}摄像头 + {lidars}LiDAR") + self.sensor_managers[vehicle.id] = sensor_manager return True @@ -562,17 +621,39 @@ def collect_data(self): self.is_running = True 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: - frames = self.sensor_manager.get_frame_count() + 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}%) | 帧数: {frames}") + Log.info(f"进度: {elapsed:.0f}/{duration}秒 ({progress:.1f}%) | 总帧数: {total_frames}") last_update = current_time time.sleep(0.05) @@ -583,17 +664,99 @@ def collect_data(self): self.is_running = False elapsed = time.time() - self.start_time - self.collected_frames = self.sensor_manager.get_frame_count() + self.collected_frames = sum(mgr.get_frame_count() for mgr in self.sensor_managers.values()) 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,9 +765,19 @@ def _save_metadata(self): } } - if self.sensor_manager: - sensor_summary = self.sensor_manager.generate_sensor_summary() - metadata['sensor_summary'] = sensor_summary + # 传感器摘要 + 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() meta_path = os.path.join(self.output_dir, "metadata", "collection_info.json") with open(meta_path, 'w', encoding='utf-8') as f: @@ -640,11 +813,24 @@ def _print_summary(self): total_points += points_in_file print(f" 估计总点数: {total_points:,}") - # 统计融合数据 - 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)} 个同步文件") + # 统计协同数据 + 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)} 协同车") print(f"\n输出目录: {self.output_dir}") print("=" * 60) @@ -662,27 +848,39 @@ def run_analysis(self): def cleanup(self): Log.info("清理场景...") - if self.sensor_manager: - self.sensor_manager.cleanup() + # 清理传感器 + for sensor_manager in self.sensor_managers.values(): + sensor_manager.cleanup() + # 清理交通 if self.traffic_manager: self.traffic_manager.cleanup() - if self.ego_vehicle and self.ego_vehicle.is_alive: - try: - self.ego_vehicle.destroy() - except: - pass + # 清理协同管理 + 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 Log.info("清理完成") def main(): - parser = argparse.ArgumentParser(description='CVIPS 多传感器数据收集系统') + parser = argparse.ArgumentParser(description='CVIPS 多车辆协同数据收集系统 v10.0') # 基础参数 parser.add_argument('--config', type=str, help='配置文件路径') - parser.add_argument('--scenario', type=str, default='multi_sensor_scene', help='场景名称') + parser.add_argument('--scenario', type=str, default='multi_vehicle_cooperative', help='场景名称') parser.add_argument('--town', type=str, default='Town10HD', choices=['Town03', 'Town04', 'Town05', 'Town10HD'], help='地图') parser.add_argument('--weather', type=str, default='clear', @@ -693,6 +891,7 @@ 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='收集时长(秒)') @@ -702,6 +901,8 @@ 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,21 +917,23 @@ def main(): # 显示配置 print("\n" + "=" * 60) - print("CVIPS 多传感器数据收集系统 v9.0") + print("CVIPS 多车辆协同数据收集系统 v10.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['traffic']['background_vehicles']}背景车辆 + {config['traffic']['pedestrians']}行人") + print(f"协同: {config['cooperative']['num_coop_vehicles']} 协同车辆") 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" 标注: {'启用' if config['output']['save_annotations'] else '禁用'}") + print(f" V2X: {'启用' if config['v2x']['enabled'] else '禁用'}") + print(f" 协同: {'启用' if config['output']['save_cooperative'] else '禁用'}") collector = DataCollector(config) 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