diff --git a/src/V2X_edge_intelligence/src/main4.py b/src/V2X_edge_intelligence/src/main4.py new file mode 100644 index 0000000000..46d23d8f78 --- /dev/null +++ b/src/V2X_edge_intelligence/src/main4.py @@ -0,0 +1,229 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +CARLA 0.9.10 +""" +import sys +import os +import time +import math + +# ====================== 1. CARLA加载(兼容相对路径,无硬编码绝对路径) ====================== +# 优先从项目内carla_lib加载,无则回退到默认路径(保证兼容性) +CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) +CARLA_LIB_DIR = os.path.join(CURRENT_DIR, "../carla_lib") # 上级目录carla_lib +DEFAULT_CARLA_PATH = "D:/WindowsNoEditor" + +# 自动查找egg文件 +egg_file = None +# 尝试项目内carla_lib +if os.path.exists(CARLA_LIB_DIR): + egg_files = [f for f in os.listdir(CARLA_LIB_DIR) if f.endswith(".egg") and "0.9.10" in f] + if egg_files: + egg_file = os.path.join(CARLA_LIB_DIR, egg_files[0]) +# 回退到默认路径 +if not egg_file: + egg_file = os.path.join(DEFAULT_CARLA_PATH, "PythonAPI", "carla", "dist", "carla-0.9.10-py3.7-win-amd64.egg") + +try: + sys.path.append(egg_file) + import carla + print(f"✅ CARLA加载成功(egg路径:{egg_file})") +except Exception as e: + print(f"❌ CARLA加载失败:{e}") + print("请确保:1. CARLA egg文件存在 2. Python版本为3.7") + 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【重中之重】之前0.7,角度大幅增大 +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/main7.py b/src/V2X_edge_intelligence/src/main7.py new file mode 100644 index 0000000000..8086db9eb2 --- /dev/null +++ b/src/V2X_edge_intelligence/src/main7.py @@ -0,0 +1,271 @@ +# main.py(CARLA V2X三区均衡变速测试 - 唯一入口+无绝对路径) +import sys +import os +import time +import json +import math + +# ===================== 1. 自动适配CARLA路径(无绝对路径) ===================== +def setup_carla_path(): + """ + 自动配置CARLA PythonAPI路径(优先级:环境变量 > 相对路径 > 手动输入) + 彻底移除硬编码绝对路径,适配不同环境/安装位置 + """ + # 优先级1:读取系统环境变量(推荐长期使用) + carla_api_env = os.environ.get("CARLA_PYTHON_API_PATH") + if carla_api_env and os.path.exists(carla_api_env): + egg_files = [f for f in os.listdir(carla_api_env) if f.endswith(".egg")] + if egg_files: + carla_egg = os.path.join(carla_api_env, egg_files[0]) + print(f"🔍 从环境变量加载CARLA egg:{carla_egg}") + sys.path.insert(0, carla_egg) + return True + + # 优先级2:自动查找常见相对路径(适配多数用户目录结构) + common_relative_paths = [ + "./PythonAPI/carla/dist", # 当前目录下的CARLA API + "../WindowsNoEditor/PythonAPI/carla/dist", # 上级目录的CARLA + "./WindowsNoEditor/PythonAPI/carla/dist" # 当前目录的CARLA + ] + for path in common_relative_paths: + if os.path.exists(path): + egg_files = [f for f in os.listdir(path) if f.endswith(".egg")] + if egg_files: + carla_egg = os.path.join(path, egg_files[0]) + print(f"🔍 自动找到CARLA egg:{carla_egg}") + sys.path.insert(0, carla_egg) + return True + + # 优先级3:提示用户手动输入(兜底方案) + print("\n⚠️ 未自动识别CARLA PythonAPI路径!") + print("📌 请先配置环境变量(推荐):") + print(" Windows: set CARLA_PYTHON_API_PATH=你的CARLA路径\\PythonAPI\\carla\\dist") + print(" Linux/Mac: export CARLA_PYTHON_API_PATH=你的CARLA路径/PythonAPI/carla/dist") + manual_path = input("\n请输入CARLA egg文件所在目录(留空退出):").strip() + if manual_path and os.path.exists(manual_path): + egg_files = [f for f in os.listdir(manual_path) if f.endswith(".egg")] + if egg_files: + carla_egg = os.path.join(manual_path, egg_files[0]) + sys.path.insert(0, carla_egg) + print(f"✅ 手动加载CARLA egg:{carla_egg}") + return True + + return False + +# 初始化CARLA路径(无绝对路径) +print(f"🔍 当前Python解释器路径:{sys.executable}") +print(f"🔍 当前Python版本:{sys.version.split()[0]}") + +if not setup_carla_path(): + print("\n❌ 无法加载CARLA PythonAPI,请检查路径配置!") + sys.exit(1) + +# 导入CARLA(适配0.9.10+版本) +try: + import carla + print("✅ CARLA模块导入成功!") +except Exception as e: + print(f"\n❌ CARLA导入失败:{str(e)}") + sys.exit(1) + +# ===================== 2. 核心逻辑:三区均衡分配+低速精准控速 ===================== +class RoadSideUnit: + def __init__(self, carla_world, vehicle): + self.world = carla_world + self.vehicle = vehicle + # 三区等距坐标(基于车辆生成位置动态计算,无绝对坐标) + spawn_loc = vehicle.get_location() + # 高速区:生成位置前5-15米(长度10米) + self.high_zone_start = carla.Location(spawn_loc.x, spawn_loc.y + 5, spawn_loc.z) + self.high_zone_end = carla.Location(spawn_loc.x, spawn_loc.y + 15, spawn_loc.z) + # 中速区:生成位置前15-25米(长度10米) + self.mid_zone_start = carla.Location(spawn_loc.x, spawn_loc.y + 15, spawn_loc.z) + self.mid_zone_end = carla.Location(spawn_loc.x, spawn_loc.y + 25, spawn_loc.z) + # 低速区:生成位置前25-35米(长度10米) + self.low_zone_start = carla.Location(spawn_loc.x, spawn_loc.y + 25, spawn_loc.z) + self.low_zone_end = carla.Location(spawn_loc.x, spawn_loc.y + 35, spawn_loc.z) + + # 三区计时逻辑(确保每区停留10秒) + self.current_zone = "high" # 初始区:高速 + self.zone_start_time = time.time() + self.zone_duration = 10 # 每区停留10秒(30秒测试周期) + self.speed_map = {"high": 40, "mid": 25, "low": 10} + + def get_balance_speed_limit(self): + """计时+位置双重判断,确保三区平均分配""" + current_time = time.time() + vehicle_loc = self.vehicle.get_location() + vehicle_y = vehicle_loc.y + spawn_y = self.vehicle.get_location().y + + # 1. 计时强制切换:每区停留10秒必切换 + if current_time - self.zone_start_time > self.zone_duration: + zone_switch = {"high": "mid", "mid": "low", "low": "high"} + self.current_zone = zone_switch[self.current_zone] + self.zone_start_time = current_time + + # 2. 位置验证:确保区域与物理位置匹配 + if spawn_y + 5 <= vehicle_y < spawn_y + 15: + self.current_zone = "high" + elif spawn_y + 15 <= vehicle_y < spawn_y + 25: + self.current_zone = "mid" + elif spawn_y + 25 <= vehicle_y < spawn_y + 35: + self.current_zone = "low" + + # 返回速度和区域名称 + speed_limit = self.speed_map[self.current_zone] + zone_name = { + "high": "高速区(40km/h)", + "mid": "中速区(25km/h)", + "low": "低速区(10km/h)" + }[self.current_zone] + return speed_limit, zone_name + + def send_speed_command(self, vehicle_id, speed_limit, zone_type): + command = { + "vehicle_id": vehicle_id, + "speed_limit_kmh": speed_limit, + "zone_type": zone_type, + "timestamp": time.time() + } + print(f"\n📡 路侧V2X指令:{json.dumps(command, indent=2, ensure_ascii=False)}") + return command + +class VehicleUnit: + def __init__(self, vehicle): + self.vehicle = vehicle + self.vehicle.set_autopilot(False) + self.control = carla.VehicleControl() + self.control.steer = 0.0 # 强制直行 + self.control.hand_brake = False + print("✅ 车辆已设置为手动直行(三区精准控速)") + + def get_actual_speed(self): + """计算车辆实际速度(km/h)""" + velocity = self.vehicle.get_velocity() + speed_kmh = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) * 3.6 + return round(speed_kmh, 1) + + def precise_speed_control(self, target_speed): + """三区精准控速,低速区加大油门确保到10km/h""" + actual_speed = self.get_actual_speed() + + # 高速区:38-42km/h + if target_speed == 40: + if actual_speed > 42: + self.control.throttle = 0.0 + self.control.brake = 0.4 + elif actual_speed < 38: + self.control.throttle = 0.9 + self.control.brake = 0.0 + else: + self.control.throttle = 0.2 + self.control.brake = 0.0 + + # 中速区:23-27km/h + elif target_speed == 25: + if actual_speed > 27: + self.control.throttle = 0.0 + self.control.brake = 0.3 + elif actual_speed < 23: + self.control.throttle = 0.6 + self.control.brake = 0.0 + else: + self.control.throttle = 0.1 + self.control.brake = 0.0 + + # 低速区:9-11km/h(0.4油门确保速度达标) + elif target_speed == 10: + if actual_speed > 11: + self.control.throttle = 0.0 + self.control.brake = 0.2 + elif actual_speed < 9: + self.control.throttle = 0.4 # 加大油门确保到10km/h + self.control.brake = 0.0 + else: + self.control.throttle = 0.15 + self.control.brake = 0.0 + + self.vehicle.apply_control(self.control) + return actual_speed + + def receive_speed_command(self, command): + target_speed = command["speed_limit_kmh"] + actual_speed = self.precise_speed_control(target_speed) + print( + f"🚗 车载执行:目标{target_speed}km/h → 实际{actual_speed}km/h | 油门={round(self.control.throttle, 1)} 刹车={round(self.control.brake, 1)}") + +# ===================== 3. 近距离视角配置 ===================== +def set_near_observation_view(world, vehicle): + """设置车辆后方近距离视角(无绝对坐标)""" + spectator = world.get_spectator() + vehicle_transform = vehicle.get_transform() + forward_vector = vehicle_transform.rotation.get_forward_vector() + right_vector = vehicle_transform.rotation.get_right_vector() + view_location = vehicle_transform.location - forward_vector * 8 + right_vector * 2 + carla.Location(z=2) + view_rotation = carla.Rotation(pitch=-15, yaw=vehicle_transform.rotation.yaw, roll=0) + spectator.set_transform(carla.Transform(view_location, view_rotation)) + print("✅ 初始视角已设置:车辆后方近距离") + print("📌 视角操作:鼠标拖拽=旋转 | 滚轮=缩放 | WASD=移动") + +def get_valid_spawn_point(world): + """获取道路有效生成点(无绝对坐标)""" + spawn_points = world.get_map().get_spawn_points() + valid_spawn = spawn_points[10] if len(spawn_points) >= 10 else spawn_points[5] + print(f"✅ 车辆生成位置:(x={valid_spawn.location.x:.1f}, y={valid_spawn.location.y:.1f})") + return valid_spawn + +# ===================== 4. 主入口逻辑(唯一入口) ===================== +def main(): + # 1. 连接CARLA服务器(通用提示,无绝对路径) + try: + client = carla.Client('localhost', 2000) + client.set_timeout(20.0) + world = client.get_world() + print(f"\n✅ 连接CARLA成功!服务器版本:{client.get_server_version()}") + except Exception as e: + print(f"\n❌ CARLA服务器连接失败:{str(e)}") + print("📌 请先启动CARLA服务器(通用路径参考):") + print(" Windows: ./WindowsNoEditor/CarlaUE4.exe") + print(" Linux/Mac: ./CarlaUE4.sh") + sys.exit(1) + + # 2. 生成测试车辆(红色车身) + try: + bp_lib = world.get_blueprint_library() + vehicle_bp = bp_lib.filter('vehicle.tesla.model3')[0] + vehicle_bp.set_attribute('color', '255,0,0') + valid_spawn = get_valid_spawn_point(world) + vehicle = world.spawn_actor(vehicle_bp, valid_spawn) + print(f"✅ 车辆生成成功,ID:{vehicle.id}(红色车身)") + except Exception as e: + print(f"\n❌ 车辆生成失败:{str(e)}") + sys.exit(1) + + # 3. 初始化V2X组件+设置视角 + rsu = RoadSideUnit(world, vehicle) + vu = VehicleUnit(vehicle) + set_near_observation_view(world, vehicle) + + # 4. 启动三区均衡测试 + print("\n✅ 开始V2X三区均衡变速测试(30秒)...") + print("📌 高速/中速/低速区各停留10秒,低速精准到10km/h!") + start_time = time.time() + try: + while time.time() - start_time < 30: + speed_limit, zone_type = rsu.get_balance_speed_limit() + command = rsu.send_speed_command(vehicle.id, speed_limit, zone_type) + vu.receive_speed_command(command) + time.sleep(1) # 1秒高频更新,响应更快 + except KeyboardInterrupt: + print("\n⚠️ 用户手动中断测试") + finally: + # 安全停车并销毁车辆 + vehicle.apply_control(carla.VehicleControl(brake=1.0, throttle=0.0, steer=0.0)) + time.sleep(2) + vehicle.destroy() + print("\n✅ 测试结束,车辆已销毁") + +# 唯一入口(确保仅main.py作为脚本运行) +if __name__ == "__main__": + main() \ No newline at end of file