Update camera_ui_node to first check state before attempting to modify camera_node

Author: kPatch

coffee_ws/src/coffee_vision_ui/coffee_vision_ui/camera_ui_node.py

@@ -28,6 +28,7 @@
 from rclpy.node import Node
 from python_qt_binding.QtWidgets import QApplication, QMainWindow, QHBoxLayout, QVBoxLayout, QWidget
 from python_qt_binding.QtCore import QTimer
+from std_msgs.msg import String
 
 # Import our modular components
 from .ros_interface import CameraController, FrameReceiver
@@ -72,12 +73,15 @@ def __init__(self, node):
 
         # Initialize ROS communication components
         self.camera_controller = CameraController(node)
-        self.frame_receiver = FrameReceiver(node, 'camera_frame')
+        self.frame_receiver = FrameReceiver(node, '/coffee_bot/camera/image_raw')
 
         # Initialize UI components
         self.camera_display = CameraDisplay()
         self.control_panel = ControlPanel()
 
+        # Set up state query publisher
+        self.state_query_pub = node.create_publisher(String, '/coffee_bot/camera/query/state', 10)
+        
         # Set up connection timeout monitoring
         self.connection_timer = QTimer()
         self.connection_timer.timeout.connect(self._check_connection)
@@ -86,8 +90,8 @@ def __init__(self, node):
         self.init_ui()
         self.setup_connections()
 
-        # Request initial camera scan after a short delay
-        QTimer.singleShot(1000, self.control_panel.refresh_cameras_requested.emit)
+        # Query camera_node state instead of sending commands
+        QTimer.singleShot(1000, self._query_camera_node_state)
 
     def init_ui(self):
         """Initialize the user interface layout and components."""
@@ -120,6 +124,7 @@ def setup_connections(self):
         self.camera_controller.cameras_updated.connect(self.control_panel.update_camera_list)
         self.camera_controller.camera_status_updated.connect(self.control_panel.update_status)
         self.camera_controller.diagnostics_ready.connect(self.control_panel.show_diagnostics)
+        self.camera_controller.state_received.connect(self.control_panel.sync_to_camera_state)
 
         # Control panel to camera controller connections
         self.control_panel.camera_selected.connect(self.camera_controller.select_camera)
@@ -130,6 +135,24 @@ def setup_connections(self):
 
         self.node.get_logger().info('Signal connections established between all components')
 
+    def _query_camera_node_state(self):
+        """Query camera_node for its current state instead of sending commands"""
+        self.node.get_logger().info('Querying camera_node for current state')
+        
+        # Send state query
+        query_msg = String()
+        query_msg.data = "get_state"
+        self.state_query_pub.publish(query_msg)
+        
+        # Set up one-time listener for state response
+        QTimer.singleShot(2000, self._setup_state_listener)
+    
+    def _setup_state_listener(self):
+        """Set up listener for state response from camera_node"""
+        # The state response comes through the same topic as camera list updates
+        # We'll handle it in the existing camera_controller callback
+        self.node.get_logger().info('Listening for camera_node state response')
+    
     def _check_connection(self):
         """Periodically check frame receiver connection status."""
         self.frame_receiver.check_connection_timeout()

coffee_ws/src/coffee_vision_ui/coffee_vision_ui/ros_interface/camera_controller.py

@@ -37,6 +37,7 @@ class CameraController(QObject):
     cameras_updated = pyqtSignal(list)      # Available cameras: [(index, name), ...]
     camera_status_updated = pyqtSignal(str) # Current camera status
     diagnostics_ready = pyqtSignal(str)     # Diagnostic information
+    state_received = pyqtSignal(dict)       # Full camera state for synchronization
 
     def __init__(self, node):
         """
@@ -156,19 +157,27 @@ def _camera_status_callback(self, msg):
 
     def _available_cameras_callback(self, msg):
         """
-        Handle available cameras list from camera_node.
+        Handle available cameras list or state response from camera_node.
         
         Args:
-            msg: String message containing JSON list of cameras
+            msg: String message containing JSON list of cameras or full state
         """
         try:
-            cameras_data = json.loads(msg.data)
-            # Expected format: [{"index": 0, "name": "Camera 0"}, ...]
-            cameras_list = [(cam["index"], cam["name"]) for cam in cameras_data]
-            self.cameras_updated.emit(cameras_list)
+            data = json.loads(msg.data)
+            
+            # Check if this is a full state response (has camera_index, high_quality, etc.)
+            if isinstance(data, dict) and 'camera_index' in data:
+                # This is a state response - emit for state sync
+                self.node.get_logger().info("Received camera_node state for synchronization")
+                self.state_received.emit(data)
+            else:
+                # This is a regular camera list
+                # Expected format: [{"index": 0, "name": "Camera 0"}, ...]
+                cameras_list = [(cam["index"], cam["name"]) for cam in data]
+                self.cameras_updated.emit(cameras_list)
         except json.JSONDecodeError as e:
             self.node.get_logger().error(f"Error parsing cameras data: {e}")
-        except KeyError as e:
+        except (KeyError, TypeError) as e:
             self.node.get_logger().error(f"Invalid camera data format: {e}")
 
     def _diagnostics_callback(self, msg):

coffee_ws/src/coffee_vision_ui/coffee_vision_ui/widgets/control_panel.py

@@ -296,5 +296,46 @@ def set_settings(self, settings):
                         self.camera_combo.setCurrentIndex(i)
                         self.current_camera_index = camera_index
                         break
+        finally:
+            self._updating_controls = False
+    
+    def sync_to_camera_state(self, state_data):
+        """
+        Sync control panel to camera_node state without triggering commands.
+        
+        Args:
+            state_data: Dictionary with current camera state
+        """
+        self._updating_controls = True
+        try:
+            # Sync camera selection
+            if 'camera_index' in state_data and 'available_cameras' in state_data:
+                self.available_cameras = [
+                    (cam["index"], cam["name"]) 
+                    for cam in state_data['available_cameras']
+                ]
+                self.camera_combo.clear()
+                
+                current_camera_index = state_data['camera_index']
+                for i, (idx, name) in enumerate(self.available_cameras):
+                    self.camera_combo.addItem(name, idx)
+                    if idx == current_camera_index:
+                        self.camera_combo.setCurrentIndex(i)
+                        self.current_camera_index = current_camera_index
+            
+            # Sync quality setting
+            if 'high_quality' in state_data:
+                self.high_quality = state_data['high_quality']
+                self.quality_checkbox.setChecked(self.high_quality)
+            
+            # Sync face detection setting
+            if 'face_detection_enabled' in state_data:
+                self.face_detection_enabled = state_data['face_detection_enabled']
+                self.face_detection_checkbox.setChecked(self.face_detection_enabled)
+            
+            # Update status
+            camera_count = len(self.available_cameras)
+            self.update_status(f"Synced with camera_node - {camera_count} camera(s) available")
+            
         finally:
             self._updating_controls = False