Proposal: find motors with a broadcast ping, and set their id and baud rate

Changelog.md

@@ -23,6 +23,31 @@
   `ServoDefinition`; it fails the build on a servo without a `lock` register.
 - Python: `Bus.is_open`, and `Bus` as a context manager: `with Bus(...) as bus:` releases
   the serial port on the way out, an exception raised in the block included.
+- `DynamixelProtocolHandler::broadcast_ping` pings every motor at once and returns the ids
+  that answer, listening for `broadcast_ping_window(baudrate)`: the status packets of
+  every possible id, 3 ms of turn per id and 16 ms for a USB adapter's latency timer, the
+  window the vendor SDKs use (about 0.8 s at 1 Mbps). Protocol v2 Dynamixel and Feetech
+  STS answer it; protocol v1 Dynamixel and Feetech SCS do not.
+- `broadcast_scan(ids)` / `broadcast_scan_all()` on `ServoDefinition`, the controllers and
+  `Bus` (`broadcast_scan(ids=None)` on Python): which ids answer, found with one broadcast
+  ping, then the model number of each id asked that answered, read under the port's own
+  timeout. The ping's window does not shrink with the number of ids, so `scan` stays the
+  fast way to check a few ids and is unchanged; `broadcast_scan` is for sweeping many,
+  and for USB adapters whose latency timer outlasts `scan`'s short timeout. Servos state
+  `supports_broadcast_ping: true` (STS3215, XL320, XL330, XL430) on `ServoInfo`, the
+  Python controller classes and `ServoDefinition`; on any other, `broadcast_scan` is a
+  `RegisterError::BroadcastPing` (`ValueError` on Python) and nothing is sent.
+- `change_id(definition, id, new_id)` and `change_baudrate(definition, id, baudrate)` on
+  `ServoDefinition` and `Bus` (and the Python `Bus`): torque off and, on servos with a
+  lock register, lock open, then the write. The baud rate is given in bauds and written
+  as the value the definition's `baudrates` lists for it; a rate it does not list is a
+  `RegisterError::Baudrate` (`ValueError` on Python) and nothing is sent; the port stays
+  at its rate, for the caller to switch with `set_baudrate`. Like `scan`, both reach
+  motors the bus does not have.
+- Python: `baudrates()` (slowest first) and `models()` (by name) on the controller classes
+  return their dict in a fixed order, like the `ServoDefinition` getters. They came from
+  a `HashMap`, so the order changed from one process to the next, and so did the order in
+  which a caller trying each baud rate found a motor.
 
 ## Version 1.10.0
 

README.md

@@ -28,9 +28,10 @@ If you want to quickly setup/check motors configuration, you can directly use a
 
 ## APIs
 
-It exposes two APIs:
+It exposes three APIs, from the wire up:
 * `DynamixelProtocolHandler`: low-level API. It handles the serial communication and the Dynamixel protocol parsing. It can be used for fine-grained control of the shared bus with other communication.
