host.match_monitor.connector

  1"""Host-side TCP connector that polls for the roboRIO and maintains a control channel."""
  2
  3import json
  4import logging
  5import socket
  6import threading
  7import time
  8from typing import Optional
  9
 10logger = logging.getLogger("match_monitor")
 11
 12CONTROL_PORT = 5805
 13ROBOT_ADDRESSES = ['roboRIO-3200-FRC.local', '10.32.0.2', '172.22.11.2']
 14POLL_INTERVAL = 10  # seconds between connection attempts
 15KEEPALIVE_INTERVAL = 10  # seconds between PINGs
 16KEEPALIVE_TIMEOUT = 30  # seconds to wait for PONG
 17
 18
 19class RobotConnector:
 20    """Polls for the roboRIO, opens a TCP control channel, and handles keepalive.
 21
 22    When the robot sends UPLOAD_COMPLETE, triggers .wpilog analysis via the
 23    provided callback.
 24    """
 25
 26    def __init__(self, http_port: int, on_upload_complete=None) -> None:
 27        self._http_port = http_port
 28        self._on_upload_complete = on_upload_complete
 29        self._thread: Optional[threading.Thread] = None
 30        self._stop_event = threading.Event()
 31        self._paused = threading.Event()  # set = paused (don't poll)
 32        self._sock: Optional[socket.socket] = None
 33        self._sock_lock = threading.Lock()
 34        self._robot_addr: Optional[str] = None
 35        self._connect_attempts = 0
 36        self._connected_since: Optional[float] = None
 37        self._last_attempt_time: Optional[float] = None
 38        self._last_attempt_addr: Optional[str] = None
 39
 40    @property
 41    def is_connected(self) -> bool:
 42        return self._sock is not None
 43
 44    @property
 45    def robot_address(self) -> Optional[str]:
 46        return self._robot_addr
 47
 48    @property
 49    def status_info(self) -> dict:
 50        """Return a dict of connection status details."""
 51        if self._paused.is_set():
 52            state = 'paused'
 53        elif self._sock is not None:
 54            state = 'connected'
 55        else:
 56            state = 'polling'
 57        info = {
 58            'state': state,
 59            'robot_address': self._robot_addr,
 60            'http_port': self._http_port,
 61            'connect_attempts': self._connect_attempts,
 62            'poll_addresses': ROBOT_ADDRESSES,
 63            'last_attempt_addr': self._last_attempt_addr,
 64            'last_attempt_time': self._last_attempt_time,
 65            'connected_since': self._connected_since,
 66        }
 67        return info
 68
 69    def start(self) -> None:
 70        """Start the connector thread."""
 71        if self._thread is not None and self._thread.is_alive():
 72            return
 73        self._stop_event.clear()
 74        self._paused.clear()
 75        self._thread = threading.Thread(target=self._poll_loop, daemon=True)
 76        self._thread.start()
 77
 78    def stop(self) -> None:
 79        self._stop_event.set()
 80
 81    def connect(self) -> None:
 82        """Resume polling / reconnect immediately."""
 83        self._paused.clear()
 84        # If thread died or was never started, restart it
 85        if self._thread is None or not self._thread.is_alive():
 86            self.start()
 87        logger.info("Connection polling resumed")
 88
 89    def disconnect(self) -> None:
 90        """Close current connection and stop polling."""
 91        self._paused.set()
 92        with self._sock_lock:
 93            if self._sock is not None:
 94                try:
 95                    self._sock.close()
 96                except Exception:
 97                    logger.debug("Error closing socket during disconnect", exc_info=True)
 98                self._sock = None
 99                self._robot_addr = None
