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.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

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

@@ -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.