-* `Controller`: high-level API for the Dynamixel protocol. Simpler and cleaner API but it takes full ownership of the io (it can still be shared if wrapped with a mutex for instance).
+* `Controller`: one per servo model, with a typed accessor for each register. Simpler and cleaner API but it takes full ownership of the io (it can still be shared if wrapped with a mutex for instance).
+* `Bus` and `ServoDefinition`: a higher-level, family-agnostic API, designed mostly for the [LeRobot](https://github.com/huggingface/lerobot) library. Motors of several models and of both protocols on one port, registers by name, the facts about each servo, finding and setting up motors, and the torque of a whole bus. Some of its choices are LeRobot conventions rather than vendor rules; the controllers stay the per-servo API underneath.
 
 See the examples below for usage.
 
@@ -207,7 +208,9 @@ info.resolution(), info.word_order()  # (4096, 'little')
 info.register("present_position").sign_bit  # 15
 ```
 
-### Mixed buses
+### Mixed buses, and the LeRobot-oriented API
+
+`Bus` and `ServoDefinition` are the higher-level, family-agnostic API (see [APIs](#apis)), designed mostly for the [LeRobot](https://github.com/huggingface/lerobot) library; the controllers above stay the per-servo API.
 
 A controller speaks one servo definition. When a port carries motors of several definitions, or of both protocols (Reachy Mini has STS3215 motors on protocol v1 and XL330 motors on v2 on one port), open a `Bus` with the definition of each motor:
 
@@ -226,6 +229,15 @@ found = bus.scan(Xl330PyController.definition())  # the v2 motors, as {id: model
 
 Each motor is read and written through its own definition and protocol. A sync read or write sends one instruction per group of motors sharing a protocol and the register's address and size, and hands the values back in the order asked: here, one Sync Read for motor 11 and one for motors 1 and 2. A group with a motor that has no Sync Read is read one id at a time.
 
+Beyond register access, they carry what a library driving several motor families needs:
+
+- `find_model("xl330-m288")` returns the model number and the definition, whose getters give the servo's facts (`resolution`, `baudrates`, `factory_baudrate`, `homing_offset_sign`, `operating_modes`, ...);
+- `bus.broadcast_scan(definition)` finds the motors on the port with one broadcast ping, and `bus.change_id` / `bus.change_baudrate` set a new motor up;
+- `bus.set_torque(ids, enabled)` turns the torque of every motor on or off, and reports the ones that failed;
+- `with Bus(...) as bus:` releases the port on the way out.
+
+Some of these follow LeRobot's conventions rather than a vendor's: `set_torque` closes the lock of a Feetech servo when the torque goes on and opens it when it goes off.
+
 In Rust the same access is on each definition, taking the protocol handler and the port like the typed module functions: `sts3215::DEFINITION.read_register(&dph_v1, port, 11, "present_position")`. `servo::definition::sync_read_register` and `sync_write_register` reach motors of several definitions sharing a protocol and a layout in one instruction, and `bus::Bus` groups them for a whole bus.
 
 ### Threading and the GIL

src/bus.rs

@@ -5,6 +5,12 @@
 //! STS3215 (protocol v1) and XL330 (protocol v2) motors on one port. A [`Bus`] holds the
 //! port, a handler for each protocol and the definition of each motor, and calls the
 //! definition's functions with the handler that matches it.
+//!
+//! With [`ServoDefinition`], it is rustypot's higher-level, family-agnostic API, designed
+//! mostly for the LeRobot library: registers by name, the facts about each servo, finding
+//! and setting up motors, the torque of a whole bus. Some of its choices are LeRobot
+//! conventions rather than vendor rules, such as the Feetech lock following the torque in
+//! [`Bus::set_torque`]. The per-servo controllers stay the API underneath.
 
 use std::collections::BTreeMap;
 use std::time::Duration;
@@ -193,6 +199,24 @@ impl Bus {
         self.scan(definition, &ids)
     }
 
+    /// Which of `ids` answer, with their model number read as `definition` lays it out:
+    /// one broadcast ping, then a read of each id asked that answered. See
+    /// [`ServoDefinition::broadcast_scan`].
+    pub fn broadcast_scan(
+        &mut self,
+        definition: ServoDefinition,
+        ids: &[u8],
+    ) -> Result<BTreeMap<u8, u16>> {
+        let dph = &self.handlers[definition.protocol as usize - 1];
+        definition.broadcast_scan(dph, self.serial_port.as_mut(), ids)
+    }
+
+    /// [`broadcast_scan`](Self::broadcast_scan) every id `definition`'s protocol allows.
+    pub fn broadcast_scan_all(&mut self, definition: ServoDefinition) -> Result<BTreeMap<u8, u16>> {
+        let ids: Vec<u8> = (0..=self.handlers[definition.protocol as usize - 1].max_id()).collect();
+        self.broadcast_scan(definition, &ids)
+    }
+
     /// Turn the torque of `ids` on or off. Every motor is tried, even after one fails,
     /// and a failure on the bus is tried again up to `retries` more times; the motors
     /// that still failed come back with their error.
@@ -224,6 +248,27 @@ impl Bus {
         failed
     }
 
+    /// Give motor `id` the id `new_id`, through `definition`. See
+    /// [`ServoDefinition::change_id`]. Like [`scan`](Self::scan), it reaches motors the
+    /// bus does not have: a new motor answers at its factory id.
+    pub fn change_id(&mut self, definition: ServoDefinition, id: u8, new_id: u8) -> Result<()> {
+        let dph = &self.handlers[definition.protocol as usize - 1];
+        definition.change_id(dph, self.serial_port.as_mut(), id, new_id)
+    }
+
+    /// Set motor `id` to talk at `baudrate`, through `definition`. See
+    /// [`ServoDefinition::change_baudrate`]. The bus's port stays at the old rate: call
+    /// [`set_baudrate`](Self::set_baudrate) to keep talking to the motor afterwards.
+    pub fn change_baudrate(
+        &mut self,
+        definition: ServoDefinition,
+        id: u8,
+        baudrate: u32,
+    ) -> Result<()> {
+        let dph = &self.handlers[definition.protocol as usize - 1];
+        definition.change_baudrate(dph, self.serial_port.as_mut(), id, baudrate)
+    }
+
     /// Switch the open serial port to `baudrate`.
     pub fn set_baudrate(&mut self, baudrate: u32) -> Result<()> {
         Ok(self.serial_port.set_baud_rate(baudrate)?)
@@ -257,7 +302,10 @@ mod python {
     use super::Bus;
     use crate::servo::{RegisterError, ServoDefinition};
 
-    /// Motors of several definitions, and of both protocols, on one serial port.
+    /// Motors of several definitions, and of both protocols, on one serial port: the
+    /// higher-level, family-agnostic API, designed mostly for the LeRobot library, where
+    /// the controller classes are the per-servo one. Some of its choices are LeRobot
+    /// conventions, such as the Feetech lock following the torque in `set_torque`.
     ///
     /// ```python
     /// with Bus("/dev/ttyUSB0", 1_000_000, 0.1, {
@@ -487,6 +535,32 @@ mod python {
             })
         }
 
