aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Remove logger() outputs and assume always connected to prevent breaking

Browse Source
This commit is contained in:
Tristan McGinnis
2025-05-25 21:21:28 -06:00
parent 7d9ca314a7
commit 9d10607b2e
2 changed files with 44 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
src/core_pkg/core_pkg/core_ptz.py
View File

@@ -46,7 +46,7 @@ class PtzNode(Node):
self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10) self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10)
self.debug_pub = self.create_publisher(String, '/ptz/debug', 10) self.debug_pub = self.create_publisher(String, '/ptz/debug', 10)
#create timer to publish feedback at a regular interval #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 # Create subscribers
self.control_sub = self.create_subscription( self.control_sub = self.create_subscription(
@@ -54,6 +54,8 @@ class PtzNode(Node):
# Create timers # Create timers
self.connection_timer = self.create_timer(5.0, self.check_camera_connection) 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 # Create feedback message
self.feedback_msg = PtzFeedback() self.feedback_msg = PtzFeedback()
@@ -95,19 +97,21 @@ class PtzNode(Node):
self.feedback_msg.connected = True self.feedback_msg.connected = True
self.feedback_msg.error_msg = "" 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") self.publish_debug("Camera connected successfully")
except Exception as e: except Exception as e:
self.camera_connected = False #self.camera_connected = False
self.feedback_msg.connected = False self.feedback_msg.connected = False
self.feedback_msg.error_msg = f"Connection error: {str(e)}" 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)}") self.publish_debug(f"Camera connection failed: {str(e)}")
def camera_data_callback(self, cmd_id, data): def camera_data_callback(self, cmd_id, data):
"""Handle data received from the camera.""" """Handle data received from the camera."""
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData): 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 # Update feedback message with attitude data
self.feedback_msg.yaw = data.yaw self.feedback_msg.yaw = data.yaw
self.feedback_msg.pitch = data.pitch self.feedback_msg.pitch = data.pitch
@@ -126,15 +130,32 @@ class PtzNode(Node):
def check_camera_connection(self): def check_camera_connection(self):
"""Periodically check camera connection and attempt to reconnect if needed.""" """Periodically check camera connection and attempt to reconnect if needed."""
if not self.camera_connected and not self.shutdown_requested: if not self.camera_connected and not self.shutdown_requested:
self.get_logger().info("Attempting to reconnect to camera...") #self.get_logger().info("Attempting to reconnect to camera...")
self.connect_task = self.thread_pool.submit( # Changed from self.executor 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() 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): def handle_control_command(self, msg):
"""Handle incoming control commands.""" """Handle incoming control commands."""
if not self.camera_connected: 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 return
# Submit the control command to the async executor # Submit the control command to the async executor

25
src/core_pkg/core_pkg/siyi_sdk/siyi.py
View File

@@ -153,7 +153,7 @@ class SiyiGimbalCamera:
self.ip, self.port self.ip, self.port
) )
self.is_connected = True 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()) asyncio.create_task(self.heartbeat_loop())
# Start the data stream listener in the background. # Start the data stream listener in the background.
asyncio.create_task(self._data_stream_listener()) asyncio.create_task(self._data_stream_listener())
@@ -168,14 +168,15 @@ class SiyiGimbalCamera:
self.is_connected = False self.is_connected = False
self.writer.close() self.writer.close()
await self.writer.wait_closed() await self.writer.wait_closed()
logger.info("Disconnected") #logger.info("Disconnected")
else: else:
logger.warning("Not connected, cannot disconnect.") #logger.warning("Not connected, cannot disconnect.")
pass
async def heartbeat_loop(self) -> None: async def heartbeat_loop(self) -> None:
"""Periodically send heartbeat packets to maintain the TCP connection.""" """Periodically send heartbeat packets to maintain the TCP connection."""
if not self.is_connected: if not self.is_connected:
logger.warning("Heartbeat loop started before connection was established.") #logger.warning("Heartbeat loop started before connection was established.")
return return
try: try:
@@ -183,13 +184,13 @@ class SiyiGimbalCamera:
try: try:
self.writer.write(HEARTBEAT_PACKET) self.writer.write(HEARTBEAT_PACKET)
await self.writer.drain() await self.writer.drain()
logger.debug("Sent heartbeat packet") #logger.debug("Sent heartbeat packet")
await asyncio.sleep(self.heartbeat_interval) await asyncio.sleep(self.heartbeat_interval)
except (socket.error, BrokenPipeError) as e: 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 break
finally: finally:
logger.info("Heartbeat loop stopped.") #logger.info("Heartbeat loop stopped.")
if self.is_connected: if self.is_connected:
await self.disconnect() await self.disconnect()
@@ -289,7 +290,7 @@ class SiyiGimbalCamera:
packet = self._build_data_stream_packet(data_type, data_freq) packet = self._build_data_stream_packet(data_type, data_freq)
self.writer.write(packet) self.writer.write(packet)
await self.writer.drain() 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): async def _read_packet(self):
"""Read a full packet from the TCP stream following the protocol format.""" """Read a full packet from the TCP stream following the protocol format."""
@@ -329,8 +330,14 @@ class SiyiGimbalCamera:
try: try:
cmd_id_int, data = await self._read_packet() cmd_id_int, data = await self._read_packet()
cmd_id = CommandID(cmd_id_int) 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: 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 continue
# Process attitude data packets if applicable. # Process attitude data packets if applicable.