100        logger.info("Disconnected, polling paused")
101
102    def send_to_robot(self, msg: dict) -> bool:
103        """Send a JSON message to the connected robot. Returns False if not connected."""
104        with self._sock_lock:
105            if self._sock is None:
106                return False
107            try:
108                data = json.dumps(msg) + '\n'
109                self._sock.sendall(data.encode('utf-8'))
110                return True
111            except Exception:
112                logger.exception("Failed to send message to robot")
113                return False
114
115    def _poll_loop(self) -> None:
116        """Poll for the roboRIO and connect when found."""
117        logger.info(f"Polling for roboRIO on port {CONTROL_PORT} "
118                    f"(addresses: {', '.join(ROBOT_ADDRESSES)})")
119
120        while not self._stop_event.is_set():
121            # If paused, wait until unpaused or stopped
122            while self._paused.is_set() and not self._stop_event.is_set():
123                self._stop_event.wait(1)
124
125            if self._stop_event.is_set():
126                return
127
128            for addr in ROBOT_ADDRESSES:
129                if self._stop_event.is_set() or self._paused.is_set():
130                    break
131                try:
132                    self._connect_attempts += 1
133                    self._last_attempt_addr = addr
134                    self._last_attempt_time = time.monotonic()
135                    # Resolve address to support both IPv4 and IPv6 (mDNS may return IPv6)
136                    addrinfo = socket.getaddrinfo(addr, CONTROL_PORT, socket.AF_UNSPEC, socket.SOCK_STREAM)
137                    if not addrinfo:
138                        raise OSError(f"Could not resolve {addr}")
139                    af, socktype, proto, canonname, sockaddr = addrinfo[0]
140                    sock = socket.socket(af, socktype, proto)
141                    sock.settimeout(3)
142                    sock.connect(sockaddr)
143                    logger.info(f"Connected to robot at {addr}:{CONTROL_PORT}")
144                    self._handle_connection(sock, addr)
145                    logger.info(f"Disconnected from robot at {addr}, resuming polling")
146                except (socket.timeout, ConnectionRefusedError, OSError) as e:
147                    logger.debug(f"Cannot reach {addr}:{CONTROL_PORT} — {e}")
148                except Exception:
149                    logger.exception(f"Error connecting to {addr}")
150
151            # Wait before next poll cycle
152            self._stop_event.wait(POLL_INTERVAL)
153
154    def _handle_connection(self, sock: socket.socket, robot_addr: str) -> None:
155        """Run handshake and keepalive loop on an established connection."""
156        with self._sock_lock:
157            self._sock = sock
158            self._robot_addr = robot_addr
159        self._connected_since = time.monotonic()
160
161        try:
162            # Send HELLO
163            hello = json.dumps({
164                'type': 'HELLO',
165                'http_port': self._http_port,
166            }) + '\n'
167            sock.sendall(hello.encode('utf-8'))
168
169            # Wait for HELLO_ACK
170            sock.settimeout(10)
171            buf = ''
172            buf = self._read_message(sock, buf)
173            if buf is None:
174                return
175            msg, buf = self._parse_next_message(buf)
176            if msg is None or msg.get('type') != 'HELLO_ACK':
177                logger.warning(f"Expected HELLO_ACK, got: {msg}")
178                return
179
180            logger.info(f"Handshake complete with {robot_addr}")
181
182            # Keepalive + message loop
183            last_ping = time.monotonic()
184            sock.settimeout(1.0)  # short timeout for non-blocking reads
185
186            while not self._stop_event.is_set() and not self._paused.is_set():
187                now = time.monotonic()
188
189                # Send PING if interval elapsed
190                if now - last_ping >= KEEPALIVE_INTERVAL:
191                    ping = json.dumps({'type': 'PING'}) + '\n'
192                    sock.sendall(ping.encode('utf-8'))
193                    last_ping = now
194
195                # Try to read messages (non-blocking-ish with 1s timeout)
196                try:
197                    buf = self._read_message(sock, buf)
198                    if buf is None:
199                        # Connection closed
200                        return
201                    while '\n' in buf:
202                        msg, buf = self._parse_next_message(buf)
203                        if msg is None:
204                            continue
205                        self._handle_robot_message(msg, robot_addr)
206                except socket.timeout:
207                    # No data available, check if we've exceeded keepalive timeout
208                    continue
209
210        except (ConnectionResetError, BrokenPipeError, OSError):
211            logger.info(f"Connection to {robot_addr} lost")
212        except Exception:
213            logger.exception(f"Error in connection to {robot_addr}")
214        finally:
215            self._connected_since = None
216            with self._sock_lock:
217                self._sock = None
218                self._robot_addr = None
219            try:
220                sock.close()
221            except Exception:
222                logger.debug("Error closing socket after connection", exc_info=True)
223
224    def _handle_robot_message(self, msg: dict, robot_addr: str) -> None:
225        """Process a message received from the robot."""
226        msg_type = msg.get('type', '')
227
228        if msg_type == 'PONG':
229            pass  # keepalive response, all good
230
231        elif msg_type == 'FILE_SKIPPED':
232            filename = msg.get('filename', '?')
233            reason = msg.get('reason', 'unknown')
234            logger.info(f"Robot skipped {filename}: {reason}")
235
236        elif msg_type == 'UPLOAD_STARTING':
237            event = msg.get('event_name', '')
238            mtype = msg.get('match_type', '')
239            mnum = msg.get('match_number', '')
240            logger.info(f"Robot starting upload: {event} {mtype}_{mnum}")
241            print(f"  Robot starting upload: {event} {mtype}_{mnum}")
242
243        elif msg_type == 'UPLOAD_COMPLETE':
244            event = msg.get('event_name', '')
245            mtype = msg.get('match_type', '')
246            mnum = msg.get('match_number', '')
247            logger.info(f"Robot upload complete: {event} {mtype}_{mnum}")
248            print(f"  Robot upload complete: {event} {mtype}_{mnum}")
249
250            if self._on_upload_complete:
251                metadata = {
252                    'event_name': event,
253                    'match_type': mtype,
254                    'match_number': mnum,
255                }
256                try:
257                    self._on_upload_complete(metadata)
258                except Exception:
259                    logger.exception("on_upload_complete callback failed")
260
261        elif msg_type == 'MANIFEST_CLEARED':
262            count = msg.get('count', 0)
263            print(f"  Cleared {count} manifest(s) on robot")
264
265        elif msg_type == 'FORCE_UPLOAD_ACK':
266            print("  Robot upload started")
267
268        elif msg_type == 'STOP_UPLOAD_ACK':
269            print("  Robot upload stopped")
270
271        elif msg_type == 'LIST_LOGS_RESPONSE':
272            files = msg.get('files', [])
273            if not files:
274                print("  No log files on robot")
275            else:
276                print(f"  {'File':<45} {'Size':>10}  Uploaded")
277                print(f"  {'-' * 45} {'-' * 10}  {'-' * 8}")
278                for f in files:
279                    name = f.get('name', '?')
280                    size = f.get('size', 0)
281                    uploaded = 'yes' if f.get('uploaded') else 'no'
282                    if size >= 1024 * 1024:
283                        size_str = f"{size / (1024 * 1024):.1f} MB"
284                    elif size >= 1024:
285                        size_str = f"{size / 1024:.1f} KB"
286                    else:
287                        size_str = f"{size} B"
288                    print(f"  {name:<45} {size_str:>10}  {uploaded}")
289                print(f"  {len(files)} file(s)")
290
291        else:
292            logger.debug(f"Unknown message from robot: {msg}")
293
294    @staticmethod
295    def _read_message(sock: socket.socket, buf: str) -> Optional[str]:
296        """Read data until we have at least one newline. Returns None on disconnect."""
297        while '\n' not in buf:
298            data = sock.recv(4096)
299            if not data:
300                return None
301            buf += data.decode('utf-8')
302        return buf
303
304    @staticmethod
305    def _parse_next_message(buf: str) -> tuple:
306        """Extract the next JSON message from the buffer."""
307        idx = buf.index('\n')
308        line = buf[:idx].strip()
309        remaining = buf[idx + 1:]
310        if not line:
311            return None, remaining
312        try:
313            return json.loads(line), remaining
314        except json.JSONDecodeError:
315            logger.warning(f"Invalid JSON from robot: {line!r}")
316            return None, remaining