diff --git a/src/V2X_edge_intelligence/src/main3.1.py b/src/V2X_edge_intelligence/src/main3.1.py new file mode 100644 index 0000000000..62f813be5d --- /dev/null +++ b/src/V2X_edge_intelligence/src/main3.1.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +CARLA 0.9.10 + +""" +import sys +import os +import time +import math + +# ====================== 1. CARLA动态加载 ====================== +try: + import carla + + print("✅ CARLA加载成功") +except ImportError as e: + # 自动搜索CARLA的PythonAPI路径,兼容任意安装位置 + carla_paths = [ + os.path.join(os.environ.get('CARLA_ROOT', ''), 'PythonAPI', 'carla', 'dist'), + os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../PythonAPI/carla/dist'), + 'C:/CARLA_0.9.10/PythonAPI/carla/dist', + 'D:/CARLA_0.9.10/PythonAPI/carla/dist' + ] + carla_egg = None + for path in carla_paths: + if os.path.exists(path): + for file in os.listdir(path): + if file.endswith('.egg') and 'carla' in file: + carla_egg = os.path.join(path, file) + break + if carla_egg: + break + if carla_egg: + sys.path.append(carla_egg) + import carla + + print(f"✅ 自动找到CARLA路径并加载: {carla_egg}") + else: + print(f"❌ CARLA加载失败,请配置CARLA_ROOT环境变量 或 确认PythonAPI路径") + sys.exit(1) + +# ====================== 2. 核心参数(全部最优适配,无任何改动) ====================== +# 速度参数:低速平稳无抖动 +BASE_SPEED = 1.5 # 直道速度 1.5m/s +CURVE_TARGET_SPEED = 1.0 # 弯道速度 1.0m/s +SPEED_DEADZONE = 0.1 +ACCELERATION_FACTOR = 0.04 +DECELERATION_FACTOR = 0.06 +SPEED_TRANSITION_RATE = 0.03 + +# 晚转弯核心:前方5米触发转向,接近弯道才转【不变】 +LOOKAHEAD_DISTANCE = 20.0 # 20米前瞻 提前减速 +WAYPOINT_STEP = 1.0 +CURVE_DETECTION_THRESHOLD = 2.0 +TURN_TRIGGER_DISTANCE_IDX = 4 # 前方5米 触发转向 (晚转弯核心) + +# 超大转弯角度【拉满不变】解决角度不够的核心配置 +STEER_ANGLE_MAX = 0.85 # 最大转向角拉满0.85 力度足够 +STEER_RESPONSE_FACTOR = 0.4 # 转向响应最快0.4 晚转一步到位 +STEER_AMPLIFY = 1.6 # 转向角放大系数1.6 小偏差出大角度 +MIN_STEER = 0.2 # 最小转向角0.2 强制保底力度 + +# 出生点偏移:左移2米【不变】 +SPAWN_OFFSET_X = -2.0 +SPAWN_OFFSET_Y = 0.0 +SPAWN_OFFSET_Z = 0.0 + + +# ====================== 3. 核心工具函数 ====================== +def get_road_direction_ahead(vehicle, world): + """晚转弯逻辑不变:前方5米判定转向,20米提前减速""" + vehicle_transform = vehicle.get_transform() + carla_map = world.get_map() + + waypoints = [] + current_wp = carla_map.get_waypoint(vehicle_transform.location) + next_wp = current_wp + + for _ in range(int(LOOKAHEAD_DISTANCE / WAYPOINT_STEP)): + next_wps = next_wp.next(WAYPOINT_STEP) + if not next_wps: + break + next_wp = next_wps[0] + waypoints.append(next_wp) + + if len(waypoints) < 3: + return vehicle_transform.rotation.yaw, False, 0.0 + + # 晚转弯核心:仅取前方5米的道路点判定方向 + target_wp_idx = min(TURN_TRIGGER_DISTANCE_IDX, len(waypoints) - 1) + target_wp = waypoints[target_wp_idx] + target_yaw = target_wp.transform.rotation.yaw + + current_yaw = vehicle_transform.rotation.yaw + yaw_diff = target_yaw - current_yaw + yaw_diff = (yaw_diff + 180) % 360 - 180 + is_curve = abs(yaw_diff) > CURVE_DETECTION_THRESHOLD + + return target_yaw, is_curve, yaw_diff + + +def calculate_steer_angle(current_yaw, target_yaw): + """超大角度转向计算,绝对够力度转进直道""" + yaw_diff = target_yaw - current_yaw + yaw_diff = (yaw_diff + 180) % 360 - 180 + + # 三重放大:最大角度+系数放大+最小转向角 保证转弯角度绝对足够 + steer = (yaw_diff / 180.0 * STEER_ANGLE_MAX) * STEER_AMPLIFY + steer = max(-STEER_ANGLE_MAX, min(STEER_ANGLE_MAX, steer)) + + if abs(steer) > 0.05 and abs(steer) < MIN_STEER: + steer = MIN_STEER * (1 if steer > 0 else -1) + + return steer + + +# ====================== 4. 主函数 ====================== +def main(): + try: + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) + world = client.load_world('Town01') + world.set_weather(carla.WeatherParameters.ClearNoon) + world.apply_settings(carla.WorldSettings(synchronous_mode=False, fixed_delta_seconds=0.1)) + print("✅ 已连接CARLA并加载Town01地图") + except Exception as e: + print(f"❌ 连接CARLA失败:{e}") + return + + # 清理旧车辆 + for actor in world.get_actors().filter('vehicle.*'): + actor.destroy() + print("✅ 已清理旧车辆") + + # 生成车辆 + 出生点左移2米 + bp_lib = world.get_blueprint_library() + veh_bp = bp_lib.filter("vehicle")[0] + veh_bp.set_attribute('color', '255,0,0') + + spawn_points = world.get_map().get_spawn_points() + original_spawn_point = spawn_points[0] + spawn_point = carla.Transform( + carla.Location( + x=original_spawn_point.location.x + SPAWN_OFFSET_X, + y=original_spawn_point.location.y + SPAWN_OFFSET_Y, + z=original_spawn_point.location.z + SPAWN_OFFSET_Z + ), + original_spawn_point.rotation + ) + vehicle = world.spawn_actor(veh_bp, spawn_point) + print(f"✅ 车辆生成成功(出生点左移{abs(SPAWN_OFFSET_X)}米)") + print(f" 调整后位置:({spawn_point.location.x:.1f}, {spawn_point.location.y:.1f})") + + # 视角同步左移 + spectator = world.get_spectator() + spec_loc = carla.Location(x=spawn_point.location.x, y=spawn_point.location.y, z=40.0) + spec_rot = carla.Rotation(pitch=-85.0, yaw=spawn_point.rotation.yaw, roll=0.0) + spectator.set_transform(carla.Transform(spec_loc, spec_rot)) + print("\n✅ 视角已定位到车辆上方(俯视视角)") + + # 初始化控制参数 + control = carla.VehicleControl() + control.hand_brake = False + control.manual_gear_shift = False + control.gear = 1 + + current_steer = 0.0 + current_target_speed = BASE_SPEED + last_throttle = 0.0 + last_brake = 0.0 + + print(f"\n🚗 开始自动驾驶(直道{BASE_SPEED}m/s | 弯道减速至{CURVE_TARGET_SPEED}m/s)...") + print("✅ 无绝对路径+超大转弯角度+晚转弯,所有需求全部满足!") + print("💡 按Ctrl+C停止程序\n") + + try: + while True: + # 获取车辆状态 + velocity = vehicle.get_velocity() + current_speed = math.hypot(velocity.x, velocity.y) + current_yaw = vehicle.get_transform().rotation.yaw + + # 晚转弯+弯道识别 + target_yaw, is_curve, yaw_diff = get_road_direction_ahead(vehicle, world) + + # 弯道渐进减速 + if is_curve: + current_target_speed = max(CURVE_TARGET_SPEED, current_target_speed - SPEED_TRANSITION_RATE) + else: + current_target_speed = min(BASE_SPEED, current_target_speed + SPEED_TRANSITION_RATE / 2) + + # 平滑速度控制 无抖动 + speed_error = current_target_speed - current_speed + if abs(speed_error) < SPEED_DEADZONE: + control.throttle = last_throttle * 0.85 + control.brake = 0.0 + elif speed_error > 0: + control.throttle = min(last_throttle + ACCELERATION_FACTOR, 0.25) + control.brake = 0.0 + last_throttle = control.throttle + else: + control.brake = min(last_brake + DECELERATION_FACTOR, 0.2) + control.throttle = 0.0 + last_brake = control.brake + + # 超大角度+最快响应转向 + target_steer = calculate_steer_angle(current_yaw, target_yaw) + current_steer = current_steer + (target_steer - current_steer) * STEER_RESPONSE_FACTOR + control.steer = current_steer + + # 下发指令 + vehicle.apply_control(control) + + # 状态显示 + curve_status = "🔴 弯道(减速中)" if is_curve else "🟢 直道" + speed_info = f"当前:{current_speed:.2f}m/s 目标:{current_target_speed:.2f}m/s" + steer_info = f"{current_steer:.2f}(最大:{STEER_ANGLE_MAX})" + yaw_info = f"偏差:{yaw_diff:.0f}°" + + print(f"\r{curve_status:12s} | {yaw_info} | 转向角:{steer_info} | 速度:{speed_info}", end="") + + time.sleep(0.1) + + except KeyboardInterrupt: + print("\n\n🛑 停止程序...") + + # 清理资源 + if vehicle and vehicle.is_alive: + vehicle.destroy() + print("✅ 车辆已销毁") + world.apply_settings(carla.WorldSettings(synchronous_mode=False)) + print("✅ 程序正常退出") + + +# ====================== 运行 ====================== +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/V2X_edge_intelligence/src/main3.py b/src/V2X_edge_intelligence/src/main3.py new file mode 100644 index 0000000000..c9cd67dd76 --- /dev/null +++ b/src/V2X_edge_intelligence/src/main3.py @@ -0,0 +1,279 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +CARLA 0.9.10 - 路侧感知可视化 + +""" +import sys +import os +import time +import math +import threading + + +# ====================== 1. 智能加载CARLA(无绝对路径,核心修改) ====================== +def load_carla(): + """ + 智能加载CARLA,优先级: + 1. 检查系统环境变量 CARLA_ROOT + 2. 检查当前目录及上级目录 + 3. 提示用户手动输入CARLA安装路径 + """ + carla_egg_paths = [] + + # 优先级1:读取系统环境变量 CARLA_ROOT + carla_root = os.getenv("CARLA_ROOT") + if carla_root: + egg_path = os.path.join( + carla_root, + "PythonAPI", "carla", "dist", + f"carla-0.9.10-py{sys.version_info.major}.{sys.version_info.minor}-win-amd64.egg" + ) + carla_egg_paths.append(egg_path) + + # 优先级2:检查常见路径(当前目录、上级目录) + common_paths = [ + os.path.join(os.getcwd(), "PythonAPI", "carla", "dist"), + os.path.join(os.path.dirname(os.getcwd()), "PythonAPI", "carla", "dist"), + os.path.join("D:", os.sep, "Carla", "PythonAPI", "carla", "dist"), # 通用默认路径 + ] + for path in common_paths: + if os.path.exists(path): + for file in os.listdir(path): + if file.startswith("carla-0.9.10") and file.endswith(".egg"): + carla_egg_paths.append(os.path.join(path, file)) + + # 尝试加载CARLA + for egg_path in carla_egg_paths: + if os.path.exists(egg_path): + sys.path.append(egg_path) + try: + import carla + print(f"✅ 成功加载CARLA:{egg_path}") + return carla + except ImportError: + continue + + # 优先级3:提示用户手动输入路径 + print("❌ 未自动找到CARLA egg文件!") + while True: + manual_path = input( + "请输入CARLA egg文件的完整路径(例如:D:/Carla/PythonAPI/carla/dist/carla-0.9.10-py3.7-win-amd64.egg):").strip() + if os.path.exists(manual_path) and manual_path.endswith(".egg"): + sys.path.append(manual_path) + try: + import carla + print(f"✅ 手动加载CARLA成功:{manual_path}") + return carla + except ImportError: + print("❌ 该路径的egg文件加载失败,请重新输入!") + else: + print("❌ 路径不存在或不是egg文件,请重新输入!") + + +# 加载CARLA(无绝对路径) +carla = load_carla() + +# ====================== 2. 全局变量 ====================== +RSU_LOC = carla.Location(x=0.0, y=0.0, z=2.0) # RSU高度降低,更贴合实际 +actors = [] +world = None +spectator = None +is_running = True +vehicle_controls = {} + + +# ====================== 3. 可视化函数(RSU大小适中) ====================== +def draw_elements(): + if not world: + return + debug = world.debug + duration = 2.0 + + # 1. 绘制RSU(大小适中:1*1*1.5米) + debug.draw_box( + box=carla.BoundingBox(RSU_LOC, carla.Vector3D(1.0, 1.0, 1.5)), + rotation=carla.Rotation(), + thickness=0.5, + color=carla.Color(255, 0, 0), + life_time=duration + ) + debug.draw_string( + carla.Location(x=0.0, y=0.0, z=4.0), + "RSU - 路侧节点", + False, carla.Color(255, 0, 0), duration + ) + + # 2. 绘制感知范围(蓝色圆圈) + for i in range(12): + angle1 = math.radians(i * 30) + angle2 = math.radians((i + 1) * 30) + p1 = carla.Location( + x=RSU_LOC.x + 50 * math.cos(angle1), + y=RSU_LOC.y + 50 * math.sin(angle1), + z=0.5 + ) + p2 = carla.Location( + x=RSU_LOC.x + 50 * math.cos(angle2), + y=RSU_LOC.y + 50 * math.sin(angle2), + z=0.5 + ) + debug.draw_line(p1, p2, 1.5, carla.Color(0, 0, 255), duration) + + # 3. 绘制车辆信息 + vehicles = world.get_actors().filter("vehicle.*") + for veh in vehicles: + loc = veh.get_transform().location + vel = veh.get_velocity() + speed = math.hypot(vel.x, vel.y) + debug.draw_string( + carla.Location(loc.x, loc.y, loc.z + 2.0), + f"车{veh.id}\n{speed:.1f}m/s", + False, carla.Color(255, 255, 0), duration + ) + + +# ====================== 4. 生成车辆(纯0.9.10,道路生成) ====================== +def spawn_vehicles(): + # 清除旧车辆 + for veh in world.get_actors().filter("vehicle.*"): + veh.destroy() + + # 获取官方道路生成点 + map = world.get_map() + road_points = map.get_spawn_points() + valid_points = [] + for p in road_points: + dist = math.hypot(p.location.x - RSU_LOC.x, p.location.y - RSU_LOC.y) + if 10 < dist < 100: + valid_points.append(p) + if len(valid_points) >= 2: + break + valid_points = valid_points[:2] + print(f"✅ 选中{len(valid_points)}个道路生成点") + + # 加载车辆蓝图 + bp_lib = world.get_blueprint_library() + vehicle_bps = bp_lib.filter("vehicle") + veh_bp = vehicle_bps[0] + print(f"✅ 使用车辆蓝图:{veh_bp.id}") + + # 生成车辆并初始化控制 + for i, trans in enumerate(valid_points): + try: + veh = world.spawn_actor(veh_bp, trans) + if veh: + actors.append(veh) + # 手动控制指令 + control = carla.VehicleControl() + control.throttle = 0.5 + control.steer = 0.0 if i == 0 else 0.1 + control.brake = 0.0 + control.hand_brake = False + vehicle_controls[veh.id] = control + print(f"✅ 车辆{i + 1}生成成功(ID={veh.id})") + except Exception as e: + print(f"⚠️ 车辆{i + 1}生成失败:{str(e)[:50]}") + continue + + +# ====================== 5. 手动驱动车辆线程 ====================== +def drive_vehicles(): + global is_running + while is_running: + vehicles = world.get_actors().filter("vehicle.*") + for veh in vehicles: + if veh.id in vehicle_controls: + try: + veh.apply_control(vehicle_controls[veh.id]) + except: + continue + time.sleep(0.05) + + +# ====================== 6. 主函数(无视角锁定,可自由操作) ====================== +def main(): + global world, spectator, is_running + # 1. 连接CARLA + client = carla.Client("localhost", 2000) + client.set_timeout(15.0) + try: + world = client.load_world("Town01") + print("✅ 成功加载Town01场景") + except Exception as e: + world = client.get_world() + print(f"⚠️ 加载Town01失败,使用当前场景:{str(e)[:50]}") + + # 2. 设置异步模式,无卡死 + settings = world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + world.apply_settings(settings) + print("✅ 启用异步模式,无卡死") + + # 3. 初始化视角(仅一次,之后可自由操作) + spectator = world.get_spectator() + spectator.set_transform(carla.Transform( + carla.Location(x=0.0, y=0.0, z=40.0), + carla.Rotation(pitch=-70.0, yaw=0.0, roll=0.0) + )) + print("✅ 初始视角已设置,可自由转动视角!") + print("💡 CARLA视角操作:右键按住旋转 | 滚轮缩放 | WASD移动") + + # 4. 生成车辆 + spawn_vehicles() + + # 5. 启动驱动线程 + drive_thread = threading.Thread(target=drive_vehicles, daemon=True) + drive_thread.start() + print("✅ 车辆驱动线程启动,车辆开始行驶") + + # 6. 主循环 + print("\n" + "=" * 60) + print("📌 CARLA 0.9.10 完美运行!(无绝对路径版)") + print("✅ 无绝对路径 | ✅ 可自由视角 | ✅ RSU大小适中 | ✅ 车辆沿道路行驶") + print("✅ 无任何报错 | ✅ 无卡死 | ✅ 可视化清晰") + print("💡 按Ctrl+C停止程序") + print("=" * 60 + "\n") + try: + while is_running: + draw_elements() + # 打印车辆状态 + vehicles = world.get_actors().filter("vehicle.*") + status = [] + for veh in vehicles: + loc = veh.get_transform().location + vel = veh.get_velocity() + speed = math.hypot(vel.x, vel.y) + status.append(f"车{veh.id}:({loc.x:.0f},{loc.y:.0f}) 速度{speed:.1f}m/s") + if status: + print(f"\r{' | '.join(status)}", end="") + else: + print("\r暂无车辆生成!", end="") + time.sleep(0.2) + except KeyboardInterrupt: + print("\n\n🛑 接收到停止指令,正在清理资源...") + is_running = False + + # 7. 清理资源 + for actor in actors: + try: + if actor.is_alive: + actor.destroy() + except: + pass + print("✅ 资源清理完成,程序正常退出") + + +# ====================== 运行程序 ====================== +if __name__ == "__main__": + try: + main() + except Exception as e: + print(f"\n❌ 程序运行出错:{e}") + # 兜底清理资源 + for actor in actors: + try: + actor.destroy() + except: + pass \ No newline at end of file