franka/network/
mod.rs

1//! The two sockets of an FCI session and the `Connect` handshake.
2//!
3//! Port of `franka::Network` (libfranka 0.21.2 `src/network.{h,cpp}`) split into a TCP command
4//! channel ([`TcpSession`]) and a UDP state/command channel ([`UdpChannel`]).
5
6pub mod tcp;
7pub mod udp;
8
9use std::net::{IpAddr, SocketAddr, ToSocketAddrs};
10use std::time::Duration;
11
12use zerocopy::{FromBytes, Immutable, IntoBytes, KnownLayout};
13
14use crate::error::{FrankaError, FrankaResult};
15use crate::wire::gripper::{
16    GripperConnectRequest, GripperConnectResponse, GripperConnectStatus, GRIPPER_VERSION,
17};
18use crate::wire::robot::{ConnectRequest, ConnectResponse, ConnectStatus};
19use crate::wire::{parse_response, HeaderLayout};
20
21pub use tcp::{TcpSession, DEFAULT_CONNECT_TIMEOUT, POLL_TIMEOUT};
22pub use udp::{UdpChannel, DEFAULT_UDP_TIMEOUT};
23
24/// A connected FCI session: the command socket, the state socket, and the server's IP.
25#[derive(Debug)]
26pub struct Network {
27    /// TCP command channel.
28    pub tcp: TcpSession,
29    /// UDP state/command channel.
30    pub udp: UdpChannel,
31    /// IP address the command socket is connected to.
32    pub server_ip: IpAddr,
33}
34
35impl Network {
36    /// Connects to `host` and binds the UDP socket, exactly like `franka::Network`'s
37    /// constructor. `host` may be `"host"` or `"host:port"`; an explicit port overrides
38    /// `tcp_port`.
39    ///
40    /// `layout` selects the 12-byte robot or the 10-byte gripper header for TCP framing.
41    pub fn connect(host: &str, tcp_port: u16, layout: HeaderLayout) -> FrankaResult<Network> {
42        Network::connect_with_timeouts(
43            host,
44            tcp_port,
45            layout,
46            DEFAULT_CONNECT_TIMEOUT,
47            DEFAULT_UDP_TIMEOUT,
48        )
49    }
50
51    /// [`Network::connect`] with explicit timeouts (libfranka's `tcp_timeout` / `udp_timeout`).
52    pub fn connect_with_timeouts(
53        host: &str,
54        tcp_port: u16,
55        layout: HeaderLayout,
56        tcp_timeout: Duration,
57        udp_timeout: Duration,
58    ) -> FrankaResult<Network> {
59        let addr = resolve(host, tcp_port)?;
60        let tcp = TcpSession::connect(addr, tcp_timeout, layout)?;
61        let udp = UdpChannel::bind(udp_timeout)?;
62        Ok(Network {
63            tcp,
64            udp,
65            server_ip: addr.ip(),
66        })
67    }
68
69    /// Source address of the last state datagram, i.e. where robot commands must be sent.
70    pub fn udp_peer(&self) -> Option<SocketAddr> {
71        self.udp.peer()
72    }
73
74    /// Reads one state datagram as a `T` if one is already queued, without blocking.
75    ///
76    /// Pass-through to [`UdpChannel::try_receive_struct`] that additionally shuts the command
77    /// socket down when the receive fails at the network level, mirroring the
78    /// `tcp_socket_.shutdown()` in libfranka's `udpBlockingReceiveUnsafe` catch block
79    /// (`network.h:146`). A size mismatch is *not* a network failure — libfranka's
80    /// `ProtocolException` is thrown inside the `try` and is not caught by that handler — so
81    /// the socket stays up in that case, exactly as in the C++.
82    pub fn try_receive_struct<T>(&self) -> FrankaResult<Option<T>>
83    where
84        T: FromBytes + IntoBytes + Immutable + KnownLayout,
85    {
86        self.shutdown_tcp_on_network_error(self.udp.try_receive_struct())
87    }
88
89    /// Blocks until a state datagram arrives and returns it as a `T`.
90    ///
91    /// Pass-through to [`UdpChannel::blocking_receive_struct`] with the same
92    /// shut-down-the-TCP-socket-on-network-failure behaviour as
93    /// [`Network::try_receive_struct`]. A receive timeout counts as a network failure, which is
94    /// what makes the next TCP command fail fast instead of blocking on a dead session.
95    pub fn blocking_receive_struct<T>(&self) -> FrankaResult<T>
96    where
97        T: FromBytes + IntoBytes + Immutable + KnownLayout,
98    {
99        self.shutdown_tcp_on_network_error(self.udp.blocking_receive_struct())
100    }
101
102    /// Reads one datagram into `buf` if one is already queued, without blocking, and returns how
103    /// many bytes it held.
104    ///
105    /// Byte-slice counterpart of [`Network::try_receive_struct`], with the same
106    /// shut-down-the-TCP-socket-on-network-failure behaviour. The length check is the caller's
107    /// job: the version-aware state reader in [`crate::robot`] passes a buffer sized for the
108    /// *largest* supported `RobotState` and compares the returned length against the negotiated
109    /// version's size, so a datagram of the other version is rejected instead of being silently
110    /// truncated.
111    pub fn try_receive_bytes(&self, buf: &mut [u8]) -> FrankaResult<Option<usize>> {
112        self.shutdown_tcp_on_network_error(self.udp.try_receive(buf))
113    }
114
115    /// Blocks until a datagram arrives, reads it into `buf` and returns its length.
116    ///
117    /// Byte-slice counterpart of [`Network::blocking_receive_struct`]; see
118    /// [`Network::try_receive_bytes`] for why the length check is left to the caller.
119    pub fn blocking_receive_bytes(&self, buf: &mut [u8]) -> FrankaResult<usize> {
120        self.shutdown_tcp_on_network_error(self.udp.blocking_receive(buf))
121    }
122
123    fn shutdown_tcp_on_network_error<T>(&self, result: FrankaResult<T>) -> FrankaResult<T> {
124        if let Err(FrankaError::Network(_)) = &result {
125            self.tcp.shutdown();
126        }
127        result
128    }
129
130    /// Sends a robot command to the address the last state came from.
131    ///
132    /// Fails before the first state has been received, which cannot happen in a control loop:
133    /// libfranka's `udp_server_address_` is likewise only valid after the first
134    /// `receiveFrom`.
135    pub fn send_udp(&self, data: &[u8]) -> FrankaResult<()> {
136        let peer = self.udp_peer().ok_or_else(|| {
137            FrankaError::Network(
138                "libfranka: UDP send: no state received yet, server address unknown".to_string(),
139            )
140        })?;
141        self.udp.send(peer, data)
142    }
143}
144
145/// Resolves `"host"` or `"host:port"` to a single socket address, preferring the port embedded
146/// in the string.
147fn resolve(host: &str, default_port: u16) -> FrankaResult<SocketAddr> {
148    if let Ok(mut addrs) = host.to_socket_addrs() {
149        if let Some(addr) = addrs.next() {
150            return Ok(addr);
151        }
152    }
153    let mut addrs = (host, default_port)
154        .to_socket_addrs()
155        .map_err(|e| FrankaError::Network(format!("libfranka: Connection error: {e}")))?;
156    addrs.next().ok_or_else(|| {
157        FrankaError::Network(format!(
158            "libfranka: Connection error: host {host} did not resolve to any address"
159        ))
160    })
161}
162
163/// Performs the `Connect` handshake and returns the server's protocol version.
164///
165/// Port of the `franka::connect<T, kLibraryVersion>` template in `src/network.h`: send the
166/// request carrying the client's UDP port, block for the reply, and map the status.
167/// `kIncompatibleLibraryVersion` becomes [`FrankaError::IncompatibleVersion`], any other
168/// non-success status becomes `Protocol("libfranka: Protocol error during connection attempt")`.
169///
170/// The robot and the gripper differ only in the width of the status field (`u8` vs `u16`), so
171/// the layout chosen at connect time selects the right response struct.
172pub fn connect_handshake(network: &Network, library_version: u16) -> FrankaResult<u16> {
173    use zerocopy::IntoBytes;
174
175    let layout = network.tcp.layout();
176    let udp_port = network.udp.port();
177
178    // The robot's `Connect::Status` (`u8`) and the gripper's (`u16`) are two distinct enums
179    // that happen to agree on their first two values today; each response is therefore parsed
180    // against *its own* enum and only the outcome is shared.
181    let (status, server_version) = match layout {
182        HeaderLayout::Robot => {
183            let request = ConnectRequest::new(library_version, udp_port);
184            let command_id = network.tcp.send_request(
185                crate::wire::robot::Command::Connect.to_u32(),
186                request.as_bytes(),
187            )?;
188            let message = network.tcp.blocking_receive_response(command_id)?;
189            let response: ConnectResponse = parse_response(layout, &message)?;
190            let status = match ConnectStatus::from_u8(response.status) {
191                Some(ConnectStatus::Success) => HandshakeStatus::Success,
192                Some(ConnectStatus::IncompatibleLibraryVersion) => {
193                    HandshakeStatus::IncompatibleLibraryVersion
194                }
195                None => HandshakeStatus::Other,
196            };
197            (status, response.version.get())
198        }
199        HeaderLayout::Gripper => {
200            let request = GripperConnectRequest::new(library_version, udp_port);
201            let command_id = network.tcp.send_request(
202                crate::wire::gripper::GripperCommand::Connect.to_u16() as u32,
203                request.as_bytes(),
204            )?;
205            let message = network.tcp.blocking_receive_response(command_id)?;
206            let response: GripperConnectResponse = parse_response(layout, &message)?;
207            let status = match GripperConnectStatus::from_u16(response.status.get()) {
208                Some(GripperConnectStatus::Success) => HandshakeStatus::Success,
209                Some(GripperConnectStatus::IncompatibleLibraryVersion) => {
210                    HandshakeStatus::IncompatibleLibraryVersion
211                }
212                None => HandshakeStatus::Other,
213            };
214            (status, response.version.get())
215        }
216    };
217
218    match status {
219        HandshakeStatus::Success => Ok(server_version),
220        HandshakeStatus::IncompatibleLibraryVersion => Err(FrankaError::IncompatibleVersion {
221            server_version,
222            library_version,
223        }),
224        HandshakeStatus::Other => Err(FrankaError::Protocol(
225            "libfranka: Protocol error during connection attempt".to_string(),
226        )),
227    }
228}
229
230/// The outcome of a `Connect` handshake, shared by the robot's and the gripper's own status
231/// enums so that neither is compared against the other's values.
232enum HandshakeStatus {
233    Success,
234    IncompatibleLibraryVersion,
235    Other,
236}
237
238/// Connects to a robot and performs the `Connect` handshake, returning the session and the
239/// server's FCI version.
240pub fn connect_robot(host: &str) -> FrankaResult<(Network, u16)> {
241    let network = Network::connect(host, crate::wire::ROBOT_COMMAND_PORT, HeaderLayout::Robot)?;
242    let version = connect_handshake(&network, crate::wire::ROBOT_VERSION)?;
243    Ok((network, version))
244}
245
246/// Connects to a gripper and performs the `Connect` handshake, returning the session and the
247/// server's gripper protocol version.
248pub fn connect_gripper(host: &str) -> FrankaResult<(Network, u16)> {
249    let network = Network::connect(
250        host,
251        crate::wire::gripper::GRIPPER_COMMAND_PORT,
252        HeaderLayout::Gripper,
253    )?;
254    let version = connect_handshake(&network, GRIPPER_VERSION)?;
255    Ok((network, version))
256}
257
258#[cfg(test)]
259mod tests {
260    use super::*;
261    use crate::wire::gripper::GripperState;
262
263    /// A UDP receive failure must shut the command socket down, as
264    /// `Network::udpBlockingReceiveUnsafe` does (`network.h:146`), so the next TCP operation
265    /// fails immediately instead of hanging on a session the robot has abandoned.
266    #[test]
267    fn udp_failure_shuts_down_the_command_socket() {
268        let listener = std::net::TcpListener::bind("127.0.0.1:0").unwrap();
269        let addr = listener.local_addr().unwrap();
270        let server = std::thread::spawn(move || {
271            let (stream, _) = listener.accept().unwrap();
272            std::thread::sleep(Duration::from_millis(500));
273            drop(stream);
274        });
275
276        let network = Network::connect_with_timeouts(
277            &addr.to_string(),
278            crate::wire::ROBOT_COMMAND_PORT,
279            HeaderLayout::Robot,
280            Duration::from_secs(1),
281            Duration::from_millis(50),
282        )
283        .unwrap();
284        assert!(network.tcp.send_request(0, &[]).is_ok());
285
286        let error = network
287            .blocking_receive_struct::<GripperState>()
288            .unwrap_err();
289        assert_eq!(error.to_string(), "libfranka: UDP receive: Timeout");
290        assert!(network.tcp.send_request(0, &[]).is_err());
291
292        drop(network);
293        server.join().unwrap();
294    }
295}
franka/network/mod.rs

franka/network/
mod.rs