+        /// Give motor `id` the id `new_id`, through `definition`: torque off and, on a
+        /// Feetech motor, lock open first. The motor need not be one of the bus's.
+        pub fn change_id(
+            &self,
+            py: Python,
+            definition: ServoDefinition,
+            id: u8,
+            new_id: u8,
+        ) -> PyResult<()> {
+            self.run(py, |bus| bus.change_id(definition, id, new_id))
+        }
+
+        /// Set motor `id` to talk at `baudrate`, through `definition`, as `change_id`
+        /// does it. A rate the servo cannot take raises `ValueError`. The bus's port stays
+        /// at the old rate: call `set_baudrate(baudrate)` to keep talking to the motor
+        /// afterwards.
+        pub fn change_baudrate(
+            &self,
+            py: Python,
+            definition: ServoDefinition,
+            id: u8,
+            baudrate: u32,
+        ) -> PyResult<()> {
+            self.run(py, |bus| bus.change_baudrate(definition, id, baudrate))
+        }
+
         /// Which of `ids` answer, as {id: model number} read as `definition` lays it out,
         /// over its protocol; every id that protocol allows when `ids` is left out.
         #[pyo3(signature = (definition, ids = None))]
@@ -501,6 +575,26 @@ mod python {
                 None => bus.scan_all(definition),
             })
         }
+
+        /// Which of `ids` answer, as {id: model number} read as `definition` lays it out;
+        /// every id its protocol allows when `ids` is left out. One broadcast ping, which
+        /// listens for its whole window (about 0.8 s at 1 Mbps) however few ids are asked,
+        /// then a read of each id asked that answered: faster than `scan` over many ids,
+        /// and safe behind a USB adapter with a long latency timer. A servo that does not
+        /// answer a broadcast ping (`definition.supports_broadcast_ping`) raises
+        /// `ValueError`.
+        #[pyo3(signature = (definition, ids = None))]
+        pub fn broadcast_scan(
+            &self,
+            py: Python,
+            definition: ServoDefinition,
+            ids: Option<Vec<u8>>,
+        ) -> PyResult<BTreeMap<u8, u16>> {
+            self.run(py, |bus| match &ids {
+                Some(ids) => bus.broadcast_scan(definition, ids),
+                None => bus.broadcast_scan_all(definition),
+            })
+        }
     }
 }
 
@@ -592,6 +686,22 @@ mod tests {
         assert_eq!([written[1][5], written[1][6]], [55, 1]);
     }
 
