diff --git a/main.py b/main.py index 6d79ce8..69a1169 100644 --- a/main.py +++ b/main.py @@ -37,11 +37,15 @@ def main(): time.sleep(100) except KeyboardInterrupt: - pass - - Ctrl.quit() - image_processor.destroy() - sys.exit() + print("\n程序被用户中断") + except Exception as e: + print(f"发生错误: {e}") + finally: + print("正在清理资源...") + Ctrl.quit() + image_processor.destroy() + print("程序已退出") + sys.exit() class Robot_Ctrl(object): diff --git a/utils/image_raw.py b/utils/image_raw.py index ccac59f..57dada7 100644 --- a/utils/image_raw.py +++ b/utils/image_raw.py @@ -7,6 +7,7 @@ from cv_bridge import CvBridge import cv2 from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy from qreader import QReader +from threading import Thread class ImageSubscriber(Node): @@ -43,14 +44,24 @@ class ImageProcessor: rclpy.init() self.image_subscriber = ImageSubscriber() self.qreader = QReader() + self.spin_thread = None + self.running = True def run(self): + self.spin_thread = Thread(target=self._spin) + self.spin_thread.start() + + def _spin(self): try: - rclpy.spin(self.image_subscriber) - except KeyboardInterrupt: - pass + while self.running: + rclpy.spin_once(self.image_subscriber, timeout_sec=0.1) + except Exception as e: + print(f"Spin thread error: {e}") def destroy(self): + self.running = False + if self.spin_thread: + self.spin_thread.join() self.image_subscriber.destroy_node() rclpy.shutdown()