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/main5.py b/src/V2X_edge_intelligence/src/main5.py new file mode 100644 index 0000000000..9029221313 --- /dev/null +++ b/src/V2X_edge_intelligence/src/main5.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +CARLA 0.9.10 车路协同避障 +""" +import sys +import time +import math + +# ====================== 1. 导入CARLA(无绝对路径,依赖环境配置) ====================== +try: + import carla + + print("✅ CARLA模块导入成功!") +except ImportError as e: + print("❌ CARLA模块导入失败!请按以下步骤配置环境:") + print(" 1. 确保CARLA 0.9.10服务器已启动") + print(" 2. 将CARLA安装目录下的PythonAPI路径加入sys.path,示例:") + print(" sys.path.append('/path/to/CARLA_0.9.10/PythonAPI/carla/dist/carla-0.9.10-py3.7-win-amd64.egg')") + print(" 3. 或设置环境变量PYTHONPATH包含上述egg文件路径") + sys.exit(1) +except Exception as e: + print(f"❌ 导入CARLA时发生未知错误:{e}") + sys.exit(1) + +# ====================== 2. 核心参数(远距离停止+渐进减速) ====================== +DECEL_DISTANCE = 20.0 # 距离<20米开始减速(提前缓冲) +STOP_DISTANCE = 12.0 # 距离<12米完全停止(远离蓝车,不撞) +NORMAL_THROTTLE = 0.7 # 正常直行油门 +DECEL_THROTTLE = 0.1 # 减速阶段油门(缓慢靠近) +OBSTACLE_DISTANCE = 25.0 # 蓝车在红车同车道正前方25米(更远初始距离) +BRAKE_FORCE = 1.0 # 满刹车(停止彻底) + + +# ====================== 3. 计算两车距离 ====================== +def calculate_distance(actor1, actor2): + loc1 = actor1.get_transform().location + loc2 = actor2.get_transform().location + return math.sqrt((loc1.x - loc2.x) ** 2 + (loc1.y - loc2.y) ** 2) + + +# ====================== 4. 主程序(远距离停止+渐进减速) ====================== +def main(): + try: + # 1. 连接CARLA+加载地图 + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) + world = client.load_world('Town01') + world.set_weather(carla.WeatherParameters.ClearNoon) + print("✅ 连接CARLA成功!加载Town01场景") + + # 2. 清理残留Actor + for actor in world.get_actors(): + if actor.type_id in ['vehicle.*', 'static.prop.*', 'sensor.*']: + actor.destroy() + + # 3. 生成红色主车(同车道起点,手动挂前进挡) + blueprint_lib = world.get_blueprint_library() + main_car_bp = blueprint_lib.filter('vehicle.tesla.model3')[0] + main_car_bp.set_attribute('color', '255,0,0') # 红色 + spawn_points = world.get_map().get_spawn_points() + main_car_spawn = spawn_points[5] # 开阔直车道生成点(无围栏) + main_car = world.spawn_actor(main_car_bp, main_car_spawn) + + # 适配0.9.10:手动挂前进挡+解除手刹 + init_control = carla.VehicleControl( + throttle=NORMAL_THROTTLE, + steer=0.0, # 全程直行,不转向 + manual_gear_shift=True, # 开启手动换挡 + gear=1, # 前进挡 + hand_brake=False, + reverse=False + ) + main_car.apply_control(init_control) + print("✅ 生成红色主车:同车道起点,手动挂前进挡(直行)") + + # 4. 生成蓝色障碍车(红车同车道正前方25米,y坐标一致=同车道) + obstacle_car_bp = blueprint_lib.filter('vehicle.tesla.model3')[0] + obstacle_car_bp.set_attribute('color', '0,0,255') # 蓝色 + obstacle_transform = carla.Transform( + carla.Location( + x=main_car_spawn.location.x + OBSTACLE_DISTANCE, # 正前方25米 + y=main_car_spawn.location.y, # 同一车道(y坐标一致) + z=main_car_spawn.location.z + ), + main_car_spawn.rotation + ) + obstacle_car = world.spawn_actor(obstacle_car_bp, obstacle_transform) + obstacle_car.apply_control(carla.VehicleControl(hand_brake=True)) # 蓝车静止 + print(f"✅ 生成蓝色障碍车:红车同车道正前方{OBSTACLE_DISTANCE}米") + + # 5. 生成路侧边缘节点(V2X感知设备) + edge_node_bp = blueprint_lib.filter('static.prop.*')[0] + edge_node_transform = carla.Transform( + carla.Location( + x=main_car_spawn.location.x + 15, + y=main_car_spawn.location.y + 3, + z=3.0 + ), + main_car_spawn.rotation + ) + edge_node = world.spawn_actor(edge_node_bp, edge_node_transform) + print("✅ 生成路侧边缘节点(感知障碍)") + + # 6. 初始近视角(紧贴红车,看清同车道蓝车) + spectator = world.get_spectator() + spectator_transform = carla.Transform( + carla.Location( + x=main_car_spawn.location.x + 4, + y=main_car_spawn.location.y, + z=main_car_spawn.location.z + 6 # 稍高,看清25米外蓝车 + ), + carla.Rotation(pitch=-45, yaw=main_car_spawn.rotation.yaw) # 直视同车道 + ) + spectator.set_transform(spectator_transform) + print("✅ 初始视角设置完成:紧贴红车,看清同车道蓝车") + + # 7. 运行提示 + print("\n======= 车路协同避障仿真(远距离停止版) =======") + print(f"✅ 红蓝车:同一车道,蓝车在红车正前方{OBSTACLE_DISTANCE}米") + print("✅ 红车逻辑:直行→20米处减速→12米处完全停止(远离蓝车不撞)") + print("✅ 镜头:自由操作(左键旋转/滚轮缩放/WASD平移)") + print("✅ 退出方式:Ctrl+C 停止程序") + print("==============================================\n") + + main_car_control = init_control + is_stopped = False # 红车停止标记 + + while True: + # 计算红车与蓝车的实时距离 + current_distance = calculate_distance(main_car, obstacle_car) + + # 核心逻辑:渐进减速+远距离停止(避免碰撞) + if not is_stopped: + if current_distance > DECEL_DISTANCE: + # 阶段1:距离>20米,正常直行(无减速) + main_car_control.throttle = NORMAL_THROTTLE + main_car_control.brake = 0.0 + current_speed = math.hypot(main_car.get_velocity().x, main_car.get_velocity().y) + print(f"\r【直行中】距离蓝车:{current_distance:.1f}米 | 当前速度:{current_speed:.2f}m/s", end="") + elif DECEL_DISTANCE >= current_distance > STOP_DISTANCE: + # 阶段2:20米≥距离>12米,渐进减速(缓慢靠近) + main_car_control.throttle = DECEL_THROTTLE + main_car_control.brake = 0.0 + current_speed = math.hypot(main_car.get_velocity().x, main_car.get_velocity().y) + print(f"\r【减速中】距离蓝车:{current_distance:.1f}米 | 当前速度:{current_speed:.2f}m/s", end="") + else: + # 阶段3:距离≤12米,满刹车完全停止(远离蓝车,不撞) + main_car_control.throttle = 0.0 + main_car_control.brake = BRAKE_FORCE + print(f"\r【已停止】距离蓝车:{current_distance:.1f}米 → 远离蓝车,完全停止", end="") + is_stopped = True + else: + # 保持停止状态,避免再次移动 + main_car_control.throttle = 0.0 + main_car_control.brake = BRAKE_FORCE + print(f"\r【保持停止】距离蓝车:{current_distance:.1f}米 | 红车静止不动", end="") + + # 持续发送控制指令,确保状态生效 + main_car.apply_control(main_car_control) + time.sleep(0.02) + + except KeyboardInterrupt: + print("\n\n🛑 程序终止,清理资源...") + except Exception as e: + print(f"\n⚠️ 运行错误:{e} | 请确认CARLA 0.9.10已启动(localhost:2000)") + finally: + # 清理所有资源 + for actor_name in ['main_car', 'obstacle_car', 'edge_node']: + if actor_name in locals(): + locals()[actor_name].destroy() + print("✅ 资源清理完成,程序退出!") + + +# ====================== 程序入口(仅main.py) ====================== +if __name__ == "__main__": + main() \ No newline at end of file