+    #[test]
+    fn a_motor_the_bus_does_not_have_can_be_given_its_id() {
+        // A new STS3215 answers at id 1, which the bus gives to an XL330.
+        let status = vec![0xFF, 0xFF, 0x01, 0x02, 0x00, 0xFC];
+        let port = FakePort::new(vec![status.clone(), status.clone(), status]);
+        let written = port.written();
+        let mut bus = Bus::new(Box::new(port), BTreeMap::from([(1, xl330::DEFINITION)]));
+
+        bus.change_id(sts3215::DEFINITION, 1, 11).unwrap();
+
+        // Protocol v1 all the way: three writes, the last one the id.
+        let written = written.lock().unwrap();
+        assert_eq!(written.len(), 3);
+        assert_eq!(written[2][..7], [0xFF, 0xFF, 0x01, 0x04, 0x03, 0x05, 11]);
+    }
+
     #[test]
     fn a_value_that_does_not_fit_leaves_the_whole_bus_unwritten() {
         let port = FakePort::new(vec![]);

src/dynamixel_protocol/mod.rs

@@ -178,6 +178,43 @@ impl DynamixelProtocolHandler {
         }
     }
 
+    /// Ping every motor at once, and return the ids that answer.
+    ///
+    /// Each motor that hears the broadcast ping answers with its own status packet, in
+    /// turn, so finding all of them takes one listening window
+    /// ([`broadcast_ping_window`](Self::broadcast_ping_window) at the port's baud rate)
+    /// instead of one timeout per absent id. The port's timeout is put back afterwards.
+    ///
+    /// Only some firmwares answer: protocol v2 Dynamixel and Feetech STS do, protocol v1
+    /// Dynamixel and Feetech SCS stay silent, which reads as no motor at all.
+    pub fn broadcast_ping(&self, serial_port: &mut dyn serialport::SerialPort) -> Result<Vec<u8>> {
+        let window = self.broadcast_ping_window(serial_port.baud_rate()?);
+        let timeout = serial_port.timeout();
+        let ids = match &self.protocol {
+            ProtocolKind::V1(p) => p.broadcast_ping(serial_port, window),
+            ProtocolKind::V2(p, _) => p.broadcast_ping(serial_port, window),
+        };
+        serial_port.set_timeout(timeout)?;
+        self.sleep_post_delay();
+        ids
+    }
+
+    /// How long a broadcast ping listens at `baudrate`: the window the vendor SDKs use.
+    ///
+    /// The status packets of every id the protocol allows (6 bytes each on v1, 14 on v2),
+    /// 3 ms of turn per id, and 16 ms for the latency timer of a USB serial adapter, which
+    /// can hold a short answer that long before passing it on.
+    pub fn broadcast_ping_window(&self, baudrate: u32) -> Duration {
+        let (status_size, max_id) = match self.protocol {
+            ProtocolKind::V1(_) => (6, v1::MAX_ID),
+            ProtocolKind::V2(..) => (14, v2::MAX_ID),
+        };
+        let max_id = u64::from(max_id);
+        let bits = status_size * max_id * 10;
+        Duration::from_micros(bits * 1_000_000 / u64::from(baudrate))
+            + Duration::from_millis(3 * max_id + 16)
+    }
+
     /// Send a reboot instruction.
     ///
     /// Reboot the motor with specified `id`.
@@ -562,6 +599,30 @@ trait Protocol<P: Packet> {
         Ok(self.read_status_packet(port, id).is_ok())
     }
 
+    /// Ping the broadcast id and keep the id of every status packet that parses until
+    /// `window` closes. Changes the port's timeout; the caller puts it back.
+    fn broadcast_ping(&self, port: &mut dyn SerialPort, window: Duration) -> Result<Vec<u8>> {
+        self.send_instruction_packet(port, P::ping_packet(P::BROADCAST_ID).as_ref())?;
+        let deadline = Instant::now() + window;
+        let mut ids = Vec::new();
+        loop {
+            let left = deadline.saturating_duration_since(Instant::now());
+            if left.is_zero() {
+                break;
+            }
+            port.set_timeout(left)?;
+            // Silence until the deadline, or bytes that do not frame a packet, end it.
+            let Ok(data) = self.read_status_packet_bytes(port) else {
+                break;
+            };
+            let id = data[P::ID_POSITION];
+            if P::status_packet(&data, id).is_ok() {
+                ids.push(id);
+            }
+        }
+        Ok(ids)
+    }
+
     fn reboot(&self, port: &mut dyn SerialPort, id: u8) -> Result<bool> {
         self.send_instruction_packet(port, P::reboot_packet(id).as_ref())?;
 
@@ -762,7 +823,10 @@ trait Protocol<P: Packet> {
     }
 }
 
