Skip to content

Commit e9d09f3

Browse files
committed
Full remove QT dependencies from camera_node
1 parent b192af9 commit e9d09f3

File tree

1 file changed

+14
-19
lines changed

1 file changed

+14
-19
lines changed

coffee_ws/src/coffee_vision/coffee_vision/camera_node.py

Lines changed: 14 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
import json
1212
import collections
1313
from rclpy.node import Node
14-
from python_qt_binding.QtCore import pyqtSignal, QObject
14+
1515
from std_msgs.msg import Float32MultiArray, String, Bool, Int32
1616
from sensor_msgs.msg import Image
1717
from geometry_msgs.msg import Point
@@ -22,13 +22,10 @@
2222
os.makedirs(MODELS_DIR, exist_ok=True)
2323

2424

25-
class FrameGrabber(QObject):
25+
class FrameGrabber:
2626
"""Dedicated thread for frame capture to improve performance"""
27-
frame_ready = pyqtSignal(np.ndarray)
28-
error = pyqtSignal(str)
2927

3028
def __init__(self, node=None):
31-
super().__init__()
3229
self.node = node
3330
# Camera properties
3431
self.camera = None
@@ -64,9 +61,8 @@ def __init__(self, node=None):
6461
self.frame_lock = threading.Lock()
6562
self.frame_timestamp = 0 # Track frame freshness
6663

67-
# UI frame rate control
68-
self.last_ui_update = 0
69-
self.min_ui_interval = 1.0 / 30 # Target 30 FPS for UI
64+
# Frame processing control
65+
# (UI-related frame rate controls removed in headless mode)
7066

7167
# Face detection control
7268
self.last_detection_time = 0
@@ -463,8 +459,7 @@ def set_quality(self, high_quality):
463459

464460
print(f"Quality changed to: {actual_width}x{actual_height} @ {actual_fps:.1f} FPS")
465461

466-
# Update UI with blank frame of new size
467-
self.frame_ready.emit(np.zeros((self.frame_height, self.frame_width, 3), dtype=np.uint8))
462+
# No UI update needed in headless mode
468463

469464
def toggle_face_detection(self, enable):
470465
"""Enable or disable face detection"""
@@ -691,7 +686,8 @@ def _capture_loop(self):
691686
continue
692687

693688
if not success:
694-
self.error.emit(f"Failed to open camera: {error_msg}")
689+
if self.node:
690+
self.node.get_logger().error(f"Failed to open camera: {error_msg}")
695691
return
696692

697693
# Configure camera with optimal settings
@@ -736,7 +732,8 @@ def _capture_loop(self):
736732
self.frame_timestamp = time.time()
737733

738734
except Exception as e:
739-
self.error.emit(f"Error in capture thread: {str(e)}")
735+
if self.node:
736+
self.node.get_logger().error(f"Error in capture thread: {str(e)}")
740737
finally:
741738
if self.camera and self.camera.isOpened():
742739
self.camera.release()
@@ -825,13 +822,10 @@ def _process_loop(self):
825822
with self.frame_lock:
826823
self.processed_frame = frame
827824

828-
# Emit frame for UI with rate limiting
829-
current_time = time.time()
830-
if current_time - self.last_ui_update >= self.min_ui_interval:
831-
self.frame_ready.emit(frame)
832-
self.last_ui_update = current_time
825+
# No UI frame emission needed in headless mode
833826
except Exception as e:
834-
self.error.emit(f"Error in process thread: {str(e)}")
827+
if self.node:
828+
self.node.get_logger().error(f"Error in process thread: {str(e)}")
835829

836830
def _publish_loop(self):
837831
"""Handle ROS publishing at controlled rate"""
@@ -861,7 +855,8 @@ def _publish_loop(self):
861855

862856
self.last_publish_time = current_time
863857
except Exception as e:
864-
self.error.emit(f"Error in publish thread: {str(e)}")
858+
if self.node:
859+
self.node.get_logger().error(f"Error in publish thread: {str(e)}")
865860

866861

867862
class CameraNode(Node):

0 commit comments

Comments
 (0)