diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/config.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/config.py index 565a3cf51c..31b4533265 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/config.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/config.py @@ -67,21 +67,26 @@ def get_current_trajectory(): VEHICLE_HEIGHT = 0.25 # 车辆显示高度 # ==================== 视角配置 ==================== -TOP_DOWN_HEIGHT = 30.0 # 俯视视角高度(提高) -TOP_DOWN_PITCH = -85.0 # 俯视角 (几乎垂直向下,-90是完全垂直) -SMOOTH_FOLLOW_FACTOR = 0.01 # 视角平滑系数 (0-1,越小越平滑) -MIN_SMOOTH_FACTOR = 0.001 # 最小平滑系数 +TOP_DOWN_HEIGHT = 5.0 # 俯视视角高度(提高) +TOP_DOWN_PITCH = -30.0 # 俯视角 (几乎垂直向下,-90是完全垂直) +SMOOTH_FOLLOW_FACTOR = 0.01 # 视角平滑系数 (0-1,越小越平滑) +MIN_SMOOTH_FACTOR = 0.001 # 最小平滑系数 MAX_SMOOTH_FACTOR = 0.3 # 最大平滑系数 SMOOTH_FACTOR_ADAPTIVE = True # 是否自适应平滑系数 -DISTANCE_THRESHOLD = 5.0 # 距离阈值,超过此值加大平滑系数 - +DISTANCE_THRESHOLD = 1.0 # 距离阈值,超过此值加大平滑系数 +FOLLOW_DISTANCE= 8.0 # 跟随距离(米) +FOLLOW_HEIGHT = 3.0 # 跟随高度(米) +FOLLOW_PITCH = -20.0 # 俯视角(度) +VIEW_UPDATE_ENABLED = True # 是否启用视角更新 +VIEW_UPDATE_FPS = 60 # 视角更新频率 (Hz) +VIEW_UPDATE_THREADED = True # 是否使用独立线程更新视角 # ==================== 性能配置 ==================== DEBUG_MODE = True # 调试模式 -FPS_LIMIT = 120 # FPS限制 (0为无限制,提高性能) +FPS_LIMIT = 60 # FPS限制 (0为无限制,提高性能) UPDATE_RATE = 0.5 # 更新率 (1.0 = 每步更新) USE_SMOOTH_INTERPOLATION = True # 使用平滑插值 INTERPOLATION_STEPS = 5 # 插值步数 -MAX_FRAME_SKIP = 2 # 最大跳帧数 +MAX_FRAME_SKIP = 1 # 最大跳帧数 # ==================== CARLA配置 ==================== CARLA_HOST = "localhost" @@ -89,7 +94,7 @@ def get_current_trajectory(): CARLA_TIMEOUT = 20.0 # ==================== 仿真配置 ==================== -FIXED_DELTA_SECONDS = 0.0166 # 固定时间步长 (~30 FPS) +FIXED_DELTA_SECONDS = 0.0166 # 固定时间步长 (~60 FPS) SYNCHRONOUS_MODE = False # 同步模式(影响性能,暂不启用) NO_RENDERING_MODE = False # 无渲染模式 diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/main.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/main.py index 1307269b1b..5dab272650 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/main.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/main.py @@ -1,4 +1,3 @@ - #!/usr/bin/env python3 """ 主程序 - 简化版 @@ -134,8 +133,8 @@ def run_episode(env, traffic_mgr, visualizer, tracker, episode_num): print("未找到车辆") return False - # 设置俯视视角 - tracker.set_top_down_view(ego_vehicle) + # 设置后方跟随视角(启动独立线程) + tracker.set_follow_view(ego_vehicle) # 绘制路线 if hasattr(env, 'path') and env.path: @@ -145,7 +144,7 @@ def run_episode(env, traffic_mgr, visualizer, tracker, episode_num): route_points.append((location.x, location.y, location.z)) visualizer.draw_planned_route(route_points) - # 运行循环 + # 运行循环 - 不再在主循环中更新视角 step_count = 0 done = False fps_counter = deque(maxlen=30) @@ -154,8 +153,8 @@ def run_episode(env, traffic_mgr, visualizer, tracker, episode_num): step_count += 1 step_start = time.time() - # 每步都更新视角 - tracker.smooth_follow_vehicle(ego_vehicle) + # 注意:不再调用 tracker.smooth_follow_vehicle(ego_vehicle) + # 视角更新由独立线程处理 # 更新车辆显示 vehicle_state = tracker.get_vehicle_state(ego_vehicle) @@ -182,12 +181,16 @@ def run_episode(env, traffic_mgr, visualizer, tracker, episode_num): if cfg.DEBUG_MODE and step_count % 50 == 0: fps = len(fps_counter) / sum(fps_counter) if fps_counter else 0 - print(f"步骤 {step_count}, FPS: {fps:.1f}, 动作: {cfg.ACTION_NAMES[action]}") + vehicle_speed = vehicle_state.get('speed_2d', 0) if vehicle_state else 0 + print(f"步骤 {step_count}, FPS: {fps:.1f}, 速度: {vehicle_speed:.1f}m/s, 动作: {cfg.ACTION_NAMES[action]}") if done: print(f"Episode {episode_num} 完成,步数: {step_count}") break + # 清理跟踪器资源 + tracker.cleanup() + return True def main(): @@ -199,19 +202,22 @@ def main(): env, traffic_mgr, visualizer, tracker, trajectory = result - # 运行episode - for episode in range(cfg.TOTAL_EPISODES): - success = run_episode(env, traffic_mgr, visualizer, tracker, episode + 1) - - if episode < cfg.TOTAL_EPISODES - 1: - print(f"等待 {cfg.EPISODE_INTERVAL} 秒...") - time.sleep(cfg.EPISODE_INTERVAL) - - # 清理 - if traffic_mgr: - traffic_mgr.cleanup() + try: + # 运行episode + for episode in range(cfg.TOTAL_EPISODES): + success = run_episode(env, traffic_mgr, visualizer, tracker, episode + 1) + + if episode < cfg.TOTAL_EPISODES - 1: + print(f"等待 {cfg.EPISODE_INTERVAL} 秒...") + time.sleep(cfg.EPISODE_INTERVAL) + finally: + # 确保清理 + if tracker: + tracker.cleanup() + if traffic_mgr: + traffic_mgr.cleanup() print("\n程序结束") - + if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/traffic_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/traffic_manager.py index 584670f20f..09cb1e70ed 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/traffic_manager.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/traffic_manager.py @@ -1,6 +1,6 @@ """ -交通管理器 - 简化版 +交通管理器-生成行人和车辆 """ import sys diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/vehicle_tracker.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/vehicle_tracker.py index d76c20a489..667413d7fd 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/vehicle_tracker.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/vehicle_tracker.py @@ -1,11 +1,12 @@ """ -车辆跟踪和视角控制模块 - 平滑跟随车辆,获取车辆状态 +车辆跟踪和视角控制模块 - 从后方跟随车辆(独立更新版本) """ import carla import math import time import numpy as np +import threading import config as cfg class VehicleTracker: @@ -37,73 +38,108 @@ def __init__(self, world): self.velocity_history = [] self.max_velocity_history = 10 - # 视角参数 - self.target_height = cfg.TOP_DOWN_HEIGHT - self.pitch_angle = cfg.TOP_DOWN_PITCH + # 后方跟随参数 + self.follow_distance = 8.0 # 跟随距离(米) + self.follow_height = 3.0 # 跟随高度(米) + self.follow_pitch = -20.0 # 俯视角(度) - print(f"📐 车辆跟踪器初始化完成,平滑系数: {self.smooth_factor}") + # 车辆引用 + self.current_vehicle = None + + # 线程控制 + self.view_update_enabled = cfg.VIEW_UPDATE_ENABLED + self.view_update_fps = cfg.VIEW_UPDATE_FPS + self.view_update_thread = None + self.is_running = False + self.last_frame_time = 0 + + print(f"📐 车辆跟踪器初始化完成,后方跟随模式") + print(f" 跟随距离: {self.follow_distance}m") + print(f" 跟随高度: {self.follow_height}m") + print(f" 俯视角: {self.follow_pitch}°") + print(f" 视角更新FPS: {self.view_update_fps}") - def set_top_down_view(self, vehicle, height=None): - """设置俯视视角""" - if vehicle is None: - return False + def start_view_update_thread(self, vehicle): + """启动独立视角更新线程""" + if not self.view_update_enabled: + return - try: - if height is not None: - self.target_height = height - - transform = vehicle.get_transform() - location = transform.location - - # 设置相机在车辆正上方 - camera_location = carla.Location( - x=location.x, - y=location.y, - z=location.z + self.target_height - ) - - # 设置俯视角度 - camera_rotation = carla.Rotation( - pitch=self.pitch_angle, - yaw=transform.rotation.yaw, - roll=0.0 - ) - - camera_transform = carla.Transform(camera_location, camera_rotation) - self.spectator.set_transform(camera_transform) - self.last_camera_transform = camera_transform - self.last_vehicle_transform = transform - - print(f"📐 设置俯视视角,高度: {self.target_height}m") - return True - - except Exception as e: - print(f"❌ 设置俯视视角失败: {e}") - return False + self.current_vehicle = vehicle + + if self.view_update_thread and self.is_running: + self.stop_view_update_thread() + + self.is_running = True + self.view_update_thread = threading.Thread( + target=self._view_update_loop, + daemon=True + ) + self.view_update_thread.start() + print(f"🔧 启动独立视角更新线程,频率: {self.view_update_fps}Hz") - def smooth_follow_vehicle(self, vehicle, height=None): - """平滑跟随车辆(每帧调用)- 改进版本""" - if vehicle is None: - return False + def stop_view_update_thread(self): + """停止视角更新线程""" + if self.view_update_thread: + self.is_running = False + self.view_update_thread.join(timeout=1.0) + self.view_update_thread = None + print("🛑 停止视角更新线程") + + def _view_update_loop(self): + """独立视角更新循环""" + target_interval = 1.0 / self.view_update_fps + + while self.is_running and self.current_vehicle: + try: + start_time = time.time() + + # 检查是否有足够的车辆信息 + if not self.current_vehicle: + time.sleep(target_interval) + continue + + # 更新视角 + self._update_camera_view() + + # 计算实际耗时并补偿 + elapsed = time.time() - start_time + sleep_time = max(0, target_interval - elapsed) + + if sleep_time > 0: + time.sleep(sleep_time) + else: + # 如果处理时间超过目标间隔,跳过等待 + if cfg.DEBUG_MODE and self.frame_count % 100 == 0: + print(f"[视角] 帧率下降: {elapsed:.3f}s > {target_interval:.3f}s") + + except Exception as e: + if cfg.DEBUG_MODE: + print(f"⚠️ 视角更新线程异常: {e}") + time.sleep(target_interval) + + def _update_camera_view(self): + """更新相机视角(线程安全)""" + if not self.current_vehicle: + return try: current_time = time.time() time_delta = current_time - self.last_update_time - # 限制最小时间间隔,避免计算过于频繁 + # 限制最小时间间隔 if time_delta < 0.001: - return True + return self.last_update_time = current_time self.frame_count += 1 # 获取车辆当前状态 - vehicle_transform = vehicle.get_transform() + vehicle_transform = self.current_vehicle.get_transform() vehicle_location = vehicle_transform.location vehicle_rotation = vehicle_transform.rotation # 获取车辆速度用于预测 - vehicle_velocity = vehicle.get_velocity() + vehicle_velocity = self.current_vehicle.get_velocity() speed = math.sqrt(vehicle_velocity.x**2 + vehicle_velocity.y**2 + vehicle_velocity.z**2) # 更新速度历史 @@ -111,19 +147,22 @@ def smooth_follow_vehicle(self, vehicle, height=None): if len(self.velocity_history) > self.max_velocity_history: self.velocity_history.pop(0) - if height is not None: - self.target_height = height + # 计算车辆的后方偏移方向 + yaw_rad = math.radians(vehicle_rotation.yaw) + + # 计算目标相机位置(车辆后方) + offset_x = -self.follow_distance * math.cos(yaw_rad) + offset_y = -self.follow_distance * math.sin(yaw_rad) - # 计算目标相机位置(车辆正上方) target_location = carla.Location( - x=vehicle_location.x, - y=vehicle_location.y, - z=vehicle_location.z + self.target_height + x=vehicle_location.x + offset_x, + y=vehicle_location.y + offset_y, + z=vehicle_location.z + self.follow_height ) - # 目标相机旋转(保持俯视,yaw跟随车辆) + # 目标相机旋转(朝向车辆) target_rotation = carla.Rotation( - pitch=self.pitch_angle, + pitch=self.follow_pitch, yaw=vehicle_rotation.yaw, roll=0.0 ) @@ -136,7 +175,7 @@ def smooth_follow_vehicle(self, vehicle, height=None): # 预测目标位置(如果启用) if self.prediction_enabled and len(self.velocity_history) > 1: avg_speed = np.mean(self.velocity_history[-3:]) if len(self.velocity_history) >= 3 else speed - target_location = self._predict_target_position( + target_location = self._predict_follow_position( target_location, vehicle_rotation, avg_speed, time_delta ) @@ -174,20 +213,86 @@ def smooth_follow_vehicle(self, vehicle, height=None): self.last_camera_transform = smooth_transform self.last_vehicle_transform = vehicle_transform - # 每100帧输出一次调试信息 - if cfg.DEBUG_MODE and self.frame_count % 100 == 0: + # 调试信息 + if cfg.DEBUG_MODE and self.frame_count % 200 == 0: print(f"[视角] 帧: {self.frame_count}, " f"平滑系数: {effective_smooth_factor:.3f}, " - f"速度: {speed:.2f}m/s, " - f"时差: {time_delta:.3f}s") - - return True + f"速度: {speed:.2f}m/s") except Exception as e: if cfg.DEBUG_MODE: print(f"⚠️ 视角更新失败: {e}") + + def set_follow_view(self, vehicle): + """设置后方跟随视角""" + if vehicle is None: + return False + + try: + transform = vehicle.get_transform() + location = transform.location + rotation = transform.rotation + + # 计算车辆的后方偏移方向 + yaw_rad = math.radians(rotation.yaw) + + # 计算相机位置(车辆后方) + offset_x = -self.follow_distance * math.cos(yaw_rad) + offset_y = -self.follow_distance * math.sin(yaw_rad) + + camera_location = carla.Location( + x=location.x + offset_x, + y=location.y + offset_y, + z=location.z + self.follow_height + ) + + # 相机朝向车辆 + camera_rotation = carla.Rotation( + pitch=self.follow_pitch, + yaw=rotation.yaw, + roll=0.0 + ) + + camera_transform = carla.Transform(camera_location, camera_rotation) + self.spectator.set_transform(camera_transform) + self.last_camera_transform = camera_transform + self.last_vehicle_transform = transform + + # 设置当前车辆并启动更新线程 + self.current_vehicle = vehicle + if cfg.VIEW_UPDATE_THREADED: + self.start_view_update_thread(vehicle) + + print(f"📐 设置后方跟随视角") + return True + + except Exception as e: + print(f"❌ 设置后方跟随视角失败: {e}") return False + def _predict_follow_position(self, target_loc, vehicle_rot, speed, time_delta): + """预测后方跟随位置""" + if speed < 0.1: # 速度太慢不预测 + return target_loc + + # 预测未来0.3秒的位置 + prediction_time = 0.3 + angle_rad = math.radians(vehicle_rot.yaw) + + # 车辆前进方向 + vehicle_dx = speed * math.cos(angle_rad) * prediction_time + vehicle_dy = speed * math.sin(angle_rad) * prediction_time + + # 相机跟随车辆移动 + predicted_x = target_loc.x + vehicle_dx + predicted_y = target_loc.y + vehicle_dy + + return carla.Location( + x=predicted_x, + y=predicted_y, + z=target_loc.z + ) + def _calculate_adaptive_smooth_factor(self, vehicle_loc, target_loc, speed, time_delta): """计算自适应平滑系数""" base_factor = self.smooth_factor @@ -226,24 +331,6 @@ def _calculate_adaptive_smooth_factor(self, vehicle_loc, target_loc, speed, time return base_factor - def _predict_target_position(self, target_loc, vehicle_rot, speed, time_delta): - """预测目标位置""" - if speed < 0.1: # 速度太慢不预测 - return target_loc - - # 预测未来0.2秒的位置 - prediction_time = 0.2 - angle_rad = math.radians(vehicle_rot.yaw) - - predicted_x = target_loc.x + speed * math.cos(angle_rad) * prediction_time - predicted_y = target_loc.y + speed * math.sin(angle_rad) * prediction_time - - return carla.Location( - x=predicted_x, - y=predicted_y, - z=target_loc.z - ) - def _multi_step_interpolation(self, current_transform, target_loc, target_rot, smooth_factor): """多步插值,实现更平滑的移动""" current_loc = current_transform.location @@ -385,6 +472,17 @@ def calculate_progress(self, x, y, route_points): return f"进度: {progress:.1f}% | 距起点: {dist_to_start:.1f}m | 距终点: {dist_to_end:.1f}m" + def update_follow_params(self, distance=None, height=None, pitch=None): + """更新后方跟随参数""" + if distance is not None and distance > 0: + self.follow_distance = distance + if height is not None: + self.follow_height = height + if pitch is not None: + self.follow_pitch = pitch + + print(f"🔄 后方跟随参数更新 - 距离: {self.follow_distance}m, 高度: {self.follow_height}m, 俯角: {self.follow_pitch}°") + def update_smooth_factor(self, factor): """更新平滑系数""" if 0 < factor <= 1: @@ -393,9 +491,14 @@ def update_smooth_factor(self, factor): else: print(f"❌ 无效的平滑系数: {factor},保持为: {self.smooth_factor}") + def cleanup(self): + """清理资源""" + self.stop_view_update_thread() + self.current_vehicle = None + def reset(self): """重置跟踪器状态""" self.velocity_history = [] self.frame_count = 0 self.last_update_time = time.time() - print("🔄 车辆跟踪器已重置") + print("🔄 车辆跟踪器已重置") \ No newline at end of file