-use std::{fmt, time::Duration};
+use std::{
+    fmt,
+    time::{Duration, Instant},
+};
 
 /// The error field of a status packet.
 ///
@@ -952,6 +1016,59 @@ mod tests {
         );
     }
 
+    #[test]
+    fn a_broadcast_ping_lists_every_motor_that_answers() {
+        // Motors 1 and 3 answer; motor 2's status packet arrives with a broken checksum.
+        let answers = [
+            [0xFF, 0xFF, 0x01, 0x02, 0x00, 0xFC],
+            [0xFF, 0xFF, 0x02, 0x02, 0x00, 0x00],
+            [0xFF, 0xFF, 0x03, 0x02, 0x00, 0xFA],
+        ]
+        .concat();
+        let mut port = FakePort::new(vec![answers]);
+        let written = port.written();
+        let settings = port.settings();
+
+        let ids = DynamixelProtocolHandler::v1()
+            .broadcast_ping(&mut port)
+            .unwrap();
+
+        assert_eq!(ids, [1, 3]);
+        // One ping, to the broadcast id.
+        assert_eq!(
+            *written.lock().unwrap(),
+            [vec![0xFF, 0xFF, 0xFE, 0x02, 0x01, 0xFE]]
+        );
+        // The port's own timeout is back once the window closes.
+        let timeouts = &settings.lock().unwrap().timeouts;
+        assert_eq!(timeouts.last(), timeouts.first());
+    }
+
+    #[test]
+    fn a_broadcast_ping_reads_protocol_v2_status_packets() {
+        let answer = vec![
+            0xFF, 0xFF, 0xFD, 0x00, 0x01, 0x08, 0x00, 0x55, 0x00, 0xA6, 0x00, 0x00, 0x00, 0x8C,
+            0xC0,
+        ];
+        let mut port = FakePort::new(vec![answer]);
+
+        let ids = DynamixelProtocolHandler::v2()
+            .broadcast_ping(&mut port)
+            .unwrap();
+
+        assert_eq!(ids, [1]);
+    }
+
+    #[test]
+    fn the_broadcast_ping_window_leaves_every_id_its_turn() {
+        // 253 ids at 3 ms each, the 16 ms of a USB latency timer, and 253 six-byte status
+        // packets at 1 Mbps.
+        assert_eq!(
+            DynamixelProtocolHandler::v1().broadcast_ping_window(1_000_000),
+            Duration::from_micros(253 * 3_000 + 16_000 + 253 * 6 * 10)
+        );
+    }
+
     #[test]
     fn no_post_delay_configured_means_no_sleep() {
         let dph = DynamixelProtocolHandler::v1();

src/dynamixel_protocol/packet.rs

@@ -4,6 +4,10 @@ use crate::Result;
 
 pub trait Packet {
     const HEADER_SIZE: usize;
+    /// The id every motor listens to.
+    const BROADCAST_ID: u8;
+    /// Where a status packet carries the id of the motor that sent it.
+    const ID_POSITION: usize;
     type ErrorKind: Debug;
     type InstructionKind: Debug;
 

src/dynamixel_protocol/v1.rs

@@ -13,6 +13,8 @@ pub(crate) const MAX_ID: u8 = 253;
 pub(crate) struct PacketV1;
 impl Packet for PacketV1 {
     const HEADER_SIZE: usize = 4;
+    const BROADCAST_ID: u8 = BROADCAST_ID;
+    const ID_POSITION: usize = 2;
 
     type ErrorKind = DynamixelErrorV1;
     type InstructionKind = InstructionKindV1;