franka/network/
tcp.rs

1//! TCP command channel: framing plus command-id demultiplexing.
2//!
3//! Port of `Network::tcpSendRequest`, `Network::tcpReadFromBuffer`,
4//! `Network::tcpReceiveResponse`, `Network::tcpBlockingReceiveResponse`,
5//! `Network::isTcpSocketAlive` and `Network::tcpThrowIfConnectionClosed` from libfranka 0.21.2
6//! (`src/network.h`, `src/network.cpp`).
7//!
8//! The FCI server answers commands out of order — a `Move` reply can arrive long after a
9//! `StopMove` sent later — so every reply is stored under the `command_id` of its request until
10//! the thread that issued that request picks it up. This is exactly libfranka's
11//! `received_responses_` map.
12
13use std::collections::HashMap;
14use std::io::{ErrorKind, Write};
15use std::net::{SocketAddr, TcpStream};
16use std::os::fd::{AsRawFd, RawFd};
17use std::sync::atomic::{AtomicU32, Ordering};
18use std::sync::Mutex;
19use std::time::Duration;
20
21use crate::error::{FrankaError, FrankaResult};
22use crate::wire::HeaderLayout;
23
24/// Poll/read granularity of [`TcpSession::blocking_receive_response`]
25/// (libfranka's `franka::kTimeout`).
26pub const POLL_TIMEOUT: Duration = Duration::from_millis(10);
27
28/// Default TCP connect/read timeout (libfranka's `Network` default `tcp_timeout`).
29pub const DEFAULT_CONNECT_TIMEOUT: Duration = Duration::from_secs(1);
30
31/// Upper bound on a single TCP message's announced `size`, so that a malformed or hostile peer
32/// cannot make [`TcpSession::read_from_buffer`] allocate an unbounded amount of memory purely
33/// from an untrusted header field.
34///
35/// libfranka has no such bound (it resizes `pending_response_` to `header.size` unconditionally),
36/// but one costs nothing here: the largest legitimate message is the v5 model library at roughly
37/// 330 KB, and a URDF is tens of KB, so 16 MiB leaves generous headroom.
38const MAX_TCP_MESSAGE_SIZE: usize = 16 * 1024 * 1024;
39
40/// Partially received message plus the completed messages waiting to be claimed.
41///
42/// Mirrors `pending_response_`, `pending_response_offset_`, `pending_command_id_` and
43/// `received_responses_` in `franka::Network`.
44#[derive(Debug, Default)]
45struct Demux {
46    /// Complete messages (header **and** payload), keyed by `command_id`.
47    received: HashMap<u32, Vec<u8>>,
48    /// Bytes of the message currently being assembled.
49    pending: Vec<u8>,
50    /// Total length of the pending message once its header has been parsed.
51    pending_len: Option<usize>,
52    /// `command_id` of the pending message.
53    pending_id: u32,
54}
55
56/// A connected FCI command socket.
57///
58/// `TcpSession` is `Sync`: one thread may block in
59/// [`TcpSession::blocking_receive_response`] while another calls
60/// [`TcpSession::send_request`], which is how `Robot::stop()` interrupts a running motion in
61/// libfranka.
62#[derive(Debug)]
63pub struct TcpSession {
64    socket: TcpStream,
65    layout: HeaderLayout,
66    state: Mutex<Demux>,
67    next_command_id: AtomicU32,
68}
69
70impl TcpSession {
71    /// Connects to `addr` and configures the socket the way `franka::Network`'s constructor
72    /// does: blocking, 1 s connect timeout, TCP keepalive with a 1 s idle time, 3 probes and a
73    /// 1 s interval (best effort — libfranka swallows failures of the three `setOption` calls).
74    ///
75    /// The receive timeout is set to [`POLL_TIMEOUT`] so that a blocking receive wakes every
76    /// 10 ms exactly like libfranka's `poll(kTimeout, SELECT_READ)`.
77    ///
78    /// Error texts are libfranka's, verbatim.
79    pub fn connect(
80        addr: SocketAddr,
81        timeout: Duration,
82        layout: HeaderLayout,
83    ) -> FrankaResult<TcpSession> {
84        let socket = TcpStream::connect_timeout(&addr, timeout).map_err(|e| match e.kind() {
85            ErrorKind::ConnectionRefused => FrankaError::Network(
86                "libfranka: Connection to FCI refused. Please install FCI feature or enable FCI mode in Desk."
87                    .to_string(),
88            ),
89            ErrorKind::TimedOut => FrankaError::Network(
90                "libfranka: Connection timeout. Please check your network connection or settings."
91                    .to_string(),
92            ),
93            _ => FrankaError::Network(format!("libfranka: Connection error: {e}")),
94        })?;
95
96        socket
97            .set_nodelay(true)
98            .map_err(|e| FrankaError::Network(format!("libfranka: Connection error: {e}")))?;
99        socket
100            .set_read_timeout(Some(POLL_TIMEOUT))
101            .map_err(|e| FrankaError::Network(format!("libfranka: Connection error: {e}")))?;
102        socket
103            .set_write_timeout(Some(timeout))
104            .map_err(|e| FrankaError::Network(format!("libfranka: Connection error: {e}")))?;
105
106        // Best effort, like libfranka's `try { setOption(...) } catch (...) {}`.
107        let keepalive = socket2::TcpKeepalive::new()
108            .with_time(Duration::from_secs(1))
109            .with_interval(Duration::from_secs(1))
110            .with_retries(3);
111        let _ = socket2::SockRef::from(&socket).set_tcp_keepalive(&keepalive);
112
113        Ok(TcpSession {
114            socket,
115            layout,
116            state: Mutex::new(Demux::default()),
117            next_command_id: AtomicU32::new(0),
118        })
119    }
120
121    /// The header layout this session frames with.
122    pub fn layout(&self) -> HeaderLayout {
123        self.layout
124    }
125
126    /// Local address of the command socket (used by tests).
127    pub fn local_addr(&self) -> FrankaResult<SocketAddr> {
128        self.socket
129            .local_addr()
130            .map_err(|e| FrankaError::Network(format!("libfranka: {e}")))
131    }
132
133    /// Sends a request and returns its `command_id`.
134    ///
135    /// The header's `size` is `header_len + payload.len()`, i.e. it counts the header, exactly
136    /// like `CommandHeader::size` in libfranka. Command ids start at 0 and increase by one per
137    /// request (`Network::command_id_`).
138    pub fn send_request(&self, command: u32, payload: &[u8]) -> FrankaResult<u32> {
139        let command_id = self.next_command_id.fetch_add(1, Ordering::Relaxed);
140        let mut message = self
141            .layout
142            .encode_header(command, command_id, payload.len());
143        message.extend_from_slice(payload);
144
145        let _guard = self.state.lock().unwrap_or_else(|e| e.into_inner());
146        (&self.socket)
147            .write_all(&message)
148            .map_err(|e| FrankaError::Network(format!("libfranka: TCP send bytes: {e}")))?;
149        Ok(command_id)
150    }
151
152    /// Blocks until the response with `command_id` has been received and returns the whole
153    /// message, header included.
154    ///
155    /// Port of `Network::tcpBlockingReceiveResponse`: lock, read for at most 10 ms, look the id
156    /// up, unlock, `std::this_thread::yield()`, repeat.
157    pub fn blocking_receive_response(&self, command_id: u32) -> FrankaResult<Vec<u8>> {
158        loop {
159            {
160                let mut state = self.state.lock().unwrap_or_else(|e| e.into_inner());
161                self.read_from_buffer(&mut state, false)?;
162                if let Some(message) = state.received.remove(&command_id) {
163                    return Ok(message);
164                }
165            }
166            std::thread::yield_now();
167        }
168    }
169
170    /// Tries to claim the response with `command_id` without blocking.
171    ///
172    /// Port of `Network::tcpReceiveResponse`: a `try_lock` that gives up immediately if another
173    /// thread holds the socket, then a zero-timeout read. Returns `Ok(None)` when the response
174    /// has not arrived yet.
175    pub fn try_receive_response(&self, command_id: u32) -> FrankaResult<Option<Vec<u8>>> {
176        let Ok(mut state) = self.state.try_lock() else {
177            return Ok(None);
178        };
179        self.read_from_buffer(&mut state, true)?;
180        Ok(state.received.remove(&command_id))
181    }
182
183    /// Whether the socket is free of pending errors (`Network::isTcpSocketAlive`).
184    pub fn is_alive(&self) -> bool {
185        !poll_error(self.socket.as_raw_fd())
186    }
187
188    /// Returns `Err(Network("libfranka: server closed connection"))` when the peer performed an
189    /// orderly shutdown (`Network::tcpThrowIfConnectionClosed`).
190    ///
191    /// Like libfranka this is a no-op when another thread holds the socket lock, and it peeks
192    /// rather than consuming, so a queued response is not lost.
193    pub fn throw_if_connection_closed(&self) -> FrankaResult<()> {
194        let Ok(_state) = self.state.try_lock() else {
195            return Ok(());
196        };
197        if !poll_readable(self.socket.as_raw_fd()) {
198            return Ok(());
199        }
200        let mut buffer = [0u8; 1];
201        let fd = self.socket.as_raw_fd();
202        let received = unsafe {
203            libc::recv(
204                fd,
205                buffer.as_mut_ptr().cast(),
206                buffer.len(),
207                libc::MSG_PEEK | libc::MSG_DONTWAIT,
208            )
209        };
210        let errno = if received < 0 {
211            std::io::Error::last_os_error().raw_os_error().unwrap_or(0)
212        } else {
213            0
214        };
215        classify_peek(received, errno)
216    }
217
218    /// Shuts the command socket down in both directions, best effort.
219    ///
220    /// libfranka calls `tcp_socket_.shutdown()` whenever a UDP receive fails
221    /// (`network.h:146`) so that the next TCP operation fails fast instead of waiting on a
222    /// connection the robot has already given up on; [`crate::network::Network`] does the same.
223    pub fn shutdown(&self) {
224        let _ = self.socket.shutdown(std::net::Shutdown::Both);
225    }
226
227    /// One pass of `Network::tcpReadFromBuffer`.
228    ///
229    /// Reads at most the bytes still missing from the current message, so a message is never
230    /// read into the next one. `non_blocking` selects between the socket's 10 ms receive
231    /// timeout and `MSG_DONTWAIT` (libfranka's `kTimeout` vs `0us` poll).
232    fn read_from_buffer(&self, state: &mut Demux, non_blocking: bool) -> FrankaResult<()> {
233        let fd = self.socket.as_raw_fd();
234        if poll_error(fd) {
235            return Err(FrankaError::Network(
236                "libfranka: TCP connection got interrupted.".to_string(),
237            ));
238        }
239
240        let header_len = self.layout.header_len();
241        let target = state.pending_len.unwrap_or(header_len);
242        let missing = target - state.pending.len();
243        debug_assert!(missing > 0);
244
245        let start = state.pending.len();
246        state.pending.resize(target, 0);
247        let read = recv(fd, &mut state.pending[start..], non_blocking);
248        let read = match read {
249            Ok(n) => n,
250            Err(e) if e.kind() == ErrorKind::WouldBlock || e.kind() == ErrorKind::TimedOut => {
251                state.pending.truncate(start);
252                return Ok(());
253            }
254            Err(e) => {
255                state.pending.truncate(start);
256                return Err(FrankaError::Network(format!("libfranka: TCP receive: {e}")));
257            }
258        };
259        state.pending.truncate(start + read);
260
261        if read == 0 {
262            // Poco reports the orderly shutdown as a zero-byte read; libfranka detects it in
263            // `tcpThrowIfConnectionClosed`. Reporting it here as well keeps a blocking receive
264            // from spinning forever on a closed socket.
265            return Err(FrankaError::Network(
266                "libfranka: server closed connection".to_string(),
267            ));
268        }
269
270        if state.pending_len.is_none() && state.pending.len() == header_len {
271            let (_command, command_id, size) = self
272                .layout
273                .decode_header(&state.pending)
274                .expect("header_len bytes are available");
275            if (size as usize) < header_len || (size as usize) > MAX_TCP_MESSAGE_SIZE {
276                state.pending.clear();
277                return Err(FrankaError::Protocol(
278                    "libfranka: Incorrect TCP message size.".to_string(),
279                ));
280            }
281            state.pending_len = Some(size as usize);
282            state.pending_id = command_id;
283            state.pending.reserve(size as usize - header_len);
284        }
285
286        if Some(state.pending.len()) == state.pending_len {
287            let message = std::mem::take(&mut state.pending);
288            state.received.insert(state.pending_id, message);
289            state.pending_len = None;
290            state.pending_id = 0;
291        }
292        Ok(())
293    }
294}
295
296impl Drop for TcpSession {
297    /// `franka::Network::~Network` shuts the socket down; dropping the `TcpStream` closes it.
298    fn drop(&mut self) {
299        let _ = self.socket.shutdown(std::net::Shutdown::Both);
300    }
301}
302
303/// `recv(2)` with `MSG_DONTWAIT` when `non_blocking`, retrying on `EINTR`.
304fn recv(fd: RawFd, buf: &mut [u8], non_blocking: bool) -> std::io::Result<usize> {
305    let flags = if non_blocking { libc::MSG_DONTWAIT } else { 0 };
306    loop {
307        let n = unsafe { libc::recv(fd, buf.as_mut_ptr().cast(), buf.len(), flags) };
308        if n < 0 {
309            let e = std::io::Error::last_os_error();
310            if e.kind() == ErrorKind::Interrupted {
311                continue;
312            }
313            return Err(e);
314        }
315        return Ok(n as usize);
316    }
317}
318
319/// `poll(fd, 0)` checking for `POLLERR`/`POLLHUP`/`POLLNVAL`
320/// (Poco's `Socket::SELECT_ERROR`).
321fn poll_error(fd: RawFd) -> bool {
322    poll_once(fd, 0) & (libc::POLLERR | libc::POLLNVAL) != 0
323}
324
325/// Turns the result of the `MSG_PEEK | MSG_DONTWAIT` probe into libfranka's verdict on the
326/// connection.
327///
328/// `0` is an orderly shutdown by the server. `-1` is `EAGAIN`/`EWOULDBLOCK` on a healthy but
329/// empty socket, or `EINTR` if a signal arrived -- neither says anything about the connection
330/// -- but any other errno is a real socket error, which libfranka's Asio `available()` reports
331/// by throwing rather than by returning "still connected".
332fn classify_peek(received: isize, errno: i32) -> FrankaResult<()> {
333    if received == 0 {
334        return Err(FrankaError::Network(
335            "libfranka: server closed connection".to_string(),
336        ));
337    }
338    if received < 0 {
339        let error = std::io::Error::from_raw_os_error(errno);
340        if !matches!(
341            error.kind(),
342            std::io::ErrorKind::WouldBlock | std::io::ErrorKind::Interrupted
343        ) {
344            return Err(FrankaError::Network(format!(
345                "libfranka: Connection error: {error}"
346            )));
347        }
348    }
349    Ok(())
350}
351
352/// `poll(fd, 0)` checking for `POLLIN` (Poco's `Socket::SELECT_READ`).
353fn poll_readable(fd: RawFd) -> bool {
354    poll_once(fd, libc::POLLIN) & libc::POLLIN != 0
355}
356
357fn poll_once(fd: RawFd, events: libc::c_short) -> libc::c_short {
358    let mut pollfd = libc::pollfd {
359        fd,
360        events,
361        revents: 0,
362    };
363    let rc = unsafe { libc::poll(&mut pollfd, 1, 0) };
364    if rc <= 0 {
365        return 0;
366    }
367    pollfd.revents
368}
369
370#[cfg(test)]
371mod tests {
372    use super::*;
373    use std::io::Read;
374    use std::net::TcpListener;
375
376    /// `recv` returning `-1` is only "still connected" for `EAGAIN`/`EWOULDBLOCK`/`EINTR`;
377    /// any other errno is a [`FrankaError::Network`], not a silently healthy connection.
378    #[test]
379    fn a_failed_peek_is_a_network_error_unless_it_would_block() {
380        assert!(classify_peek(1, 0).is_ok());
381        assert!(classify_peek(-1, libc::EAGAIN).is_ok());
382        assert!(classify_peek(-1, libc::EWOULDBLOCK).is_ok());
383        assert!(classify_peek(-1, libc::EINTR).is_ok());
384
385        match classify_peek(0, 0) {
386            Err(FrankaError::Network(message)) => {
387                assert_eq!(message, "libfranka: server closed connection")
388            }
389            other => panic!("expected a closed connection, got {other:?}"),
390        }
391        for errno in [libc::ECONNRESET, libc::ENOTCONN, libc::EPIPE, libc::EBADF] {
392            match classify_peek(-1, errno) {
393                Err(FrankaError::Network(message)) => {
394                    assert!(
395                        message.starts_with("libfranka: Connection error: "),
396                        "unexpected message {message}"
397                    )
398                }
399                other => panic!("errno {errno}: expected a Network error, got {other:?}"),
400            }
401        }
402    }
403
404    /// Builds a complete message: robot header (`command`, `command_id`, `size`) + payload.
405    fn message(command: u32, command_id: u32, payload: &[u8]) -> Vec<u8> {
406        let mut out = HeaderLayout::Robot.encode_header(command, command_id, payload.len());
407        out.extend_from_slice(payload);
408        out
409    }
410
411    /// Accepts one connection, hands the stream to `serve`, and keeps the listener alive.
412    fn mock_server<F>(serve: F) -> (SocketAddr, std::thread::JoinHandle<()>)
413    where
414        F: FnOnce(TcpStream) + Send + 'static,
415    {
416        let listener = TcpListener::bind("127.0.0.1:0").unwrap();
417        let addr = listener.local_addr().unwrap();
418        let handle = std::thread::spawn(move || {
419            let (stream, _) = listener.accept().unwrap();
420            serve(stream);
421        });
422        (addr, handle)
423    }
424
425    fn client(addr: SocketAddr) -> TcpSession {
426        TcpSession::connect(addr, Duration::from_secs(1), HeaderLayout::Robot).unwrap()
427    }
428
429    #[test]
430    fn responses_are_demultiplexed_and_reassembled() {
431        let (addr, server) = mock_server(|mut stream| {
432            // Drain the two requests so the client's writes cannot block.
433            let mut scratch = [0u8; 64];
434            let _ = stream.read(&mut scratch);
435            // Answer the *second* request first, split across three writes.
436            let second = message(2, 1, b"second");
437            stream.write_all(&second[..5]).unwrap();
438            stream.flush().unwrap();
439            std::thread::sleep(Duration::from_millis(30));
440            stream.write_all(&second[5..14]).unwrap();
441            stream.flush().unwrap();
442            std::thread::sleep(Duration::from_millis(30));
443            stream.write_all(&second[14..]).unwrap();
444            stream.write_all(&message(1, 0, b"first")).unwrap();
445            stream.flush().unwrap();
446            std::thread::sleep(Duration::from_millis(200));
447        });
448
449        let session = client(addr);
450        assert_eq!(session.send_request(1, b"a").unwrap(), 0);
451        assert_eq!(session.send_request(2, b"b").unwrap(), 1);
452
453        let first = session.blocking_receive_response(0).unwrap();
454        assert_eq!(&first[12..], b"first");
455        let second = session.blocking_receive_response(1).unwrap();
456        assert_eq!(&second[12..], b"second");
457
458        drop(session);
459        server.join().unwrap();
460    }
461
462    #[test]
463    fn unclaimed_response_stays_queued() {
464        let (addr, server) = mock_server(|mut stream| {
465            let mut scratch = [0u8; 64];
466            let _ = stream.read(&mut scratch);
467            stream.write_all(&message(11, 99, b"late")).unwrap();
468            stream.flush().unwrap();
469            std::thread::sleep(Duration::from_millis(300));
470        });
471
472        let session = client(addr);
473        session.send_request(11, &[]).unwrap();
474
475        // Polling for a different id must not consume the queued message.
476        let deadline = std::time::Instant::now() + Duration::from_secs(2);
477        loop {
478            assert!(session.try_receive_response(0).unwrap().is_none());
479            if let Some(message) = session.try_receive_response(99).unwrap() {
480                assert_eq!(&message[12..], b"late");
481                break;
482            }
483            assert!(
484                std::time::Instant::now() < deadline,
485                "response never arrived"
486            );
487            std::thread::sleep(Duration::from_millis(5));
488        }
489
490        drop(session);
491        server.join().unwrap();
492    }
493
494    #[test]
495    fn undersized_header_is_a_protocol_error() {
496        let (addr, server) = mock_server(|mut stream| {
497            let mut scratch = [0u8; 64];
498            let _ = stream.read(&mut scratch);
499            // size = 5 < sizeof(CommandHeader) = 12.
500            let mut header = Vec::new();
501            header.extend_from_slice(&1u32.to_le_bytes());
502            header.extend_from_slice(&0u32.to_le_bytes());
503            header.extend_from_slice(&5u32.to_le_bytes());
504            stream.write_all(&header).unwrap();
505            stream.flush().unwrap();
506            std::thread::sleep(Duration::from_millis(200));
507        });
508
509        let session = client(addr);
510        session.send_request(1, &[]).unwrap();
511        let error = session.blocking_receive_response(0).unwrap_err();
512        assert_eq!(error.to_string(), "libfranka: Incorrect TCP message size.");
513        assert!(matches!(error, FrankaError::Protocol(_)));
514
515        drop(session);
516        server.join().unwrap();
517    }
518
519    #[test]
520    fn oversized_header_is_a_protocol_error_without_allocating() {
521        let (addr, server) = mock_server(|mut stream| {
522            let mut scratch = [0u8; 64];
523            let _ = stream.read(&mut scratch);
524            // size = 0xFFFF_FFFF, far beyond MAX_TCP_MESSAGE_SIZE.
525            let mut header = Vec::new();
526            header.extend_from_slice(&1u32.to_le_bytes());
527            header.extend_from_slice(&0u32.to_le_bytes());
528            header.extend_from_slice(&0xFFFF_FFFFu32.to_le_bytes());
529            stream.write_all(&header).unwrap();
530            stream.flush().unwrap();
531            std::thread::sleep(Duration::from_millis(200));
532        });
533
534        let session = client(addr);
535        session.send_request(1, &[]).unwrap();
536        let error = session.blocking_receive_response(0).unwrap_err();
537        assert_eq!(error.to_string(), "libfranka: Incorrect TCP message size.");
538        assert!(matches!(error, FrankaError::Protocol(_)));
539
540        drop(session);
541        server.join().unwrap();
542    }
543
544    #[test]
545    fn connection_refused_uses_libfranka_text() {
546        // Bind and immediately drop, so the port is (almost certainly) closed.
547        let listener = TcpListener::bind("127.0.0.1:0").unwrap();
548        let addr = listener.local_addr().unwrap();
549        drop(listener);
550
551        match TcpSession::connect(addr, Duration::from_secs(1), HeaderLayout::Robot) {
552            Err(FrankaError::Network(message)) => assert_eq!(
553                message,
554                "libfranka: Connection to FCI refused. Please install FCI feature or enable FCI mode in Desk."
555            ),
556            other => panic!("expected a refused connection, got {other:?}"),
557        }
558    }
559
560    #[test]
561    fn closed_connection_is_detected() {
562        let (addr, server) = mock_server(|stream| {
563            drop(stream);
564        });
565        let session = client(addr);
566        server.join().unwrap();
567
568        // Give the FIN time to arrive.
569        let deadline = std::time::Instant::now() + Duration::from_secs(2);
570        loop {
571            match session.throw_if_connection_closed() {
572                Err(FrankaError::Network(message)) => {
573                    assert_eq!(message, "libfranka: server closed connection");
574                    break;
575                }
576                _ => assert!(std::time::Instant::now() < deadline, "FIN never observed"),
577            }
578            std::thread::sleep(Duration::from_millis(5));
579        }
580    }
581}
franka/network/tcp.rs

franka/network/
tcp.rs
