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 3b3140a43e..1005b196d3 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 @@ -1,6 +1,6 @@ #!/usr/bin/env python3 """ -CARLA自动驾驶ROS节点 +CARLA自动驾驶ROS节点 - 完整修复版本 """ import rospy @@ -10,28 +10,49 @@ import threading import numpy as np -# 禁用TensorFlow警告 +# TensorFlow配置 os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' -os.environ['CUDA_VISIBLE_DEVICES'] = '-1' # 使用CPU +os.environ['CUDA_VISIBLE_DEVICES'] = '-1' # ROS消息 -from std_msgs.msg import Header, Float32, Bool, Int32, String +from std_msgs.msg import Header, Float32, Bool, Int32, String, Float32MultiArray from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 from sensor_msgs.msg import Image from nav_msgs.msg import Odometry, Path from visualization_msgs.msg import Marker, MarkerArray import tf from cv_bridge import CvBridge -from carla_autonomous.srv import StartEpisode, Stop, Reset + +# 导入服务消息 +try: + from carla_autonomous.srv import Reset, ResetResponse + from carla_autonomous.srv import StartEpisode, StartEpisodeResponse + from carla_autonomous.srv import Stop, StopResponse +except ImportError: + # 如果服务消息还未生成,创建简单的替代 + rospy.logwarn("服务消息未找到,使用简单替代") + from std_srvs.srv import Empty, EmptyResponse, Trigger, TriggerResponse + +# 添加项目路径 +current_dir = os.path.dirname(os.path.abspath(__file__)) +src_dir = os.path.join(current_dir, '..', 'src') +sys.path.insert(0, os.path.abspath(src_dir)) + # 导入项目模块 -project_dir = os.path.join(os.path.dirname(__file__), '..', 'src') -sys.path.insert(0, os.path.abspath(project_dir)) -from car_env import CarEnv -from route_visualizer import RouteVisualizer -from vehicle_tracker import VehicleTracker -from traffic_manager import TrafficManager -from model_manager import ModelManager -import config as cfg +try: + from car_env import CarEnv + from route_visualizer import RouteVisualizer + from vehicle_tracker import VehicleTracker + from traffic_manager import TrafficManager + from model_manager import ModelManager + import config as cfg + rospy.loginfo("成功导入项目模块") +except ImportError as e: + rospy.logerr(f"导入项目模块失败: {e}") + rospy.logerr(f"Python路径: {sys.path}") + import traceback + rospy.logerr(traceback.format_exc()) + sys.exit(1) class CarlaAutonomousROS: """CARLA自动驾驶ROS节点""" @@ -49,6 +70,8 @@ def __init__(self): if not self.trajectory: rospy.logerr("无法加载轨迹配置") sys.exit(1) + + rospy.loginfo(f"使用轨迹: {self.trajectory['description']}") # ROS发布器 self.vehicle_state_pub = rospy.Publisher('/carla/vehicle_state', Odometry, queue_size=10) @@ -60,10 +83,19 @@ def __init__(self): self.status_pub = rospy.Publisher('/carla/status', String, queue_size=10) self.reward_pub = rospy.Publisher('/carla/reward', Float32, queue_size=10) - # ROS服务 - self.reset_srv = rospy.Service('/carla/reset', Reset, self.handle_reset) - self.start_srv = rospy.Service('/carla/start_episode', StartEpisode, self.handle_start_episode) - self.stop_srv = rospy.Service('/carla/stop', Stop, self.handle_stop) + # ROS服务 - 使用简单的替代方案 + try: + # 尝试使用自定义服务 + self.reset_srv = rospy.Service('/carla/reset', Reset, self.handle_reset) + self.start_srv = rospy.Service('/carla/start_episode', StartEpisode, self.handle_start_episode) + self.stop_srv = rospy.Service('/carla/stop', Stop, self.handle_stop) + rospy.loginfo("使用自定义服务") + except: + # 使用标准ROS服务 + rospy.loginfo("使用标准ROS服务(Empty/Trigger)") + self.reset_srv = rospy.Service('/carla/reset', Empty, self.handle_reset_simple) + self.start_srv = rospy.Service('/carla/start_episode', Trigger, self.handle_start_simple) + self.stop_srv = rospy.Service('/carla/stop', Trigger, self.handle_stop_simple) # ROS订阅器 rospy.Subscriber('/carla/control_cmd', Twist, self.control_callback) @@ -81,19 +113,28 @@ def __init__(self): self.total_reward = 0.0 self.step_count = 0 + # 手动控制标志 + self.manual_control_active = False + self.last_control_msg = None + # 线程锁 self.lock = threading.Lock() # 初始化CARLA环境 - self.init_carla_environment() + if not self.init_carla_environment(): + rospy.logerr("CARLA环境初始化失败,节点将退出") + sys.exit(1) 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() @@ -101,12 +142,14 @@ def init_carla_environment(self): 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, @@ -119,29 +162,234 @@ def init_carla_environment(self): except Exception as e: rospy.logerr(f"初始化CARLA环境失败: {e}") + import traceback + rospy.logerr(traceback.format_exc()) return False def control_callback(self, msg): """控制命令回调""" - if not self.env or not self.env.vehicle: + if not self.env or not hasattr(self.env, 'vehicle') or self.env.vehicle is None: + rospy.logwarn("无法处理控制命令:车辆未就绪") return try: - # 将ROS Twist消息转换为CARLA控制 - control = carla.VehicleControl() - control.throttle = max(0.0, min(1.0, msg.linear.x)) - control.steer = max(-1.0, min(1.0, msg.angular.z)) - control.brake = max(0.0, min(1.0, msg.linear.z)) + self.last_control_msg = msg + self.manual_control_active = True - # 应用控制 - self.env.vehicle.apply_control(control) + # 记录控制命令 + rospy.logdebug(f"收到控制命令: throttle={msg.linear.x:.2f}, steer={msg.angular.z:.2f}, brake={msg.linear.z:.2f}") except Exception as e: rospy.logwarn(f"控制命令处理失败: {e}") + def apply_control(self): + """应用当前控制""" + if not self.env or not self.env.vehicle: + return + + try: + if self.manual_control_active and self.last_control_msg: + # 手动控制模式 + import carla + control = carla.VehicleControl() + control.throttle = max(0.0, min(1.0, self.last_control_msg.linear.x)) + control.steer = max(-1.0, min(1.0, self.last_control_msg.angular.z)) + control.brake = max(0.0, min(1.0, self.last_control_msg.linear.z)) + + self.env.vehicle.apply_control(control) + elif self.running: + # 自主驾驶模式 - 由run_episode处理 + pass + else: + # 停止车辆 + import carla + control = carla.VehicleControl() + control.throttle = 0.0 + control.brake = 1.0 + self.env.vehicle.apply_control(control) + + except Exception as e: + rospy.logwarn(f"应用控制失败: {e}") + + def handle_reset(self, req): + """重置环境服务处理(自定义服务)""" + with self.lock: + try: + rospy.loginfo("收到重置环境请求") + + # 重置环境 + if self.env: + current_state = self.env.reset() + + # 转换为Float32MultiArray + state_array = Float32MultiArray() + state_array.data = current_state + + self.publish_status("环境重置成功") + + return ResetResponse(state_array) + else: + rospy.logerr("环境未初始化") + 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): + """重置环境服务处理(标准服务)""" + with self.lock: + try: + rospy.loginfo("重置环境") + if self.env: + self.env.reset() + self.publish_status("环境重置成功") + return EmptyResponse() + except Exception as e: + rospy.logerr(f"重置失败: {e}") + return EmptyResponse() + + def handle_start_episode(self, req): + """开始episode服务处理(自定义服务)""" + with self.lock: + if self.running: + rospy.logwarn("已经有episode在运行") + return StartEpisodeResponse(False, "已有episode在运行") + + try: + self.running = True + self.current_episode += 1 + self.total_reward = 0.0 + self.step_count = 0 + self.manual_control_active = False + + rospy.loginfo(f"开始Episode {self.current_episode}") + + # 在新线程中运行episode + episode_thread = threading.Thread(target=self.run_episode) + episode_thread.daemon = True + episode_thread.start() + + return StartEpisodeResponse(True, f"Episode {self.current_episode} 已开始") + + except Exception as e: + rospy.logerr(f"开始episode失败: {e}") + self.running = False + return StartEpisodeResponse(False, str(e)) + + def handle_start_simple(self, req): + """开始episode服务处理(标准服务)""" + with self.lock: + try: + if not self.running: + self.running = True + self.current_episode += 1 + self.total_reward = 0.0 + self.step_count = 0 + self.manual_control_active = False + + # 在新线程中运行episode + episode_thread = threading.Thread(target=self.run_episode) + episode_thread.daemon = True + episode_thread.start() + + return TriggerResponse(True, f"Episode {self.current_episode} started") + else: + return TriggerResponse(False, "Already running") + except Exception as e: + return TriggerResponse(False, str(e)) + + def handle_stop(self, req): + """停止服务处理(自定义服务)""" + with self.lock: + rospy.loginfo("收到停止请求") + self.running = False + self.manual_control_active = False + return StopResponse(True, "已停止") + + def handle_stop_simple(self, req): + """停止服务处理(标准服务)""" + with self.lock: + rospy.loginfo("停止") + self.running = False + self.manual_control_active = False + return TriggerResponse(True, "Stopped") + + def run_episode(self): + """运行一个episode""" + rospy.loginfo(f"开始运行Episode {self.current_episode}") + + try: + # 重置环境 + if not self.env: + rospy.logerr("环境未初始化") + return + + current_state = self.env.reset() + + # 设置车辆跟踪 + if self.env.vehicle: + self.tracker.set_follow_view(self.env.vehicle) + + # 主循环 + done = False + 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}") + + # 执行动作 + new_state, reward, done, _ = self.env.step(action, current_state) + + # 更新统计 + 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 + 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() + + # 发布最终状态 + status_msg = f"Episode {self.current_episode} 结束,步数: {self.step_count},总奖励: {self.total_reward:.2f}" + self.publish_status(status_msg) + rospy.loginfo(status_msg) + + # 重置运行标志 + self.running = False + + except Exception as e: + rospy.logerr(f"运行episode时出错: {e}") + import traceback + rospy.logerr(traceback.format_exc()) + self.running = False + def publish_vehicle_state(self): """发布车辆状态""" - if not self.env or not self.env.vehicle: + if not self.env or not hasattr(self.env, 'vehicle') or self.env.vehicle is None: return try: @@ -197,16 +445,18 @@ def publish_camera_images(self): image_msg = self.cv_bridge.cv2_to_imgmsg(cv_image, encoding="bgr8") image_msg.header.stamp = rospy.Time.now() + image_msg.header.frame_id = "camera" self.camera_image_pub.publish(image_msg) # 分割图像 if hasattr(self.env, 'seg_array') and self.env.seg_array is not None: seg_msg = self.cv_bridge.cv2_to_imgmsg(self.env.seg_array, encoding="bgr8") seg_msg.header.stamp = rospy.Time.now() + seg_msg.header.frame_id = "camera" self.seg_image_pub.publish(seg_msg) except Exception as e: - rospy.logwarn(f"发布图像失败: {e}") + rospy.logdebug(f"发布图像失败: {e}") def publish_planned_path(self): """发布规划路径""" @@ -231,11 +481,11 @@ def publish_planned_path(self): self.path_pub.publish(path_msg) except Exception as e: - rospy.logwarn(f"发布规划路径失败: {e}") + rospy.logdebug(f"发布规划路径失败: {e}") def publish_visualization_markers(self): """发布可视化标记""" - if not self.env or not self.env.vehicle: + if not self.env or not hasattr(self.env, 'vehicle') or self.env.vehicle is None: return try: @@ -302,7 +552,7 @@ def publish_visualization_markers(self): self.marker_pub.publish(marker_array) except Exception as e: - rospy.logwarn(f"发布可视化标记失败: {e}") + rospy.logdebug(f"发布可视化标记失败: {e}") def publish_status(self, status_msg): """发布状态信息""" @@ -313,122 +563,6 @@ def publish_status(self, status_msg): except Exception as e: rospy.logwarn(f"发布状态失败: {e}") - def handle_reset(self, req): - """重置环境服务处理""" - with self.lock: - try: - rospy.loginfo("收到重置环境请求") - - # 重置环境 - if self.env: - current_state = self.env.reset() - self.publish_status("环境重置成功") - - # 返回当前状态 - return ResetResponse(current_state) - else: - rospy.logerr("环境未初始化") - return ResetResponse([]) - - except Exception as e: - rospy.logerr(f"重置环境失败: {e}") - return ResetResponse([]) - - def handle_start_episode(self, req): - """开始episode服务处理""" - with self.lock: - if self.running: - rospy.logwarn("已经有episode在运行") - return StartEpisodeResponse(False, "已有episode在运行") - - try: - self.running = True - self.current_episode += 1 - self.total_reward = 0.0 - self.step_count = 0 - - rospy.loginfo(f"开始Episode {self.current_episode}") - - # 在新线程中运行episode - episode_thread = threading.Thread(target=self.run_episode) - episode_thread.start() - - return StartEpisodeResponse(True, f"Episode {self.current_episode} 已开始") - - except Exception as e: - rospy.logerr(f"开始episode失败: {e}") - self.running = False - return StartEpisodeResponse(False, str(e)) - - def handle_stop(self, req): - """停止服务处理""" - with self.lock: - rospy.loginfo("收到停止请求") - self.running = False - return StopResponse(True, "已停止") - - def run_episode(self): - """运行一个episode""" - rospy.loginfo(f"开始运行Episode {self.current_episode}") - - try: - # 重置环境 - current_state = self.env.reset() - - # 设置车辆跟踪 - self.tracker.set_follow_view(self.env.vehicle) - - # 主循环 - done = False - while not done and not rospy.is_shutdown() and self.running: - if self.step_count >= cfg.MAX_STEPS_PER_EPISODE: - rospy.loginfo("达到最大步数") - break - - # 模型预测动作 - action = self.model_manager.predict_action(current_state) - - # 执行动作 - new_state, reward, done, _ = self.env.step(action, current_state) - - # 更新统计 - 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 - - # 发布数据 - self.publish_all_data() - - # 检查ROS是否正常 - if rospy.is_shutdown(): - break - - # 控制循环频率 - self.rate.sleep() - - # 发布最终状态 - if done: - status_msg = f"Episode {self.current_episode} 完成,总奖励: {self.total_reward:.2f}" - else: - status_msg = f"Episode {self.current_episode} 中断,步数: {self.step_count}" - - self.publish_status(status_msg) - rospy.loginfo(status_msg) - - # 重置运行标志 - self.running = False - - except Exception as e: - rospy.logerr(f"运行episode时出错: {e}") - self.running = False - def publish_all_data(self): """发布所有数据""" try: @@ -436,6 +570,10 @@ def publish_all_data(self): self.publish_camera_images() self.publish_planned_path() self.publish_visualization_markers() + + # 应用控制 + self.apply_control() + except Exception as e: rospy.logwarn(f"发布数据时出错: {e}") @@ -452,7 +590,7 @@ def run(self): # 主循环 while not rospy.is_shutdown(): try: - if self.running and self.env and self.env.vehicle: + if self.env and hasattr(self.env, 'vehicle') and self.env.vehicle: # 发布数据 self.publish_all_data() @@ -463,6 +601,8 @@ def run(self): break except Exception as e: rospy.logerr(f"主循环出错: {e}") + import traceback + rospy.logerr(traceback.format_exc()) time.sleep(1.0) # 清理 @@ -491,9 +631,12 @@ def cleanup(self): if __name__ == '__main__': try: + rospy.loginfo("启动CARLA自动驾驶ROS节点...") node = CarlaAutonomousROS() node.run() except rospy.ROSInterruptException: pass except Exception as e: - rospy.logerr(f"节点启动失败: {e}") \ No newline at end of file + rospy.logerr(f"节点启动失败: {e}") + import traceback + rospy.logerr(traceback.format_exc()) \ 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_control_client.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_control_client.py index 0b6b82a93c..6612b12c4d 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 @@ -1,27 +1,68 @@ #!/usr/bin/env python3 """ -CARLA控制客户端 - 用于手动控制或外部控制 +CARLA控制客户端 - 修复版 """ import rospy -from std_msgs.msg import String, Float32 +import sys +import select +import tty +import termios +from std_msgs.msg import String from geometry_msgs.msg import Twist -from carla_autonomous.srv import StartEpisode, Reset, Stop + +# 尝试导入自定义服务,如果失败使用标准服务 +try: + from carla_autonomous.srv import StartEpisode, Reset, Stop + USE_CUSTOM_SERVICES = True +except ImportError: + from std_srvs.srv import Empty, Trigger + USE_CUSTOM_SERVICES = False + rospy.logwarn("使用标准ROS服务") class CarlaControlClient: def __init__(self): - rospy.init_node('carla_control_client') - - # 服务代理 - self.start_episode = rospy.ServiceProxy('/carla/start_episode', StartEpisode) - self.reset = rospy.ServiceProxy('/carla/reset', Reset) - self.stop = rospy.ServiceProxy('/carla/stop', Stop) + rospy.init_node('carla_control_client', anonymous=True) # 控制发布器 self.control_pub = rospy.Publisher('/carla/control_cmd', Twist, queue_size=10) + # 等待服务可用 + rospy.loginfo("等待服务...") + + if USE_CUSTOM_SERVICES: + rospy.wait_for_service('/carla/start_episode') + rospy.wait_for_service('/carla/reset') + rospy.wait_for_service('/carla/stop') + + self.start_episode = rospy.ServiceProxy('/carla/start_episode', StartEpisode) + self.reset = rospy.ServiceProxy('/carla/reset', Reset) + self.stop = rospy.ServiceProxy('/carla/stop', Stop) + else: + rospy.wait_for_service('/carla/start_episode') + rospy.wait_for_service('/carla/reset') + rospy.wait_for_service('/carla/stop') + + self.start_episode = rospy.ServiceProxy('/carla/start_episode', Trigger) + self.reset = rospy.ServiceProxy('/carla/reset', Empty) + self.stop = rospy.ServiceProxy('/carla/stop', Trigger) + rospy.loginfo("CARLA控制客户端已启动") + def get_key(self): + """获取单个按键""" + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(sys.stdin.fileno()) + if select.select([sys.stdin], [], [], 0.1)[0]: + key = sys.stdin.read(1) + else: + key = None + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return key + def manual_control(self): """手动控制模式""" rospy.loginfo("进入手动控制模式") @@ -29,66 +70,74 @@ def manual_control(self): rospy.loginfo(" W/S: 前进/后退") rospy.loginfo(" A/D: 左转/右转") rospy.loginfo(" Space: 刹车") - rospy.loginfo(" Q: 退出") + rospy.loginfo(" Q: 退出手动模式") + + print("\n开始控制(按Q退出)...") try: - import curses - stdscr = curses.initscr() - curses.noecho() - curses.cbreak() - stdscr.keypad(True) - while not rospy.is_shutdown(): - key = stdscr.getch() + key = self.get_key() + + if key is None: + continue control_msg = Twist() - if key == ord('w'): + if key == 'w' or key == 'W': control_msg.linear.x = 0.5 # 前进 - elif key == ord('s'): + print("前进", end='\r') + elif key == 's' or key == 'S': control_msg.linear.x = -0.3 # 后退 - elif key == ord('a'): + print("后退", end='\r') + elif key == 'a' or key == 'A': control_msg.angular.z = 0.5 # 左转 - elif key == ord('d'): + print("左转", end='\r') + elif key == 'd' or key == 'D': control_msg.angular.z = -0.5 # 右转 - elif key == ord(' '): + print("右转", end='\r') + elif key == ' ': control_msg.linear.z = 1.0 # 刹车 - elif key == ord('q'): + print("刹车", end='\r') + elif key == 'q' or key == 'Q': + print("退出手动模式") break else: continue self.control_pub.publish(control_msg) - except ImportError: - rospy.logwarn("curses模块未安装,无法使用键盘控制") + # 添加延迟防止过度发布 + rospy.sleep(0.05) + except Exception as e: rospy.logerr(f"手动控制出错: {e}") - finally: - try: - curses.nocbreak() - stdscr.keypad(False) - curses.echo() - curses.endwin() - except: - pass def start_autonomous_episode(self): """启动自主驾驶episode""" try: response = self.start_episode() - if response.success: - rospy.loginfo(f"启动成功: {response.message}") + if USE_CUSTOM_SERVICES: + if response.success: + rospy.loginfo(f"启动成功: {response.message}") + else: + rospy.logwarn(f"启动失败: {response.message}") else: - rospy.logwarn(f"启动失败: {response.message}") + if response.success: + rospy.loginfo(f"启动成功: {response.message}") + else: + rospy.logwarn(f"启动失败: {response.message}") except rospy.ServiceException as e: rospy.logerr(f"服务调用失败: {e}") def reset_environment(self): """重置环境""" try: - response = self.reset() - rospy.loginfo("环境重置成功") + if USE_CUSTOM_SERVICES: + response = self.reset() + rospy.loginfo("环境重置成功") + else: + self.reset() + rospy.loginfo("环境重置成功") except rospy.ServiceException as e: rospy.logerr(f"重置服务调用失败: {e}") @@ -96,36 +145,55 @@ def stop_all(self): """停止所有""" try: response = self.stop() - rospy.loginfo("停止成功") + if USE_CUSTOM_SERVICES: + if response.success: + rospy.loginfo(f"停止成功: {response.message}") + else: + rospy.logwarn(f"停止失败: {response.message}") + else: + if response.success: + rospy.loginfo(f"停止成功: {response.message}") + else: + rospy.logwarn(f"停止失败: {response.message}") except rospy.ServiceException as e: rospy.logerr(f"停止服务调用失败: {e}") def run(self): """运行客户端""" - rospy.loginfo("控制命令:") - rospy.loginfo(" 1: 启动自主驾驶") - rospy.loginfo(" 2: 手动控制") - rospy.loginfo(" 3: 重置环境") - rospy.loginfo(" 4: 停止") - rospy.loginfo(" 0: 退出") + print("\n" + "="*50) + print("CARLA自动驾驶控制客户端") + print("="*50) + print("\n控制命令:") + print(" 1: 启动自主驾驶") + print(" 2: 手动控制") + print(" 3: 重置环境") + print(" 4: 停止") + print(" 0: 退出") + print("="*50) try: while not rospy.is_shutdown(): - command = input("请输入命令: ") - - if command == '1': - self.start_autonomous_episode() - elif command == '2': - self.manual_control() - elif command == '3': - self.reset_environment() - elif command == '4': - self.stop_all() - elif command == '0': - rospy.loginfo("退出客户端") + try: + command = input("\n请输入命令: ") + + if command == '1': + self.start_autonomous_episode() + elif command == '2': + self.manual_control() + elif command == '3': + self.reset_environment() + elif command == '4': + self.stop_all() + elif command == '0': + rospy.loginfo("退出客户端") + break + else: + print("未知命令,请输入 0-4") + + except EOFError: break - else: - rospy.logwarn("未知命令") + except Exception as e: + rospy.logerr(f"命令处理出错: {e}") except KeyboardInterrupt: rospy.loginfo("客户端已停止") diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/test_services.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/test_services.py new file mode 100644 index 0000000000..d67b3ad6d2 --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/test_services.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 +""" +测试ROS服务 +""" + +import rospy +from carla_autonomous.srv import StartEpisode, Reset, Stop + +def test_services(): + rospy.init_node('test_client') + + # 等待服务可用 + rospy.loginfo("等待服务...") + rospy.wait_for_service('/carla/start_episode') + rospy.wait_for_service('/carla/reset') + rospy.wait_for_service('/carla/stop') + + try: + # 测试重置服务 + reset_service = rospy.ServiceProxy('/carla/reset', Reset) + response = reset_service() + rospy.loginfo(f"重置服务响应: {response}") + + # 测试开始episode服务 + start_service = rospy.ServiceProxy('/carla/start_episode', StartEpisode) + response = start_service() + rospy.loginfo(f"开始episode响应: {response.success}, {response.message}") + + # 等待5秒 + rospy.sleep(5) + + # 测试停止服务 + stop_service = rospy.ServiceProxy('/carla/stop', Stop) + response = stop_service() + rospy.loginfo(f"停止服务响应: {response.success}, {response.message}") + + except rospy.ServiceException as e: + rospy.logerr(f"服务调用失败: {e}") + +if __name__ == '__main__': + test_services() \ No newline at end of file diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/setup.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/setup.py new file mode 100644 index 0000000000..36b5b3ea1a --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['carla_autonomous'], + package_dir={'': 'src'}, +) + +setup(**setup_args) \ No newline at end of file diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/__init__.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/__init__.py new file mode 100644 index 0000000000..2001b00a8f --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/__init__.py @@ -0,0 +1 @@ +# CARLA Autonomous Driving ROS package \ 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 cd89a466af..daf94f0239 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 @@ -17,7 +17,7 @@ } # 当前使用的轨迹 -CURRENT_TRAJECTORY = "test_trajectory" +CURRENT_TRAJECTORY = "custom_trajectory" def get_current_trajectory(): """获取当前轨迹配置""" diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Reset.srv b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Reset.srv index e96055c32a..701fd3956c 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Reset.srv +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Reset.srv @@ -1,2 +1,4 @@ +# 请求部分(空) --- +# 响应部分 std_msgs/Float32MultiArray state \ No newline at end of file diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/StartEpisode.srv b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/StartEpisode.srv index 9710dbf6a8..f1129ea70c 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/StartEpisode.srv +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/StartEpisode.srv @@ -1,3 +1,5 @@ +# 请求部分(空) +--- +# 响应部分 bool success -string message ---- \ No newline at end of file +string message \ No newline at end of file diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Stop.srv b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Stop.srv index cf5bd38fdb..f1129ea70c 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Stop.srv +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Stop.srv @@ -1,3 +1,5 @@ +# 请求部分(空) --- +# 响应部分 bool success string message \ No newline at end of file