diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/msg/VehicleControl.msg b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/msg/VehicleControl.msg new file mode 100644 index 0000000000..776215c69e --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/msg/VehicleControl.msg @@ -0,0 +1,6 @@ +# 车辆控制消息 +float32 throttle # 油门 (0-1) +float32 steer # 转向 (-1左, 1右) +float32 brake # 刹车 (0-1) +bool reverse # 倒车 +bool hand_brake # 手刹 \ No newline at end of file diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_node.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_node.py index 1005b196d3..df95773fb3 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_node.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_node.py @@ -9,7 +9,7 @@ import time import threading import numpy as np - +import math # TensorFlow配置 os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' os.environ['CUDA_VISIBLE_DEVICES'] = '-1' @@ -106,6 +106,7 @@ def __init__(self): self.visualizer = None self.tracker = None self.traffic_mgr = None + self.last_reset_time = 0.0 # 控制标志 self.running = False @@ -124,63 +125,182 @@ def __init__(self): if not self.init_carla_environment(): rospy.logerr("CARLA环境初始化失败,节点将退出") sys.exit(1) + self.speed_threshold = 1.0 # 最小速度阈值 (km/h) + self.stuck_counter = 0 + self.max_stuck_steps = 10 # 连续多少步速度为0算作卡住 + + rospy.loginfo("CARLA自动驾驶ROS节点初始化完成") + rospy.loginfo("初始化完成,等待5秒后开始测试...") + time.sleep(5) + # 创建一个测试线程 + test_thread = threading.Thread(target=self.test_action_mapping) + test_thread.daemon = True + test_thread.start() + + rospy.loginfo("CARLA自动驾驶ROS节点初始化完成") - def init_carla_environment(self): - """初始化CARLA环境""" - try: - rospy.loginfo("正在初始化CARLA环境...") - - # 创建环境 - self.env = CarEnv(self.trajectory['start'], self.trajectory['end']) - rospy.loginfo("CARLA环境创建成功") - - # 创建管理器 - self.model_manager = ModelManager() - self.visualizer = RouteVisualizer(self.env.world) - self.tracker = VehicleTracker(self.env.world) - - # 加载模型 - rospy.loginfo("正在加载模型...") - if not self.model_manager.load_models(): - rospy.logerr("模型加载失败") - return False - - # 生成交通 - if cfg.ENABLE_TRAFFIC: - rospy.loginfo("正在生成交通...") - self.traffic_mgr = TrafficManager(self.env.client, self.env.world) - self.traffic_mgr.generate_traffic( - num_vehicles=cfg.TRAFFIC_VEHICLES, - num_walkers=cfg.TRAFFIC_WALKERS, - safe_mode=cfg.TRAFFIC_SAFE_MODE - ) - - rospy.loginfo("CARLA环境初始化成功") - return True + def init_carla_environment(self, max_retries=3): + """初始化CARLA环境,支持重试""" + for attempt in range(max_retries): + try: + rospy.loginfo(f"尝试连接CARLA服务器 (尝试 {attempt+1}/{max_retries})...") + + # 创建环境 + self.env = CarEnv(self.trajectory['start'], self.trajectory['end']) + rospy.loginfo("CARLA环境创建成功") + + # 确保path存在 + if not hasattr(self.env, 'path') or not self.env.path: + rospy.loginfo("计算规划路径...") + try: + # 调用reset方法会计算路径 + initial_state = self.env.reset() + rospy.loginfo(f"规划路径计算完成,包含 {len(self.env.path)} 个点") + except Exception as e: + rospy.logwarn(f"计算规划路径失败: {e}") + + # 创建管理器 + self.model_manager = ModelManager() + self.visualizer = RouteVisualizer(self.env.world) + self.tracker = VehicleTracker(self.env.world) + + # 加载模型 + rospy.loginfo("正在加载模型...") + if not self.model_manager.load_models(): + rospy.logwarn("模型加载失败,使用默认控制") + + # 生成交通 + if cfg.ENABLE_TRAFFIC: + rospy.loginfo("正在生成交通...") + self.traffic_mgr = TrafficManager(self.env.client, self.env.world) + self.traffic_mgr.generate_traffic( + num_vehicles=min(cfg.TRAFFIC_VEHICLES, 5), + num_walkers=min(cfg.TRAFFIC_WALKERS, 10), + safe_mode=cfg.TRAFFIC_SAFE_MODE + ) + + rospy.loginfo("CARLA环境初始化成功") + self.publish_status("CARLA环境就绪") + return True + + except Exception as e: + if attempt < max_retries - 1: + rospy.logwarn(f"初始化失败,{5}秒后重试...: {e}") + time.sleep(5) + else: + rospy.logerr(f"CARLA环境初始化最终失败: {e}") + return False + return False + + def test_action_mapping(self): + """测试动作到控制命令的映射""" + rospy.loginfo("=== 开始动作映射测试 ===") + + if not self.env or not self.env.vehicle: + rospy.logerr("环境或车辆未就绪") + return + + import carla + + # 测试每个可能的动作 + for action_idx in range(len(cfg.ACTION_NAMES)): + action_name = cfg.ACTION_NAMES[action_idx] + rospy.loginfo(f"测试动作 {action_idx}: {action_name}") - except Exception as e: - rospy.logerr(f"初始化CARLA环境失败: {e}") - import traceback - rospy.logerr(traceback.format_exc()) - return False - + try: + # 使用环境的方法执行动作 + # 创建一个初始状态 + current_state = [0.0, 0.0, 0.0, 0.0] # 根据car_env.py中的状态维度设置 + + # 检查车辆是否被销毁 + if not self.env.vehicle or not self.env.vehicle.is_alive: + rospy.logerr("车辆已被销毁,跳过测试") + break + + state, reward, done, info = self.env.step(action_idx, current_state) + + # 获取应用的控制 + control = self.env.vehicle.get_control() + rospy.loginfo(f" 控制命令: 油门={control.throttle:.2f}, 刹车={control.brake:.2f}, " + f"转向={control.steer:.2f}, 倒车={control.reverse}") + + # 检查车辆速度 + velocity = self.env.vehicle.get_velocity() + speed = 3.6 * (velocity.x**2 + velocity.y**2 + velocity.z**2)**0.5 + rospy.loginfo(f" 速度: {speed:.2f} km/h") + + # 重置控制 + stop_control = carla.VehicleControl() + stop_control.brake = 1.0 + self.env.vehicle.apply_control(stop_control) + time.sleep(0.5) + + except Exception as e: + # 修复:使用正确的rospy日志方法 + rospy.logerr(f" 测试失败: {e}") + + time.sleep(0.5) + + rospy.loginfo("=== 动作映射测试完成 ===") def control_callback(self, msg): """控制命令回调""" - if not self.env or not hasattr(self.env, 'vehicle') or self.env.vehicle is None: - rospy.logwarn("无法处理控制命令:车辆未就绪") + if not self.env or not hasattr(self.env, 'vehicle'): + rospy.logdebug("车辆未就绪,忽略控制命令") return try: + # 更新控制消息 self.last_control_msg = msg self.manual_control_active = True - # 记录控制命令 - rospy.logdebug(f"收到控制命令: throttle={msg.linear.x:.2f}, steer={msg.angular.z:.2f}, brake={msg.linear.z:.2f}") + # 确保车辆存在且有效 + if not self.env.vehicle or not self.env.vehicle.is_alive: + rospy.logwarn("车辆不存在或已销毁") + return + + # 应用控制 + import carla + control = carla.VehicleControl() + + # 解析控制消息 + throttle = float(msg.linear.x) # 油门/前进 + steer = float(msg.angular.z) # 转向 + brake = float(msg.linear.z) # 刹车 + + # 处理倒车逻辑 + if throttle < 0: + # 倒车模式 + control.throttle = abs(throttle) # 油门值取绝对值 + control.brake = 0.0 + control.reverse = True # 设置倒车标志 + rospy.logdebug(f"倒车控制: 油门={control.throttle:.2f}, 转向={steer:.2f}") + elif brake > 0: + # 刹车模式 + control.throttle = 0.0 + control.brake = brake + control.reverse = False + rospy.logdebug(f"刹车控制: 刹车={control.brake:.2f}") + else: + # 前进模式 + control.throttle = throttle + control.brake = 0.0 + control.reverse = False + rospy.logdebug(f"前进控制: 油门={control.throttle:.2f}, 转向={steer:.2f}") + + # 转向控制 + control.steer = max(-1.0, min(1.0, steer)) + + # 手刹(紧急刹车时使用) + control.hand_brake = (brake > 0.8) + + # 应用控制 + self.env.vehicle.apply_control(control) except Exception as e: - rospy.logwarn(f"控制命令处理失败: {e}") + if "destroyed actor" not in str(e): + rospy.logwarn(f"控制命令处理失败: {e}") def apply_control(self): """应用当前控制""" @@ -212,30 +332,58 @@ def apply_control(self): rospy.logwarn(f"应用控制失败: {e}") def handle_reset(self, req): - """重置环境服务处理(自定义服务)""" + """重置环境服务处理""" with self.lock: try: + current_time = time.time() + if hasattr(self, 'last_reset_time'): + if current_time - self.last_reset_time < 2.0: + rospy.logwarn("重置过于频繁,请等待2秒") + return ResetResponse(Float32MultiArray()) + else: + self.last_reset_time = 0.0 # 初始化属性 rospy.loginfo("收到重置环境请求") + self.last_reset_time = current_time + + # 停止当前episode + self.running = False + self.manual_control_active = False + time.sleep(0.5) # 重置环境 if self.env: - current_state = self.env.reset() - - # 转换为Float32MultiArray - state_array = Float32MultiArray() - state_array.data = current_state - - self.publish_status("环境重置成功") - - return ResetResponse(state_array) + try: + current_state = self.env.reset() + + # 重置后确保path存在 + if not hasattr(self.env, 'path') or not self.env.path: + rospy.loginfo("重置后重新计算规划路径...") + try: + traj = self.env.trajectory(draw=False) + self.env.path = [] + for el in traj: + self.env.path.append(el[0]) + rospy.loginfo(f"规划路径重新计算完成,包含 {len(self.env.path)} 个点") + except Exception as e: + rospy.logwarn(f"重新计算规划路径失败: {e}") + + # 转换为Float32MultiArray + state_array = Float32MultiArray() + state_array.data = current_state + + self.publish_status("环境重置成功") + rospy.loginfo("环境重置成功") + + return ResetResponse(state_array) + except Exception as e: + rospy.logerr(f"重置环境时出错: {e}") else: rospy.logerr("环境未初始化") - return ResetResponse(Float32MultiArray()) + + return ResetResponse(Float32MultiArray()) except Exception as e: rospy.logerr(f"重置环境失败: {e}") - import traceback - rospy.logerr(traceback.format_exc()) return ResetResponse(Float32MultiArray()) def handle_reset_simple(self, req): @@ -318,7 +466,8 @@ def handle_stop_simple(self, req): return TriggerResponse(True, "Stopped") def run_episode(self): - """运行一个episode""" + """运行一个episode - 优化控制频率""" + import carla rospy.loginfo(f"开始运行Episode {self.current_episode}") try: @@ -333,45 +482,116 @@ def run_episode(self): if self.env.vehicle: self.tracker.set_follow_view(self.env.vehicle) - # 主循环 + # 主循环 - 大幅提高控制频率 done = False + control_interval = 0.01 # 从0.05减少到0.01,100Hz控制频率 + last_control_time = time.time() + + # 性能计数器 + control_count = 0 + total_control_time = 0 + while not done and not rospy.is_shutdown() and self.running: if self.step_count >= cfg.MAX_STEPS_PER_EPISODE: rospy.loginfo("达到最大步数") break - # 模型预测动作(仅在自主驾驶模式) - if not self.manual_control_active: - action = self.model_manager.predict_action(current_state) - rospy.logdebug(f"自主动作: {cfg.ACTION_NAMES[action] if action < len(cfg.ACTION_NAMES) else action}") + current_time = time.time() + elapsed = current_time - last_control_time + + # 只在控制间隔到达时执行动作 + if elapsed >= control_interval: + control_start = time.time() - # 执行动作 - new_state, reward, done, _ = self.env.step(action, current_state) + # 检查车辆是否被销毁 + if not self.env.vehicle or not self.env.vehicle.is_alive: + rospy.logwarn("车辆已被销毁,终止episode") + done = True + break - # 更新统计 - self.total_reward += reward - self.step_count += 1 + # 模型预测动作(仅在自主驾驶模式) + if not self.manual_control_active: + action = self.model_manager.predict_action(current_state) + + # 执行动作 + try: + new_state, reward, done, _ = self.env.step(action, current_state) + except Exception as e: + rospy.logerr(f"执行动作时出错: {e}") + done = True + break + + # 更新统计 + self.total_reward += reward + self.step_count += 1 + + # 发布奖励 + reward_msg = Float32() + reward_msg.data = reward + self.reward_pub.publish(reward_msg) + + # 更新状态 + current_state = new_state + + # 获取并记录速度和位置 + if self.env.vehicle: + velocity = self.env.vehicle.get_velocity() + speed = 3.6 * math.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + + # 每50步记录一次性能数据 + if self.step_count % 50 == 0: + # 计算路径跟踪误差 + if hasattr(self.env, 'path') and self.env.path: + waypoint_ind = self.env.get_closest_waypoint(self.env.path, self.env.vehicle.get_transform()) + if waypoint_ind < len(self.env.path): + current_waypoint = self.env.path[waypoint_ind] + vehicle_pos = self.env.vehicle.get_transform().location + waypoint_pos = current_waypoint.transform.location + lateral_error = math.sqrt( + (vehicle_pos.x - waypoint_pos.x)**2 + + (vehicle_pos.y - waypoint_pos.y)**2 + ) + rospy.loginfo(f"步骤 {self.step_count}, 速度: {speed:.1f} km/h, 横向误差: {lateral_error:.2f} m") + + # 检查是否卡住 + if speed < self.speed_threshold: + self.stuck_counter += 1 + if self.stuck_counter >= self.max_stuck_steps: + rospy.logwarn("车辆卡住,尝试恢复...") + # 尝试短暂全油门 + control = carla.VehicleControl() + control.throttle = 1.0 + control.steer = 0.0 + control.brake = 0.0 + self.env.vehicle.apply_control(control) + time.sleep(0.1) # 减少恢复等待时间 + self.stuck_counter = 0 + else: + self.stuck_counter = 0 - # 发布奖励 - reward_msg = Float32() - reward_msg.data = reward - self.reward_pub.publish(reward_msg) + control_time = time.time() - control_start + total_control_time += control_time + control_count += 1 - # 更新状态 - current_state = new_state + # 每100步记录平均控制延迟 + if control_count % 100 == 0 and control_count > 0: + avg_control_time = total_control_time / control_count + rospy.loginfo(f"平均控制延迟: {avg_control_time*1000:.1f} ms, 频率: {1.0/avg_control_time:.1f} Hz") + total_control_time = 0 + control_count = 0 + + last_control_time = current_time + + # 检查是否完成 + if done: + status_msg = f"Episode {self.current_episode} 完成,总奖励: {self.total_reward:.2f}" + rospy.loginfo(status_msg) + break else: - # 手动控制模式,等待用户操作 - time.sleep(0.1) - continue - - # 检查是否完成 - if done: - status_msg = f"Episode {self.current_episode} 完成,总奖励: {self.total_reward:.2f}" - rospy.loginfo(status_msg) - break - - # 控制循环频率 - self.rate.sleep() + # 精确等待,避免CPU占用过高 + sleep_time = control_interval - elapsed + if sleep_time > 0: + time.sleep(min(sleep_time, 0.001)) # 发布最终状态 status_msg = f"Episode {self.current_episode} 结束,步数: {self.step_count},总奖励: {self.total_reward:.2f}" @@ -386,51 +606,41 @@ def run_episode(self): import traceback rospy.logerr(traceback.format_exc()) self.running = False - - def publish_vehicle_state(self): - """发布车辆状态""" - if not self.env or not hasattr(self.env, 'vehicle') or self.env.vehicle is None: + + def publish_planned_path(self): + """发布规划路径""" + if not self.env: return try: - # 获取车辆状态 - vehicle = self.env.vehicle - transform = vehicle.get_transform() - velocity = vehicle.get_velocity() - - # 创建Odometry消息 - odom = Odometry() - odom.header = Header() - odom.header.stamp = rospy.Time.now() - odom.header.frame_id = "map" - odom.child_frame_id = "base_link" - - # 位置 - odom.pose.pose.position = Point( - x=transform.location.x, - y=transform.location.y, - z=transform.location.z - ) - - # 方向(四元数) - roll = transform.rotation.roll * np.pi / 180 - pitch = transform.rotation.pitch * np.pi / 180 - yaw = transform.rotation.yaw * np.pi / 180 - quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw) - odom.pose.pose.orientation = Quaternion(*quat) + path_msg = Path() + path_msg.header = Header() + path_msg.header.stamp = rospy.Time.now() + path_msg.header.frame_id = "map" - # 速度 - odom.twist.twist.linear = Vector3( - x=velocity.x, - y=velocity.y, - z=velocity.z - ) + # 检查是否有path属性 + if hasattr(self.env, 'path') and self.env.path: + point_count = 0 + for waypoint in self.env.path: + # 每隔5个点取一个点,避免路径太密集 + if point_count % 5 == 0: + pose = Pose() + pose.position = Point( + x=waypoint.transform.location.x, + y=waypoint.transform.location.y, + z=waypoint.transform.location.z + ) + path_msg.poses.append(pose) + point_count += 1 + + # 定期记录 + if rospy.get_time() % 30 < 0.1: # 每30秒记录一次 + rospy.loginfo(f"发布规划路径,包含 {len(path_msg.poses)} 个点") - # 发布 - self.vehicle_state_pub.publish(odom) + self.path_pub.publish(path_msg) except Exception as e: - rospy.logwarn(f"发布车辆状态失败: {e}") + rospy.logdebug(f"发布规划路径失败: {e}") def publish_camera_images(self): """发布相机图像""" @@ -457,32 +667,7 @@ def publish_camera_images(self): except Exception as e: rospy.logdebug(f"发布图像失败: {e}") - - def publish_planned_path(self): - """发布规划路径""" - if not self.env or not hasattr(self.env, 'path'): - return - - try: - path_msg = Path() - path_msg.header = Header() - path_msg.header.stamp = rospy.Time.now() - path_msg.header.frame_id = "map" - - for waypoint in self.env.path: - pose = Pose() - pose.position = Point( - x=waypoint.transform.location.x, - y=waypoint.transform.location.y, - z=waypoint.transform.location.z - ) - path_msg.poses.append(pose) - - self.path_pub.publish(path_msg) - - except Exception as e: - rospy.logdebug(f"发布规划路径失败: {e}") - + def publish_visualization_markers(self): """发布可视化标记""" if not self.env or not hasattr(self.env, 'vehicle') or self.env.vehicle is None: @@ -563,6 +748,68 @@ def publish_status(self, status_msg): except Exception as e: rospy.logwarn(f"发布状态失败: {e}") + def publish_vehicle_state(self): + """发布车辆状态信息""" + if not self.env or not hasattr(self.env, 'vehicle') or self.env.vehicle is None: + return + + try: + odom_msg = Odometry() + odom_msg.header = Header() + odom_msg.header.stamp = rospy.Time.now() + odom_msg.header.frame_id = "map" + odom_msg.child_frame_id = "vehicle" + + # 获取车辆变换 + transform = self.env.vehicle.get_transform() + location = transform.location + rotation = transform.rotation + + # 设置位置 + odom_msg.pose.pose.position = Point( + x=location.x, + y=location.y, + z=location.z + ) + + # 设置朝向 + quat = tf.transformations.quaternion_from_euler( + 0, 0, rotation.yaw * np.pi / 180 + ) + odom_msg.pose.pose.orientation = Quaternion(*quat) + + # 获取车辆速度(如果可用) + velocity = self.env.vehicle.get_velocity() + odom_msg.twist.twist.linear = Vector3( + x=velocity.x, + y=velocity.y, + z=velocity.z + ) + + # 获取车辆角速度(如果可用) + angular_velocity = self.env.vehicle.get_angular_velocity() + odom_msg.twist.twist.angular = Vector3( + x=angular_velocity.x, + y=angular_velocity.y, + z=angular_velocity.z + ) + + # 发布里程计消息 + self.vehicle_state_pub.publish(odom_msg) + + # 发布TF变换 + br = tf.TransformBroadcaster() + br.sendTransform( + (location.x, location.y, location.z), + quat, + rospy.Time.now(), + "vehicle", + "map" + ) + + except Exception as e: + rospy.logdebug(f"发布车辆状态失败: {e}") + def publish_all_data(self): """发布所有数据""" try: @@ -576,23 +823,42 @@ def publish_all_data(self): except Exception as e: rospy.logwarn(f"发布数据时出错: {e}") - + def run(self): """主运行循环""" rospy.loginfo("CARLA自动驾驶ROS节点开始运行") - # 等待CARLA服务器 - time.sleep(2.0) - - # 发布初始状态 - self.publish_status("节点就绪") + # 控制发布计数器 + last_path_publish = 0 + last_marker_publish = 0 + last_camera_publish = 0 + last_state_publish = 0 # 主循环 while not rospy.is_shutdown(): try: - if self.env and hasattr(self.env, 'vehicle') and self.env.vehicle: - # 发布数据 - self.publish_all_data() + current_time = rospy.get_time() + + if self.env: + # 发布车辆状态(中等频率,20Hz) + if current_time - last_state_publish > 0.05: + self.publish_vehicle_state() + last_state_publish = current_time + + # 发布相机图像(较低频率,10Hz) + if current_time - last_camera_publish > 0.1: + self.publish_camera_images() + last_camera_publish = current_time + + # 发布规划路径(低频,每5秒一次) + if current_time - last_path_publish > 5.0: + self.publish_planned_path() + last_path_publish = current_time + + # 发布可视化标记(低频,每1秒一次) + if current_time - last_marker_publish > 1.0: + self.publish_visualization_markers() + last_marker_publish = current_time # 控制循环频率 self.rate.sleep() @@ -601,9 +867,7 @@ def run(self): break except Exception as e: rospy.logerr(f"主循环出错: {e}") - import traceback - rospy.logerr(traceback.format_exc()) - time.sleep(1.0) + time.sleep(0.1) # 减少错误等待时间 # 清理 self.cleanup() diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_control_client.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_control_client.py index 6612b12c4d..98fa0724f5 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_control_client.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_control_client.py @@ -8,6 +8,7 @@ import select import tty import termios +import time from std_msgs.msg import String from geometry_msgs.msg import Twist @@ -67,50 +68,199 @@ def manual_control(self): """手动控制模式""" rospy.loginfo("进入手动控制模式") rospy.loginfo("使用键盘控制车辆:") - rospy.loginfo(" W/S: 前进/后退") - rospy.loginfo(" A/D: 左转/右转") + rospy.loginfo(" W: 前进") + rospy.loginfo(" S: 后退") + rospy.loginfo(" A: 左转") + rospy.loginfo(" D: 右转") rospy.loginfo(" Space: 刹车") - rospy.loginfo(" Q: 退出手动模式") + rospy.loginfo(" Q: 退出") + rospy.loginfo("注意: 按住键持续控制,松开自动回正") print("\n开始控制(按Q退出)...") + # 使用一个简单的循环,每次获取一个字符 + # 这种方法在大多数系统上都能工作 + import select + + control_msg = Twist() + + # 设置终端为非阻塞模式 + import termios + import tty + + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(fd) + + # 状态变量 + current_throttle = 0.0 # 当前油门 + current_steer = 0.0 # 当前转向 + current_brake = 0.0 # 当前刹车 + while not rospy.is_shutdown(): - key = self.get_key() - - if key is None: - continue - - control_msg = Twist() + # 检查是否有按键 + rlist, _, _ = select.select([sys.stdin], [], [], 0.1) - if key == 'w' or key == 'W': - control_msg.linear.x = 0.5 # 前进 - print("前进", end='\r') - elif key == 's' or key == 'S': - control_msg.linear.x = -0.3 # 后退 - print("后退", end='\r') - elif key == 'a' or key == 'A': - control_msg.angular.z = 0.5 # 左转 - print("左转", end='\r') - elif key == 'd' or key == 'D': - control_msg.angular.z = -0.5 # 右转 - print("右转", end='\r') - elif key == ' ': - control_msg.linear.z = 1.0 # 刹车 - print("刹车", end='\r') - elif key == 'q' or key == 'Q': - print("退出手动模式") - break + if rlist: + # 有按键 + key = sys.stdin.read(1).lower() + + if key == 'w': + current_throttle = 0.5 + current_brake = 0.0 + print("前进", end='\r') + elif key == 's': + current_throttle = -0.5 + current_brake = 0.0 + print("后退", end='\r') + elif key == 'a': + current_steer = -0.5 + print("左转", end='\r') + elif key == 'd': + current_steer = 0.5 + print("右转", end='\r') + elif key == ' ': + current_throttle = 0.0 + current_steer = 0.0 + current_brake = 1.0 + print("刹车", end='\r') + elif key == 'q': + print("\n退出") + break + else: + # 未知按键,忽略 + continue else: - continue + # 没有按键,转向自动回正 + current_steer = 0.0 + # 油门保持当前状态 + print("保持...", end='\r') - self.control_pub.publish(control_msg) + # 设置控制消息 + control_msg.linear.x = current_throttle + control_msg.angular.z = current_steer + control_msg.linear.z = current_brake - # 添加延迟防止过度发布 - rospy.sleep(0.05) + # 发布控制消息 + self.control_pub.publish(control_msg) except Exception as e: rospy.logerr(f"手动控制出错: {e}") + finally: + # 恢复终端设置 + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + + # 确保车辆停止 + control_msg.linear.x = 0.0 + control_msg.angular.z = 0.0 + control_msg.linear.z = 1.0 + try: + self.control_pub.publish(control_msg) + except: + pass + print("\n退出手动模式") + + # 确保车辆停止 + control_msg.linear.x = 0.0 + control_msg.angular.z = 0.0 + control_msg.linear.z = 1.0 + self.control_pub.publish(control_msg) + print("\n退出手动模式") + + def manual_control_incremental(self): + """增量控制模式 - 更精确的控制""" + rospy.loginfo("进入增量控制模式") + rospy.loginfo("使用键盘控制车辆:") + rospy.loginfo(" W: 增加前进油门") + rospy.loginfo(" S: 增加后退油门") + rospy.loginfo(" A: 增加左转") + rospy.loginfo(" D: 增加右转") + rospy.loginfo(" E: 减少油门") + rospy.loginfo(" Space: 急刹车") + rospy.loginfo(" R: 重置控制") + rospy.loginfo(" Q: 退出") + + print("\n开始控制(按Q退出)...") + + control_msg = Twist() + control_msg.linear.x = 0.0 + control_msg.angular.z = 0.0 + control_msg.linear.z = 0.0 + + # 控制增量 + throttle_inc = 0.1 + steer_inc = 0.1 + + try: + while not rospy.is_shutdown(): + key = self.get_key() + + if key is not None: + if key == 'w' or key == 'W': + # 增加前进油门 + control_msg.linear.x += throttle_inc + control_msg.linear.x = min(1.0, control_msg.linear.x) + control_msg.linear.z = 0.0 + print(f"前进油门: {control_msg.linear.x:.2f}") + elif key == 's' or key == 'S': + # 增加后退油门(负值) + control_msg.linear.x -= throttle_inc + control_msg.linear.x = max(-1.0, control_msg.linear.x) + control_msg.linear.z = 0.0 + print(f"后退油门: {control_msg.linear.x:.2f}") + elif key == 'a' or key == 'A': + # 左转 + control_msg.angular.z -= steer_inc + control_msg.angular.z = min(-1.0, control_msg.angular.z) + print(f"左转: {control_msg.angular.z:.2f}") + elif key == 'd' or key == 'D': + # 右转 + control_msg.angular.z += steer_inc + control_msg.angular.z = max(1.0, control_msg.angular.z) + print(f"右转: {control_msg.angular.z:.2f}") + elif key == 'e' or key == 'E': + # 减少油门(绝对值) + if control_msg.linear.x > 0: + control_msg.linear.x -= throttle_inc + control_msg.linear.x = max(0, control_msg.linear.x) + elif control_msg.linear.x < 0: + control_msg.linear.x += throttle_inc + control_msg.linear.x = min(0, control_msg.linear.x) + print(f"油门: {control_msg.linear.x:.2f}") + elif key == ' ': + # 急刹车 + control_msg.linear.z = 1.0 + control_msg.linear.x = 0.0 + print("急刹车") + elif key == 'r' or key == 'R': + # 重置 + control_msg.linear.x = 0.0 + control_msg.angular.z = 0.0 + control_msg.linear.z = 0.0 + print("重置控制") + elif key == 'q' or key == 'Q': + # 退出 + control_msg.linear.x = 0.0 + control_msg.angular.z = 0.0 + control_msg.linear.z = 1.0 + self.control_pub.publish(control_msg) + print("\n退出增量控制") + break + + # 持续发布控制消息 + self.control_pub.publish(control_msg) + time.sleep(0.05) + + except Exception as e: + rospy.logerr(f"增量控制出错: {e}") + finally: + # 确保车辆停止 + control_msg.linear.x = 0.0 + control_msg.angular.z = 0.0 + control_msg.linear.z = 1.0 + self.control_pub.publish(control_msg) def start_autonomous_episode(self): """启动自主驾驶episode""" @@ -165,9 +315,10 @@ def run(self): print("="*50) print("\n控制命令:") print(" 1: 启动自主驾驶") - print(" 2: 手动控制") - print(" 3: 重置环境") - print(" 4: 停止") + print(" 2: 手动控制(按键控制)") + print(" 3: 增量控制(精确控制)") + print(" 4: 重置环境") + print(" 5: 停止") print(" 0: 退出") print("="*50) @@ -181,14 +332,16 @@ def run(self): elif command == '2': self.manual_control() elif command == '3': - self.reset_environment() + self.manual_control_incremental() elif command == '4': + self.reset_environment() + elif command == '5': self.stop_all() elif command == '0': rospy.loginfo("退出客户端") break else: - print("未知命令,请输入 0-4") + print("未知命令,请输入 0-5") except EOFError: break diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/car_env.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/car_env.py index 230a4e6cf1..e6f95b91a2 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/car_env.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/car_env.py @@ -87,7 +87,7 @@ class CarEnv: SHOW_CAM = SHOW_PREVIEW - STEER_AMT = 1.0 # actions that the agent can take [-1, 0, 1] --> [turn left, go straight, turn right] + STEER_AMT = 0.88 # actions that the agent can take [-1, 0, 1] --> [turn left, go straight, turn right] im_width = IM_WIDTH im_height = IM_HEIGHT front_camera = None @@ -171,6 +171,8 @@ def reset(self): for el in traj: self.path.append(el[0]) + self.visualize_path(self.path) + # 使用路径上的第一个点来确定正确的朝向 if len(self.path) > 0: first_waypoint = self.path[0] @@ -277,7 +279,85 @@ def reset(self): # 返回初始状态,车辆保持静止 return [(self.distance-300)/300, -1, 0, 0] - # 其余方法保持不变... + + def visualize_path(self, path_points, color=None): + """可视化路径""" + try: + if color is None: + color = carla.Color(255, 0, 0) # 红色 + + # 绘制路径点 + for i, waypoint in enumerate(path_points): + location = waypoint.transform.location + + # 每10个点画一个标记 + if i % 10 == 0: + self.world.debug.draw_string( + carla.Location(location.x, location.y, location.z + 0.5), + '→', + draw_shadow=False, + color=carla.Color(0, 255, 0), + life_time=100.0, + persistent_lines=True + ) + + # 画线连接路径点 + if i < len(path_points) - 1: + next_location = path_points[i + 1].transform.location + self.world.debug.draw_line( + carla.Location(location.x, location.y, location.z + 0.3), + carla.Location(next_location.x, next_location.y, next_location.z + 0.3), + thickness=0.1, + color=color, + life_time=100.0, + persistent_lines=True + ) + + # 绘制起点和终点 + if path_points: + start = path_points[0].transform.location + end = path_points[-1].transform.location + + # 起点标记 + self.world.debug.draw_point( + carla.Location(start.x, start.y, start.z + 0.5), + size=0.2, + color=carla.Color(0, 255, 0), # 绿色 + life_time=100.0, + persistent_lines=True + ) + + # 终点标记 + self.world.debug.draw_point( + carla.Location(end.x, end.y, end.z + 0.5), + size=0.2, + color=carla.Color(255, 0, 0), # 红色 + life_time=100.0, + persistent_lines=True + ) + + # 添加文字标签 + self.world.debug.draw_string( + carla.Location(start.x, start.y, start.z + 2.0), + 'START', + draw_shadow=True, + color=carla.Color(255, 255, 255), + life_time=100.0 + ) + + self.world.debug.draw_string( + carla.Location(end.x, end.y, end.z + 2.0), + 'GOAL', + draw_shadow=True, + color=carla.Color(255, 255, 255), + life_time=100.0 + ) + + print(f"路径可视化完成,共绘制 {len(path_points)} 个点") + + except Exception as e: + print(f"路径可视化失败: {e}") + def collision_data(self, event): self.collision_history.append(event) @@ -421,26 +501,50 @@ def step(self, action, current_state): ''' To take 6 actions; brake, go straight, turn left, turn right, turn slightly left, turn slightly right ''' - # 应用动作控制 + # 根据当前速度和横向偏差动态调整转向灵敏度 + v = self.vehicle.get_velocity() + speed = 3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2) + + # 动态转向系数:速度越高,转向越小 + if speed > 30: # 高速时 + steer_factor = 0.5 + elif speed > 15: # 中速时 + steer_factor = 0.7 + else: # 低速时 + steer_factor = 1.0 + + # 应用动作控制 - 减小等待时间 if action == 0: self.vehicle.apply_control(carla.VehicleControl(throttle=0, brake=1.0)) print("执行动作: 刹车") elif action == 1: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer= 0*self.STEER_AMT)) + # 直行时稍微增加油门,避免停车 + self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0)) print("执行动作: 直行") elif action == 2: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=-0.3*self.STEER_AMT)) + # 根据速度动态调整转向幅度 + steer_value = -0.25 * steer_factor + throttle_value = 0.3 if speed < 10 else 0.1 # 低速时给更多油门 + self.vehicle.apply_control(carla.VehicleControl(throttle=throttle_value, steer=steer_value)) print("执行动作: 左转") elif action == 3: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer= 0.3*self.STEER_AMT)) + steer_value = 0.25 * steer_factor + throttle_value = 0.3 if speed < 10 else 0.1 + self.vehicle.apply_control(carla.VehicleControl(throttle=throttle_value, steer=steer_value)) print("执行动作: 右转") elif action == 4: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=-0.1*self.STEER_AMT)) + steer_value = -0.15 * steer_factor + throttle_value = 0.4 + self.vehicle.apply_control(carla.VehicleControl(throttle=throttle_value, steer=steer_value)) print("执行动作: 微左") elif action == 5: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer= 0.1*self.STEER_AMT)) + steer_value = 0.15 * steer_factor + throttle_value = 0.4 + self.vehicle.apply_control(carla.VehicleControl(throttle=throttle_value, steer=steer_value)) print("执行动作: 微右") + time.sleep(0.01) + # 处理图像 self.process_images() @@ -467,31 +571,83 @@ def step(self, action, current_state): # 计算角度差 waypoint_rot = current_waypoint.transform.rotation orientation_diff = waypoint_rot.yaw - rot.yaw - phi = orientation_diff % 360 - 360 * (orientation_diff % 360 > 180) + phi = ((orientation_diff + 180) % 360) - 180 # 计算横向偏差 if waypoint_ind < len(self.path) - 1: next_waypoint = self.path[waypoint_ind + 1] waypoint_loc = current_waypoint.transform.location next_waypoint_loc = next_waypoint.transform.location + vehicle_loc = pos - u = [waypoint_loc.x - next_waypoint_loc.x, waypoint_loc.y - next_waypoint_loc.y] - v = [pos.x - waypoint_loc.x, pos.y - waypoint_loc.y] - - if np.linalg.norm(u) > 0.1 and np.linalg.norm(v) > 0.1: - signed_dis = np.linalg.norm(v) * np.sin(np.sign(np.cross(u,v)) * np.arccos(np.dot(u,v)/(np.linalg.norm(u)*np.linalg.norm(v)))) + # 计算路径向量 + u = np.array([next_waypoint_loc.x - waypoint_loc.x, + next_waypoint_loc.y - waypoint_loc.y]) + v = np.array([pos.x - waypoint_loc.x, + pos.y - waypoint_loc.y]) + + if np.linalg.norm(u) > 0.1: + # 计算投影长度 + t = np.dot(u, v) / np.dot(u, u) + t = max(0, min(1, t)) # 限制在0-1之间 + + # 计算最近点 + closest_point = np.array([waypoint_loc.x, waypoint_loc.y]) + t * u + + # 计算横向偏差 + lateral_vec = v - t * u + signed_dis = np.linalg.norm(lateral_vec) + + # 判断偏差方向 + cross_product = np.cross(u, v) + if cross_product > 0: + signed_dis = -signed_dis # 车辆在路径左侧 else: signed_dis = 0 else: signed_dis = 0 - current_state[3] = current_state[3]/15 + current_state[0] = (self.distance-300)/300 + current_state[1] = (kmh-30)/30 - (self.distance-300)/300 + current_state[2] = phi + current_state[3] = signed_dis * 15 print(f"角度差: {phi:.2f}°, 横向偏差: {signed_dis:.2f}m") - print(f"距离障碍物: {current_state[0]*300+300:.2f}m") - print(f"速度: {(current_state[1]+current_state[0])*30+30:.2f}km/h") - - # [原有的奖励计算代码保持不变...] + print(f"距离障碍物: {self.distance:.2f}m") + print(f"速度: {kmh:.2f}km/h") + print(f"距离终点: {dist_from_goal:.2f}m") + + # 奖励计算 - 更精细的奖励函数 + base_reward = 0.1 # 基础生存奖励 + + # 速度奖励(鼓励合理速度20-40 km/h) + if 20 <= kmh <= 40: + speed_reward = 0.2 + elif kmh < 5: + speed_reward = -0.1 # 太慢惩罚 + else: + speed_reward = 0.1 * (kmh / 40.0) + + # 路径跟随奖励 + heading_penalty = abs(phi) / 180.0 # 角度偏差惩罚 + lateral_penalty = min(abs(signed_dis) / 3.0, 1.0) # 横向偏差惩罚 + + path_follow_reward = 0.3 * (1.0 - heading_penalty) + 0.3 * (1.0 - lateral_penalty) + + # 进度奖励 + if not hasattr(self, 'initial_distance_to_goal'): + self.initial_distance_to_goal = dist_from_goal + + if self.initial_distance_to_goal > 0: + progress = 1.0 - (dist_from_goal / self.initial_distance_to_goal) + progress_reward = 0.2 * max(0, progress) + else: + progress_reward = 0 + + # 距离障碍物奖励 + obstacle_reward = 0.2 * max(0, 1.0 - self.distance / 50.0) if self.distance < 50 else 0 + + reward = base_reward + speed_reward + path_follow_reward + progress_reward + obstacle_reward # 检查是否完成 if len(self.collision_history) != 0: @@ -515,15 +671,19 @@ def step(self, action, current_state): reward = 1000 print("✅ 成功到达终点!") - # 超时检查 - if self.episode_start + 200 < time.time(): - done = True - print("⏰ 超时") print(f"奖励: {reward}") + + self.world.debug.draw_string( + carla.Location(pos.x, pos.y, pos.z + 2.0), + f'V: {kmh}km/h', + draw_shadow=True, + color=carla.Color(255, 255, 0), + life_time=0.1 + ) # 返回新的状态、奖励、完成标志和当前路径点 - return [(self.distance-300)/300, (kmh-30)/30-(self.distance-300)/300, phi, signed_dis*15], reward, done, current_waypoint + return current_state, reward, done, current_waypoint def trajectory(self, draw = False): amap = self.world.get_map() @@ -576,23 +736,39 @@ def cleanup_self_actors(self): def get_closest_waypoint(self, waypoint_list, vehicle_transform): - """获取最近的路径点""" - closest_waypoint = 0 - closest_distance = float('inf') + """获取最近的路径点,优化搜索算法""" + if not waypoint_list: + return 0 + # 使用更高效的距离计算方法 vehicle_location = vehicle_transform.location + closest_waypoint = 0 + closest_distance_sq = float('inf') - for i, waypoint in enumerate(waypoint_list): - distance = math.sqrt( - (waypoint.transform.location.x - vehicle_location.x)**2 + - (waypoint.transform.location.y - vehicle_location.y)**2 - ) - if distance < closest_distance: + # 如果已经保存了上一次的最近点,从它附近开始搜索 + if hasattr(self, 'last_waypoint_index') and self.last_waypoint_index < len(waypoint_list): + start_index = max(0, self.last_waypoint_index - 10) + end_index = min(len(waypoint_list), self.last_waypoint_index + 20) + else: + start_index = 0 + end_index = min(50, len(waypoint_list)) # 限制搜索范围,提高速度 + + for i in range(start_index, end_index): + waypoint_location = waypoint_list[i].transform.location + dx = waypoint_location.x - vehicle_location.x + dy = waypoint_location.y - vehicle_location.y + distance_sq = dx*dx + dy*dy # 使用平方距离,避免开方运算 + + if distance_sq < closest_distance_sq: closest_waypoint = i - closest_distance = distance + closest_distance_sq = distance_sq + + # 保存最近的路径点索引,供下次使用 + self.last_waypoint_index = closest_waypoint - # 确保不会返回最后一个点(除非非常接近终点) - if closest_waypoint < len(waypoint_list) - 1: - return closest_waypoint + # 向前看几个点,避免卡在同一个点 + look_ahead = 5 + if closest_waypoint < len(waypoint_list) - look_ahead: + return closest_waypoint + look_ahead else: return len(waypoint_list) - 1 \ No newline at end of file diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config.py index daf94f0239..89e9c6e167 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config.py @@ -43,11 +43,21 @@ def get_current_trajectory(): # ==================== 动作配置 ==================== ACTION_NAMES = ["刹车", "直行", "左转", "右转", "微左", "微右"] +# ==================== 控制配置 ==================== +CONTROL_CONFIG = { + 'max_throttle': 1.0, # 最大油门 + 'max_steer': 1.0, # 最大转向 + 'max_brake': 1.0, # 最大刹车 + 'throttle_increment': 0.2, # 增加油门增量 + 'steer_increment': 0.1, # 转向增量 + 'idle_timeout': 1.0, # 空闲超时(秒) +} + # ==================== 训练配置 ==================== TOTAL_EPISODES = 3 # 总共运行的episode数 MAX_STEPS_PER_EPISODE = 2000 # 每个episode最大步数 EPISODE_INTERVAL = 2.0 # episode之间的间隔秒数 - +UPDATE_RATE = 0.1 # 降低更新率,让控制更频繁 # ==================== 交通配置 ==================== ENABLE_TRAFFIC = False # 是否启用交通流(暂时关闭,避免干扰) TRAFFIC_VEHICLES = 30 # 交通车辆数量 diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/main.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/main.py index 5dab272650..c40710b6df 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/main.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/main.py @@ -94,17 +94,26 @@ def load_models(): def predict_action(braking_model, driving_model, current_state, vehicle_state=None): """预测动作""" if not braking_model or not driving_model: - return 0 + return 1 # 默认直行而不是刹车 # 刹车模型预测 braking_state = np.array(current_state[:2]).reshape(1, -1) braking_qs = braking_model.predict(braking_state, verbose=0)[0] braking_action = np.argmax(braking_qs) + # 只有当距离障碍物很近时才刹车,否则继续前进 + if braking_action == 0 and current_state[0] > 0.3: # 距离障碍物较远 + braking_action = 1 # 改为直行 + # 如果安全,使用驾驶模型 if braking_action == 1: driving_state = np.array(current_state[2:]).reshape(1, -1) driving_qs = driving_model.predict(driving_state, verbose=0)[0] + # 避免总是选择同一个动作,增加一些探索性 + if np.random.random() > 0.7: # 30%的概率选择次优动作 + # 选择第二好的动作 + sorted_indices = np.argsort(driving_qs)[::-1] + return sorted_indices[1] + 1 return np.argmax(driving_qs) + 1 return 0 @@ -129,8 +138,8 @@ def run_episode(env, traffic_mgr, visualizer, tracker, episode_num): # 获取车辆 ego_vehicle = env.vehicle - if not ego_vehicle: - print("未找到车辆") + if not ego_vehicle or not ego_vehicle.is_alive: + print("未找到车辆或车辆已被销毁") return False # 设置后方跟随视角(启动独立线程) @@ -144,33 +153,71 @@ 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) + stuck_counter = 0 + max_stuck_steps = 10 # 减少卡住检测步数 + + # 性能监控 + total_control_time = 0 + control_count = 0 while not done and step_count < cfg.MAX_STEPS_PER_EPISODE: step_count += 1 step_start = time.time() - # 注意:不再调用 tracker.smooth_follow_vehicle(ego_vehicle) - # 视角更新由独立线程处理 + # 检查车辆是否被销毁 + if not ego_vehicle or not ego_vehicle.is_alive: + print("车辆已被销毁,终止episode") + break + + # 检查车辆是否卡住 + velocity = ego_vehicle.get_velocity() + speed = 3.6 * math.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) - # 更新车辆显示 - vehicle_state = tracker.get_vehicle_state(ego_vehicle) - if vehicle_state: - visualizer.update_vehicle_display( - vehicle_state['x'], - vehicle_state['y'], - vehicle_state['heading'] - ) + if speed < 1.0: # 速度小于1 km/h + stuck_counter += 1 + if stuck_counter >= max_stuck_steps: + print("车辆卡住,尝试全油门恢复...") + # 尝试全油门摆脱卡住状态 + ego_vehicle.apply_control(carla.VehicleControl(throttle=1.0, brake=0.0, steer=0.0)) + time.sleep(0.1) # 减少恢复时间 + stuck_counter = 0 + else: + stuck_counter = 0 + + # 更新车辆显示(降低频率) + if step_count % 10 == 0: # 每10步更新一次显示 + vehicle_state = tracker.get_vehicle_state(ego_vehicle) + if vehicle_state: + visualizer.update_vehicle_display( + vehicle_state['x'], + vehicle_state['y'], + vehicle_state['heading'] + ) # 预测并执行动作 + action_start = time.time() action = predict_action(braking_model, driving_model, current_state, vehicle_state) try: new_state, reward, done, _ = env.step(action, current_state) current_state = new_state + + # 计算控制延迟 + control_time = time.time() - action_start + total_control_time += control_time + control_count += 1 + + # 每50步报告一次性能 + if step_count % 50 == 0: + avg_control_time = total_control_time / control_count + print(f"平均控制延迟: {avg_control_time*1000:.1f} ms") + total_control_time = 0 + control_count = 0 + except Exception as e: print(f"执行动作失败: {e}") done = True @@ -179,10 +226,10 @@ def run_episode(env, traffic_mgr, visualizer, tracker, episode_num): frame_time = time.time() - step_start fps_counter.append(frame_time) - if cfg.DEBUG_MODE and step_count % 50 == 0: + if cfg.DEBUG_MODE and step_count % 100 == 0: fps = len(fps_counter) / sum(fps_counter) if fps_counter else 0 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]}") + print(f"步骤 {step_count}, FPS: {fps:.1f}, 速度: {vehicle_speed:.1f}m/s ({vehicle_speed*3.6:.1f}km/h)") if done: print(f"Episode {episode_num} 完成,步数: {step_count}")