From 9d10607b2e9f0aa2233ffa2b1ea98acf04506d2f Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 25 May 2025 21:21:28 -0600 Subject: [PATCH] Remove logger() outputs and assume always connected to prevent breaking --- src/core_pkg/core_pkg/core_ptz.py | 35 ++++++++++++++++++++------ src/core_pkg/core_pkg/siyi_sdk/siyi.py | 25 +++++++++++------- 2 files changed, 44 insertions(+), 16 deletions(-) diff --git a/src/core_pkg/core_pkg/core_ptz.py b/src/core_pkg/core_pkg/core_ptz.py index 9ce0148..47dd2c5 100644 --- a/src/core_pkg/core_pkg/core_ptz.py +++ b/src/core_pkg/core_pkg/core_ptz.py @@ -46,7 +46,7 @@ class PtzNode(Node): self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10) self.debug_pub = self.create_publisher(String, '/ptz/debug', 10) #create timer to publish feedback at a regular interval - self.create_timer(1.0, self.publish_feedback) + #self.create_timer(1.0, self.publish_feedback) # Create subscribers self.control_sub = self.create_subscription( @@ -54,6 +54,8 @@ class PtzNode(Node): # Create timers self.connection_timer = self.create_timer(5.0, self.check_camera_connection) + self.last_data_time = time.time() + self.health_check_timer = self.create_timer(2.0, self.check_camera_health) # Create feedback message self.feedback_msg = PtzFeedback() @@ -95,19 +97,21 @@ class PtzNode(Node): self.feedback_msg.connected = True self.feedback_msg.error_msg = "" - self.get_logger().info("Connected to PTZ camera") + #self.get_logger().info("Connected to PTZ camera") self.publish_debug("Camera connected successfully") except Exception as e: - self.camera_connected = False + #self.camera_connected = False self.feedback_msg.connected = False self.feedback_msg.error_msg = f"Connection error: {str(e)}" - self.get_logger().error(f"Failed to connect to camera: {e}") + #self.get_logger().error(f"Failed to connect to camera: {e}") self.publish_debug(f"Camera connection failed: {str(e)}") def camera_data_callback(self, cmd_id, data): """Handle data received from the camera.""" if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData): + self.last_data_time = time.time() # Update timestamp on successful data + # Update feedback message with attitude data self.feedback_msg.yaw = data.yaw self.feedback_msg.pitch = data.pitch @@ -126,15 +130,32 @@ class PtzNode(Node): def check_camera_connection(self): """Periodically check camera connection and attempt to reconnect if needed.""" if not self.camera_connected and not self.shutdown_requested: - self.get_logger().info("Attempting to reconnect to camera...") - self.connect_task = self.thread_pool.submit( # Changed from self.executor + #self.get_logger().info("Attempting to reconnect to camera...") + if self.camera: + # Fully clean up old connection first + try: + self.run_async_func(self.camera.disconnect()) + except Exception: + pass # Ignore errors during cleanup + self.camera = None + + self.connect_task = self.thread_pool.submit( self.run_async_func, self.connect_to_camera() ) + def check_camera_health(self): + """Check if we're still receiving data from the camera""" + if self.camera_connected: + time_since_last_data = time.time() - self.last_data_time + if time_since_last_data > 5.0: # No data for 5 seconds + #self.get_logger().warning(f"No camera data for {time_since_last_data:.1f}s, connection may be stale") + self.camera_connected = False + # Force reconnection on next check_camera_connection timer + def handle_control_command(self, msg): """Handle incoming control commands.""" if not self.camera_connected: - self.get_logger().warning("Camera not connected, ignoring control command") + #self.get_logger().warning("Camera not connected, ignoring control command") return # Submit the control command to the async executor diff --git a/src/core_pkg/core_pkg/siyi_sdk/siyi.py b/src/core_pkg/core_pkg/siyi_sdk/siyi.py index 2704d65..954da86 100644 --- a/src/core_pkg/core_pkg/siyi_sdk/siyi.py +++ b/src/core_pkg/core_pkg/siyi_sdk/siyi.py @@ -153,7 +153,7 @@ class SiyiGimbalCamera: self.ip, self.port ) self.is_connected = True - logger.info(f"Connected to {self.ip}:{self.port}") + #logger.info(f"Connected to {self.ip}:{self.port}") asyncio.create_task(self.heartbeat_loop()) # Start the data stream listener in the background. asyncio.create_task(self._data_stream_listener()) @@ -168,14 +168,15 @@ class SiyiGimbalCamera: self.is_connected = False self.writer.close() await self.writer.wait_closed() - logger.info("Disconnected") + #logger.info("Disconnected") else: - logger.warning("Not connected, cannot disconnect.") + #logger.warning("Not connected, cannot disconnect.") + pass async def heartbeat_loop(self) -> None: """Periodically send heartbeat packets to maintain the TCP connection.""" if not self.is_connected: - logger.warning("Heartbeat loop started before connection was established.") + #logger.warning("Heartbeat loop started before connection was established.") return try: @@ -183,13 +184,13 @@ class SiyiGimbalCamera: try: self.writer.write(HEARTBEAT_PACKET) await self.writer.drain() - logger.debug("Sent heartbeat packet") + #logger.debug("Sent heartbeat packet") await asyncio.sleep(self.heartbeat_interval) except (socket.error, BrokenPipeError) as e: - logger.error(f"Connection error in heartbeat loop: {e}") + #logger.error(f"Connection error in heartbeat loop: {e}") break finally: - logger.info("Heartbeat loop stopped.") + #logger.info("Heartbeat loop stopped.") if self.is_connected: await self.disconnect() @@ -289,7 +290,7 @@ class SiyiGimbalCamera: packet = self._build_data_stream_packet(data_type, data_freq) self.writer.write(packet) await self.writer.drain() - logger.info(f"Sent data stream request: data_type {data_type}, data_freq {data_freq}") + #logger.info(f"Sent data stream request: data_type {data_type}, data_freq {data_freq}") async def _read_packet(self): """Read a full packet from the TCP stream following the protocol format.""" @@ -329,8 +330,14 @@ class SiyiGimbalCamera: try: cmd_id_int, data = await self._read_packet() cmd_id = CommandID(cmd_id_int) + # Process data as before... + except (ConnectionResetError, BrokenPipeError, ConnectionError) as e: + logger.error(f"Connection error: {e}") + self.is_connected = False + break # Exit the loop to allow reconnection except Exception as e: - logger.error(f"Error reading packet: {e}") + #logger.error(f"Error reading packet: {e}") + ## Continue trying for other errors, but count failures continue # Process attitude data packets if applicable.