franka/network/
udp.rs

1//! UDP state/command channel.
2//!
3//! Port of `Network::udpReceive`, `Network::udpBlockingReceive` and `Network::udpSend` from
4//! libfranka 0.21.2 (`src/network.h`).
5//!
6//! Like libfranka the socket is bound to `0.0.0.0:0` and the server address is *learned* from
7//! the datagrams it sends (`Poco::DatagramSocket::receiveFrom` writes into
8//! `udp_server_address_`, which `udpSend` then uses). The real robot sends its states from port
9//! 1337, franka-sim from an ephemeral port; remembering the source address handles both.
10
11use std::mem::MaybeUninit;
12use std::net::{IpAddr, Ipv4Addr, Ipv6Addr, SocketAddr, SocketAddrV4, SocketAddrV6, UdpSocket};
13use std::os::fd::{AsRawFd, RawFd};
14use std::sync::Mutex;
15use std::time::Duration;
16
17use zerocopy::{FromBytes, Immutable, IntoBytes, KnownLayout};
18
19use crate::error::{FrankaError, FrankaResult};
20
21/// Default UDP receive timeout (libfranka's `Network` default `udp_timeout`).
22pub const DEFAULT_UDP_TIMEOUT: Duration = Duration::from_secs(1);
23
24/// The client's UDP socket for robot states and robot commands.
25///
26/// Neither [`UdpChannel::try_receive`] nor [`UdpChannel::send`] allocates, so both are safe to
27/// call from a 1 kHz control loop.
28#[derive(Debug)]
29pub struct UdpChannel {
30    socket: UdpSocket,
31    port: u16,
32    /// Source address of the most recently received datagram
33    /// (libfranka's `udp_server_address_`).
34    peer: Mutex<Option<SocketAddr>>,
35}
36
37impl UdpChannel {
38    /// Binds `0.0.0.0:0` and sets the receive timeout, as `franka::Network`'s constructor does.
39    pub fn bind(timeout: Duration) -> FrankaResult<UdpChannel> {
40        let socket = UdpSocket::bind((Ipv4Addr::UNSPECIFIED, 0))
41            .map_err(|e| FrankaError::Network(format!("libfranka: {e}")))?;
42        socket
43            .set_read_timeout(Some(timeout))
44            .map_err(|e| FrankaError::Network(format!("libfranka: {e}")))?;
45        let port = socket
46            .local_addr()
47            .map_err(|e| FrankaError::Network(format!("libfranka: {e}")))?
48            .port();
49        Ok(UdpChannel {
50            socket,
51            port,
52            peer: Mutex::new(None),
53        })
54    }
55
56    /// The local port, which is announced to the server in the `Connect` request.
57    pub fn port(&self) -> u16 {
58        self.port
59    }
60
61    /// Source address of the last received datagram, if any.
62    pub fn peer(&self) -> Option<SocketAddr> {
63        *self.peer.lock().unwrap_or_else(|e| e.into_inner())
64    }
65
66    /// Reads one datagram if one is already queued, without blocking and without allocating.
67    ///
68    /// Uses `recvfrom(2)` with `MSG_DONTWAIT` rather than toggling `O_NONBLOCK` around every
69    /// call, so a 1 kHz control loop performs exactly one syscall per poll. Returns `Ok(None)`
70    /// when no datagram is waiting (libfranka's `udpReceive` returning `false`).
71    ///
72    /// # Note
73    /// This raw byte API does **not** check the datagram length. libfranka's
74    /// `udpBlockingReceiveUnsafe` rejects `bytes_received != sizeof(T)` with
75    /// `ProtocolException("libfranka: incorrect object size")` (`network.h:140-142`); here that
76    /// is the caller's job. Parsing a short datagram against a full-size buffer would silently
77    /// read stale bytes, so prefer [`UdpChannel::try_receive_struct`], which performs the
78    /// check.
79    pub fn try_receive(&self, buf: &mut [u8]) -> FrankaResult<Option<usize>> {
80        match self.recvfrom(buf, true) {
81            Ok((n, from)) => {
82                self.remember(from);
83                Ok(Some(n))
84            }
85            Err(e)
86                if e.kind() == std::io::ErrorKind::WouldBlock
87                    || e.kind() == std::io::ErrorKind::TimedOut =>
88            {
89                Ok(None)
90            }
91            Err(e) => Err(FrankaError::Network(format!("libfranka: UDP receive: {e}"))),
92        }
93    }
94
95    /// Blocks until a datagram arrives or the receive timeout expires.
96    ///
97    /// A timeout is reported as `Network("libfranka: UDP receive: Timeout")`, which is the text
98    /// libfranka produces from `"libfranka: UDP receive: "s + Poco::TimeoutException::what()`.
99    ///
100    /// # Note
101    /// Like [`UdpChannel::try_receive`], this raw byte API does not check the datagram length;
102    /// use [`UdpChannel::blocking_receive_struct`] to get libfranka's
103    /// `"libfranka: incorrect object size"` check.
104    pub fn blocking_receive(&self, buf: &mut [u8]) -> FrankaResult<usize> {
105        match self.recvfrom(buf, false) {
106            Ok((n, from)) => {
107                self.remember(from);
108                Ok(n)
109            }
110            Err(e)
111                if e.kind() == std::io::ErrorKind::WouldBlock
112                    || e.kind() == std::io::ErrorKind::TimedOut =>
113            {
114                Err(FrankaError::Network(
115                    "libfranka: UDP receive: Timeout".to_string(),
116                ))
117            }
118            Err(e) => Err(FrankaError::Network(format!("libfranka: UDP receive: {e}"))),
119        }
120    }
121
122    /// Reads one datagram as a `T` if one is already queued, without blocking.
123    ///
124    /// Typed counterpart of [`UdpChannel::try_receive`] and the port of
125    /// `Network::udpReceive<T>` (`network.h:117-125`): the datagram is read into a `T`-sized
126    /// stack buffer (no allocation, so this is control-loop safe) and its length must be
127    /// exactly `size_of::<T>()`, otherwise
128    /// `Protocol("libfranka: incorrect object size")` is returned (`network.h:140-142`).
129    ///
130    /// A datagram longer than `size_of::<T>()` is truncated by `recvfrom` and therefore looks
131    /// like an exact-size read — the same blind spot libfranka has.
132    pub fn try_receive_struct<T>(&self) -> FrankaResult<Option<T>>
133    where
134        T: FromBytes + IntoBytes + Immutable + KnownLayout,
135    {
136        let mut value = T::new_zeroed();
137        match self.try_receive(value.as_mut_bytes())? {
138            None => Ok(None),
139            Some(n) if n == std::mem::size_of::<T>() => Ok(Some(value)),
140            Some(_) => Err(FrankaError::Protocol(
141                "libfranka: incorrect object size".to_string(),
142            )),
143        }
144    }
145
146    /// Blocks until a datagram arrives and returns it as a `T`.
147    ///
148    /// Typed counterpart of [`UdpChannel::blocking_receive`] and the port of
149    /// `Network::udpBlockingReceive<T>` (`network.h:128-132`), including the
150    /// `Protocol("libfranka: incorrect object size")` check on the datagram length.
151    pub fn blocking_receive_struct<T>(&self) -> FrankaResult<T>
152    where
153        T: FromBytes + IntoBytes + Immutable + KnownLayout,
154    {
155        let mut value = T::new_zeroed();
156        let received = self.blocking_receive(value.as_mut_bytes())?;
157        if received != std::mem::size_of::<T>() {
158            return Err(FrankaError::Protocol(
159                "libfranka: incorrect object size".to_string(),
160            ));
161        }
162        Ok(value)
163    }
164
165    /// Sends `data` to `addr`, which is normally [`UdpChannel::peer`].
166    ///
167    /// A short write is reported as `Network("libfranka: could not send UDP data")`, matching
168    /// `udpSend`.
169    pub fn send(&self, addr: SocketAddr, data: &[u8]) -> FrankaResult<()> {
170        let sent = self
171            .socket
172            .send_to(data, addr)
173            .map_err(|e| FrankaError::Network(format!("libfranka: UDP send: {e}")))?;
174        if sent != data.len() {
175            return Err(FrankaError::Network(
176                "libfranka: could not send UDP data".to_string(),
177            ));
178        }
179        Ok(())
180    }
181
182    fn remember(&self, from: SocketAddr) {
183        *self.peer.lock().unwrap_or_else(|e| e.into_inner()) = Some(from);
184    }
185
186    /// `recvfrom(2)` into `buf`, retrying on `EINTR`.
187    fn recvfrom(&self, buf: &mut [u8], non_blocking: bool) -> std::io::Result<(usize, SocketAddr)> {
188        let fd: RawFd = self.socket.as_raw_fd();
189        let flags = if non_blocking { libc::MSG_DONTWAIT } else { 0 };
190        let mut storage = MaybeUninit::<libc::sockaddr_storage>::zeroed();
191        loop {
192            let mut len = std::mem::size_of::<libc::sockaddr_storage>() as libc::socklen_t;
193            let n = unsafe {
194                libc::recvfrom(
195                    fd,
196                    buf.as_mut_ptr().cast(),
197                    buf.len(),
198                    flags,
199                    storage.as_mut_ptr().cast(),
200                    &mut len,
201                )
202            };
203            if n < 0 {
204                let e = std::io::Error::last_os_error();
205                if e.kind() == std::io::ErrorKind::Interrupted {
206                    continue;
207                }
208                return Err(e);
209            }
210            let addr = unsafe { socket_addr_from_storage(storage.assume_init_ref()) }.ok_or_else(
211                || {
212                    std::io::Error::new(
213                        std::io::ErrorKind::InvalidData,
214                        "unsupported address family",
215                    )
216                },
217            )?;
218            return Ok((n as usize, addr));
219        }
220    }
221}
222
223/// Converts a `sockaddr_storage` filled by `recvfrom` into a [`SocketAddr`] without allocating.
224///
225/// # Safety
226/// `storage` must have been filled by a successful `recvfrom`.
227unsafe fn socket_addr_from_storage(storage: &libc::sockaddr_storage) -> Option<SocketAddr> {
228    match storage.ss_family as libc::c_int {
229        libc::AF_INET => {
230            let addr = &*(storage as *const libc::sockaddr_storage as *const libc::sockaddr_in);
231            Some(SocketAddr::V4(SocketAddrV4::new(
232                Ipv4Addr::from(u32::from_be(addr.sin_addr.s_addr)),
233                u16::from_be(addr.sin_port),
234            )))
235        }
236        libc::AF_INET6 => {
237            let addr = &*(storage as *const libc::sockaddr_storage as *const libc::sockaddr_in6);
238            Some(SocketAddr::V6(SocketAddrV6::new(
239                Ipv6Addr::from(addr.sin6_addr.s6_addr),
240                u16::from_be(addr.sin6_port),
241                u32::from_be(addr.sin6_flowinfo),
242                addr.sin6_scope_id,
243            )))
244        }
245        _ => None,
246    }
247}
248
249/// Convenience: the loopback address of `port`, used when a test needs an explicit peer.
250pub fn loopback(port: u16) -> SocketAddr {
251    SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), port)
252}
253
254#[cfg(test)]
255mod tests {
256    use super::*;
257
258    #[test]
259    fn learns_the_peer_and_sends_back() {
260        let channel = UdpChannel::bind(Duration::from_millis(200)).unwrap();
261        let server = UdpSocket::bind((Ipv4Addr::LOCALHOST, 0)).unwrap();
262        server
263            .set_read_timeout(Some(Duration::from_secs(2)))
264            .unwrap();
265
266        let mut buf = [0u8; 64];
267        assert_eq!(channel.try_receive(&mut buf).unwrap(), None);
268        assert_eq!(channel.peer(), None);
269
270        server
271            .send_to(b"state", (Ipv4Addr::LOCALHOST, channel.port()))
272            .unwrap();
273
274        // The datagram may take a moment to be queued.
275        let deadline = std::time::Instant::now() + Duration::from_secs(2);
276        let received = loop {
277            if let Some(n) = channel.try_receive(&mut buf).unwrap() {
278                break n;
279            }
280            assert!(
281                std::time::Instant::now() < deadline,
282                "datagram never arrived"
283            );
284        };
285        assert_eq!(&buf[..received], b"state");
286        assert_eq!(channel.peer(), Some(server.local_addr().unwrap()));
287
288        channel.send(channel.peer().unwrap(), b"command").unwrap();
289        let mut back = [0u8; 64];
290        let (n, from) = server.recv_from(&mut back).unwrap();
291        assert_eq!(&back[..n], b"command");
292        assert_eq!(from.port(), channel.port());
293    }
294
295    #[test]
296    fn typed_receive_checks_the_datagram_size() {
297        use crate::wire::gripper::GripperState;
298        use zerocopy::little_endian::{F64, U16, U32};
299        use zerocopy::IntoBytes;
300
301        let channel = UdpChannel::bind(Duration::from_secs(2)).unwrap();
302        let server = UdpSocket::bind((Ipv4Addr::LOCALHOST, 0)).unwrap();
303        let target = (Ipv4Addr::LOCALHOST, channel.port());
304
305        assert!(channel
306            .try_receive_struct::<GripperState>()
307            .unwrap()
308            .is_none());
309
310        // A short datagram is rejected with libfranka's ProtocolException text.
311        server.send_to(&[1u8, 2, 3], target).unwrap();
312        let error = channel
313            .blocking_receive_struct::<GripperState>()
314            .unwrap_err();
315        assert_eq!(error.to_string(), "libfranka: incorrect object size");
316        assert!(matches!(error, FrankaError::Protocol(_)));
317
318        // An exact-size datagram parses.
319        let state = GripperState {
320            message_id: U32::new(7),
321            width: F64::new(0.05),
322            max_width: F64::new(0.08),
323            is_grasped: 1,
324            temperature: U16::new(21),
325        };
326        server.send_to(state.as_bytes(), target).unwrap();
327        let received = channel.blocking_receive_struct::<GripperState>().unwrap();
328        assert_eq!(received.message_id.get(), 7);
329        assert_eq!(received.width.get(), 0.05);
330        assert_eq!(received.is_grasped, 1);
331        assert_eq!(received.temperature.get(), 21);
332    }
333
334    #[test]
335    fn blocking_receive_reports_libfranka_timeout_text() {
336        let channel = UdpChannel::bind(Duration::from_millis(50)).unwrap();
337        let mut buf = [0u8; 64];
338        let error = channel.blocking_receive(&mut buf).unwrap_err();
339        assert_eq!(error.to_string(), "libfranka: UDP receive: Timeout");
340        assert!(matches!(error, FrankaError::Network(_)));
341    }
342
343    #[test]
344    fn blocking_receive_returns_a_datagram() {
345        let channel = UdpChannel::bind(Duration::from_secs(2)).unwrap();
346        let server = UdpSocket::bind((Ipv4Addr::LOCALHOST, 0)).unwrap();
347        let port = channel.port();
348        let sender = std::thread::spawn(move || {
349            std::thread::sleep(Duration::from_millis(20));
350            server
351                .send_to(b"hello", (Ipv4Addr::LOCALHOST, port))
352                .unwrap();
353        });
354
355        let mut buf = [0u8; 64];
356        let n = channel.blocking_receive(&mut buf).unwrap();
357        assert_eq!(&buf[..n], b"hello");
358        sender.join().unwrap();
359    }
360}
franka/network/udp.rs

franka/network/
udp.rs
